Skimap_ros integration with ORBSLAM2

[Amir-Ramezani]

Hi,

I followed the discussion in ORBSLAM 2 and saw the following clip:
https://www.youtube.com/watch?v=W3nm2LXmgqE

I can run ORBSLAM and Skimap live, but dont have any idea how to integrate them, partially because there are no topics from ORBSLAM 2. I appreciate if you could give me some idea and explanation about it.

[m4nh]

@AmirCognitive if you see the ROS+RGBD example file in the ORB_SLAM2 package (file) on row 112 there is the real "track" operation made with OrbSlam:

mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

This method returns the Camera 6DOF Pose (related obviously to the first camera reference frame) so you can retrieve this:

cv::Mat camera_pose = mpSLAM->TrackRGBD(...)

this is a 4x4 transformation matrix so you can convert easily in a Ros tf::Transform and broadcast it to the system, so it can be used by SkiMap as source for sensor pose. @raulmur may correct me if i'm wrong.

The alternative is to call in an asynch way the method to retrieve all the keyframes stored in that moment in the Orb Slam System with:

vector<KeyFrame *> keyframes;
mpSLAM->GetAllKeyFrames(keyframes);

But in this case you have a list of only the KeyFrame's poses , you don't have any information about RGB-D Images so you have to store in a separated Map all your RGB-D data, mapped with timestamp that is the same key used to store KeyFrames in OrbSlam2, @raulmur correct me also here.

[Amir-Ramezani]

Sorry for late reply.

I followed your first method and added the following to the "file":

// added for integration with skimap_ros
cv::Mat camera_pose = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

static tf::TransformBroadcaster br;
tf::Transform transform;

tf::Vector3 origin;
origin.setValue((camera_pose.at<double>(0,3)),(camera_pose.at<double>(1,3)),(camera_pose.at<double>(2,3)));

tf::Matrix3x3 tf3d;
tf3d.setValue((camera_pose.at<double>(0,0)), (camera_pose.at<double>(0,1)), (camera_pose.at<double>(0,2)), 
      (camera_pose.at<double>(1,0)), (camera_pose.at<double>(1,1)), (camera_pose.at<double>(1,2)), 
      (camera_pose.at<double>(2,0)), (camera_pose.at<double>(2,1)), (camera_pose.at<double>(2,2)));

tf::Quaternion tfqt;
tf3d.getRotation(tfqt);

transform.setOrigin(origin);
transform.setRotation(tfqt);

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "camera_pose_test"));

However, I guess there is a problem with my code adding camera 6DOF pose to tf::Transform since I receive the following error, when camera moves.

"Segmentation fault (core dumped)"

Then, "so it can be used by SkiMap as source for sensor pose", should I use the skimap_map_service? Could you explain a bit in detail.

Thanks,

[m4nh]

@AmirCognitive SkiMap testing example explained in the README works if is there a TF ready to be used as sensor 6DOF pose. This code about ORBSLAM2 produces only the abovementioned TF, just it. SEGMENTATION FAULT is a problem with your code, maybe related to some pointers or some bad indexing of arrays.

[Amir-Ramezani]

Maybe my question is very simple, but as I understand you are saying we created the TF in the previous post, should I change the following part in the launch file of skimap_live in order to connect to the new TF created or I should use the skimap_service?

<!-- Global and Camera TF Names -->
<param name="base_frame_name" value="world"/>
<param name="camera_frame_name" value="camera_pose_test"/>

[m4nh]

yes, but this not resolve SEGMENTATION FAULT that is a bug in your code

[Amir-Ramezani]

Beside what you say about Segmentation Fault which is happening after adding the code, I have this error also in the Skimap_ros launch file:

[ERROR] [1497013568.329012342]: "odom" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1497013568.427245455]: "odom" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1497013568.427980266]: "odom" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1497013568.428439365]: "odom" passed to lookupTransform argument target_frame does not exist.

[m4nh]

"odom" is the base frame of you odometry system. The one always fixed in the space that is the parent for each of the "camera_pose" tfs

[Amir-Ramezani]

After running the ORB RGB-D, and openni2 for Asus camera, this is the following TFs on my system, output of "rosrun tf tf_monitor"

Frame: /camera_depth_frame published by unknown_publisher Average Delay: -0.0998325 Max Delay: 0
Frame: /camera_depth_optical_frame published by unknown_publisher Average Delay: -0.0997629 Max Delay: 0
Frame: /camera_pose_test published by unknown_publisher Average Delay: 0.000407517 Max Delay: 0.00589606
Frame: /camera_rgb_frame published by unknown_publisher Average Delay: -0.0997913 Max Delay: 0
Frame: /camera_rgb_optical_frame published by unknown_publisher Average Delay: -0.0998036 Max Delay: 0

and these are the topics I have:

/camera/camera_nodelet_manager/bond
/camera/depth/camera_info
/camera/depth/image
/camera/depth/image/compressed
/camera/depth/image/compressed/parameter_descriptions
/camera/depth/image/compressed/parameter_updates
/camera/depth/image/compressedDepth
/camera/depth/image/compressedDepth/parameter_descriptions
/camera/depth/image/compressedDepth/parameter_updates
/camera/depth/image/theora
/camera/depth/image/theora/parameter_descriptions
/camera/depth/image/theora/parameter_updates
/camera/depth/image_raw
/camera/depth/image_raw/compressed
/camera/depth/image_raw/compressed/parameter_descriptions
/camera/depth/image_raw/compressed/parameter_updates
/camera/depth/image_raw/compressedDepth
/camera/depth/image_raw/compressedDepth/parameter_descriptions
/camera/depth/image_raw/compressedDepth/parameter_updates
/camera/depth/image_raw/theora
/camera/depth/image_raw/theora/parameter_descriptions
/camera/depth/image_raw/theora/parameter_updates
/camera/depth/image_rect
/camera/depth/image_rect/compressed
/camera/depth/image_rect/compressed/parameter_descriptions
/camera/depth/image_rect/compressed/parameter_updates
/camera/depth/image_rect/compressedDepth
/camera/depth/image_rect/compressedDepth/parameter_descriptions
/camera/depth/image_rect/compressedDepth/parameter_updates
/camera/depth/image_rect/theora
/camera/depth/image_rect/theora/parameter_descriptions
/camera/depth/image_rect/theora/parameter_updates
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/image_rect_raw/theora
/camera/depth/image_rect_raw/theora/parameter_descriptions
/camera/depth/image_rect_raw/theora/parameter_updates
/camera/depth/points
/camera/depth_rectify_depth/parameter_descriptions
/camera/depth_rectify_depth/parameter_updates
/camera/depth_registered/camera_info
/camera/depth_registered/image_raw
/camera/depth_registered/image_raw/compressed
/camera/depth_registered/image_raw/compressed/parameter_descriptions
/camera/depth_registered/image_raw/compressed/parameter_updates
/camera/depth_registered/image_raw/compressedDepth
/camera/depth_registered/image_raw/compressedDepth/parameter_descriptions
/camera/depth_registered/image_raw/compressedDepth/parameter_updates
/camera/depth_registered/image_raw/theora
/camera/depth_registered/image_raw/theora/parameter_descriptions
/camera/depth_registered/image_raw/theora/parameter_updates
/camera/depth_registered/points
/camera/depth_registered/sw_registered/camera_info
/camera/depth_registered/sw_registered/image_rect
/camera/depth_registered/sw_registered/image_rect/compressed
/camera/depth_registered/sw_registered/image_rect/compressed/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect/compressed/parameter_updates
/camera/depth_registered/sw_registered/image_rect/compressedDepth
/camera/depth_registered/sw_registered/image_rect/compressedDepth/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect/compressedDepth/parameter_updates
/camera/depth_registered/sw_registered/image_rect/theora
/camera/depth_registered/sw_registered/image_rect/theora/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect/theora/parameter_updates
/camera/depth_registered/sw_registered/image_rect_raw
/camera/depth_registered/sw_registered/image_rect_raw/compressed
/camera/depth_registered/sw_registered/image_rect_raw/compressed/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect_raw/compressed/parameter_updates
/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth
/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth/parameter_updates
/camera/depth_registered/sw_registered/image_rect_raw/theora
/camera/depth_registered/sw_registered/image_rect_raw/theora/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect_raw/theora/parameter_updates
/camera/driver/parameter_descriptions
/camera/driver/parameter_updates
/camera/ir/camera_info
/camera/ir/image
/camera/ir/image/compressed
/camera/ir/image/compressed/parameter_descriptions
/camera/ir/image/compressed/parameter_updates
/camera/ir/image/compressedDepth
/camera/ir/image/compressedDepth/parameter_descriptions
/camera/ir/image/compressedDepth/parameter_updates
/camera/ir/image/theora
/camera/ir/image/theora/parameter_descriptions
/camera/ir/image/theora/parameter_updates
/camera/rgb/camera_info
/camera/rgb/image_raw
/camera/rgb/image_raw/compressed
/camera/rgb/image_raw/compressed/parameter_descriptions
/camera/rgb/image_raw/compressed/parameter_updates
/camera/rgb/image_raw/compressedDepth
/camera/rgb/image_raw/compressedDepth/parameter_descriptions
/camera/rgb/image_raw/compressedDepth/parameter_updates
/camera/rgb/image_raw/theora
/camera/rgb/image_raw/theora/parameter_descriptions
/camera/rgb/image_raw/theora/parameter_updates
/camera/rgb/image_rect_color
/camera/rgb/image_rect_color/compressed
/camera/rgb/image_rect_color/compressed/parameter_descriptions
/camera/rgb/image_rect_color/compressed/parameter_updates
/camera/rgb/image_rect_color/compressedDepth
/camera/rgb/image_rect_color/compressedDepth/parameter_descriptions
/camera/rgb/image_rect_color/compressedDepth/parameter_updates
/camera/rgb/image_rect_color/theora
/camera/rgb/image_rect_color/theora/parameter_descriptions
/camera/rgb/image_rect_color/theora/parameter_updates
/camera/rgb_rectify_color/parameter_descriptions
/camera/rgb_rectify_color/parameter_updates
/rosout
/rosout_agg
/tf
/tf_static

Obviously I have no odom, so which one should I use?

[m4nh]

Sorry but unfortunately this is not a tutorial. Maybe you don't have a strong background about ROS and a generic SLAM system, so i can't help you, sorry. By the way your ODOM is the Identity in this case, try to publish a Static TF that is the identity 4x4 matrix, and it should work

[Amir-Ramezani]

No need to say so many sorry, after all, you did a good job by providing this package for people. I was trying to use it but as you know, there is a problem here and there is a problem in the second issue I opened in this repository, and all of these messages I am sending are just to run your package to work in a similar way to the video clips you guys uploaded on youtube.

My SLAM background is not good, not every one are good in SLAM and yes you are right, this is not a tutorial, but if you upload a package then it should not work similar to the description of the package and videos that are published by default? I wanted to use a simple 3D mapping system for model-based machine learning algorithms, since it is a better idea to do test with the packages that already exist I tried your package, but that is not a good idea for me to study SLAM from scratch, rather is better to cooperate with a person who knows it well.

[bcm0]

@m4nh In Orb_Slam_2 discussion you wrote (raulmur/ORB_SLAM2#322):

Unfortunately due to license problem i can't provide you the integration code but SkiMap is pretty simple to use...

I would like to know if it's sufficient to apply the changes you suggest in this issue to get a working system (like the one shown in the video (https://www.youtube.com/watch?v=W3nm2LXmgqE).
Do you use RGB-D or Monocular in the video?
@AmirCognitive Did you manage to get it running?

[m4nh]

@adnion , @zhengguoxian123 in the README of SkiMap is shown a simple example called "skimap_live.launch" which just works with a moving TF of the camera (with a fixed world TF) and a stream from an RGBD camera (so double topic: one for RGB and one for DEPTH). It works regardless of who publish the abovementioned data. So you need to modify the ORBSLAM2 code just to enable the TF Broadcasting of the tracked camera pose, in this way SkiMap can exploit it.

If this is not clear please answer and maybe together we can produce a working example of this. Thanks

[m4nh]

@zhengguoxian123 if you launch "skimap_live.launch" in the launch file you have to set the name of the live TF of the camera ("camera_frame_name"), the name of the map TF (odom or world, or a fixed one: "base_frame_name"), both the name of RGB/DEPTH topic of camera ("camera_rgb_topic"/"camera_depth_topic"). And it should work, it publishes on RVIZ the Visualization Marker of the whole map. You can modify the "skimap_live.cpp" node to manage map in another way

[m4nh]

@zhengguoxian123 did you change TF for camera and TF for world?

[m4nh]

@adnion SkiMap is not able to manage a Monocular camera as it is because without Depth information is impossible to build a 3D representation for RGB points. If the Slam System (like ORBSLAM2 in this case) produces a semi-dense or dense map of points during tracking SkiMap can use this subset to build the map (an example is LSD Slam (https://www.youtube.com/watch?v=GnuQzP3gty4&t=1s) that produces a semi-dense map with monocular camera)