Rtabmap mapping with two cameras

[matlabbe]

Hi,

To get camera point clouds and laser scan merged, you would have to use scan_cloud input. Laser scan should be converted to point cloud, then added to your camera point clouds. Make sure they are all in the same frame_id frame before merging them.

For rtabmap node, you have to set subscribe_scan_cloud=true and remap scan_cloud topic. Normally when subscribed to a scan input, rtabmap will automatically set Grid/FromDepth=false.

cheers,
Mathieu

[adr_arroyo]

Hello, thanks for the answer.
I am trying to merge the pointclouds of the cameras and the laser one but i cannot launch the node "point_cloud_aggregator" to do the first merge. I keep getting this error:

ERROR: cannot launch node of type [rtabmap_ros/nodelets/point_cloud_aggregator]: rtabmap_ros/nodelets
ROS path [0]=/opt/ros/indigo/share/ros
ROS path [1]=/home/adrian/catkin_ws
ROS path [2]=/home/adrian/catkin_ws/src
ROS path [3]=/opt/ros/indigo/share
ROS path [4]=/opt/ros/indigo/stacks

How can I launch it?
In addition, the remapping in this node is like this, isnt it?

		<remap from="cloud1" to="/camera_front/depth/image_raw"/>

----------------------------------------

I still have some doubts about the mapping with more than one camera. If i place two cameras en rtabmap node (depth_cameras=2) the rtabmap does the merge of the pointclouds directly right? But as I want to use the laser, and it cannot be used when there are more than one camera, I should firstly merge the pointclouds so that rtabmap only has 1.
Until here is it right?

Then, when I have my pointclouds merged, to what do I remap them in rtabmap node? I mean, how can I tell rtabmap to use this pointcloud for building the map?

Cheers

[adr_arroyo]

So I think I managed to run it myself, but the result looks a bit weird:



As you can see the pointcloud marged is totally crazy regarding the floor, the laser is put upside and the second camera appears under the other one.

Is it that i am not mixing the things correctly? I am using the scan_to_cloud_converter to get a pointcloud from the laser and then i use the nodelet point_cloud_aggregator to mix the 3 pointclouds of the cameras (camera//depth/points) with the one of the laser.

Cheers

[matlabbe]

Hi,

point_cloud_aggregator is an experimental node (not made by me). I just updated it to transform the clouds in the same frame (see frame_id parameter). That could solve the problem of point clouds not merged in the same frame.

Note that using the rtabmap_ros/RGBDImage topic interface, having two cameras + laser scan is possible:

$ rosrun rtabmap_ros rtabmap _subscribe_depth:=false _subscribe_rgbd:=true _rgbd_cameras:=2 _subscribe_scan:=true scan:=/scan
[ INFO] [1489764197.621743106]: Starting node...
[ INFO] [1489764197.724004694]: Initializing nodelet with 4 worker threads.
[ INFO] [1489764198.082766806]: /rtabmap(maps): grid_cell_size             = 0.050000
[ INFO] [1489764198.082852429]: /rtabmap(maps): grid_incremental           = false
[ INFO] [1489764198.082908356]: /rtabmap(maps): grid_size                  = 0.000000
[ INFO] [1489764198.082956431]: /rtabmap(maps): grid_eroded                = false
[ INFO] [1489764198.083008852]: /rtabmap(maps): grid_footprint_radius      = 0.000000
[ INFO] [1489764198.083074771]: /rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1489764198.083131096]: /rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1489764198.083187727]: /rtabmap(maps): map_cleanup                = true
[ INFO] [1489764198.083243966]: /rtabmap(maps): map_negative_poses_ignored = false
[ INFO] [1489764198.083294837]: /rtabmap(maps): map_negative_scan_ray_tracing = true
[ INFO] [1489764198.083350980]: /rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1489764198.083407020]: /rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1489764198.083461296]: /rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1489764198.086532043]: /rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1489764198.086588343]: /rtabmap(maps): octomap_occupancy_thr      = 0.500000
[ INFO] [1489764198.119039541]: rtabmap: frame_id      = base_link
[ INFO] [1489764198.119105050]: rtabmap: map_frame_id  = map
[ INFO] [1489764198.119154908]: rtabmap: tf_delay      = 0.050000
[ INFO] [1489764198.119198030]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1489764198.119251138]: rtabmap: odom_sensor_sync   = false
[ WARN] [1489764199.061719136]: Setting "Grid/FromDepth" parameter to false (default true) as "subscribe_scan" or "subscribe_scan_cloud" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/FromDepth" to true. To suppress this warning, add <param name="Grid/FromDepth" type="string" value="false"/>
[ INFO] [1489764199.302288943]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1489764199.302409282]: rtabmap: Using database from "/home/mathieu/.ros/rtabmap.db".
[ INFO] [1489764199.312591223]: rtabmap: Database version = "0.12.3".
[ INFO] [1489764199.361098771]: /rtabmap: queue_size    = 10
[ INFO] [1489764199.361163761]: /rtabmap: rgbd_cameras = 2
[ INFO] [1489764199.361200699]: /rtabmap: approx_sync   = true
[ INFO] [1489764199.361312658]: Setup rgbd2 callback
[ INFO] [1489764199.396932194]: 
/rtabmap subscribed to (approx sync):
   /odom,
   /rgbd_image0,
   /rgbd_image1,
   /scan
[ INFO] [1489764199.403301827]: rtabmap 0.12.3 started...

See this demo_two_kinects.launch to know how to publish rgbd_image0 and rgbd_image1.

In your case, as you want to create the occupancy grid from both depth and laser scan (also using point_cloud_aggregator):

$ rosrun nodelet nodelet manager __name:=nodelet_manager
$ rosrun nodelet nodelet load rtabmap_ros/point_cloud_aggregator nodelet_manager __name:=point_cloud_aggregator

$ rosrun rtabmap_ros rtabmap _subscribe_depth:=false _subscribe_rgbd:=true _rgbd_cameras:=2 _subscribe_scan_cloud:=true scan_cloud:=/combined_cloud
[ INFO] [1489764491.774696639]: Starting node...
[ INFO] [1489764491.880745159]: Initializing nodelet with 4 worker threads.
[ INFO] [1489764492.250434247]: /rtabmap(maps): grid_cell_size             = 0.050000
[ INFO] [1489764492.250511484]: /rtabmap(maps): grid_incremental           = false
[ INFO] [1489764492.250566082]: /rtabmap(maps): grid_size                  = 0.000000
[ INFO] [1489764492.250620536]: /rtabmap(maps): grid_eroded                = false
[ INFO] [1489764492.250678457]: /rtabmap(maps): grid_footprint_radius      = 0.000000
[ INFO] [1489764492.250727714]: /rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1489764492.250788046]: /rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1489764492.250838868]: /rtabmap(maps): map_cleanup                = true
[ INFO] [1489764492.250886769]: /rtabmap(maps): map_negative_poses_ignored = false
[ INFO] [1489764492.250932424]: /rtabmap(maps): map_negative_scan_ray_tracing = true
[ INFO] [1489764492.250979328]: /rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1489764492.251036526]: /rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1489764492.251079209]: /rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1489764492.262008719]: /rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1489764492.262088399]: /rtabmap(maps): octomap_occupancy_thr      = 0.500000
[ INFO] [1489764492.294589739]: rtabmap: frame_id      = base_link
[ INFO] [1489764492.294646963]: rtabmap: map_frame_id  = map
[ INFO] [1489764492.294706609]: rtabmap: tf_delay      = 0.050000
[ INFO] [1489764492.294741937]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1489764492.294772799]: rtabmap: odom_sensor_sync   = false
[ WARN] [1489764493.197492400]: Setting "Grid/FromDepth" parameter to false (default true) as "subscribe_scan" or "subscribe_scan_cloud" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/FromDepth" to true. To suppress this warning, add <param name="Grid/FromDepth" type="string" value="false"/>
[ INFO] [1489764493.445672677]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1489764493.445776491]: rtabmap: Using database from "/home/mathieu/.ros/rtabmap.db".
[ INFO] [1489764493.456724984]: rtabmap: Database version = "0.12.3".
[ INFO] [1489764493.506069320]: /rtabmap: queue_size    = 10
[ INFO] [1489764493.506127431]: /rtabmap: rgbd_cameras = 2
[ INFO] [1489764493.506151340]: /rtabmap: approx_sync   = true
[ INFO] [1489764493.506254783]: Setup rgbd2 callback
[ INFO] [1489764493.541796157]: 
/rtabmap subscribed to (approx sync):
   /odom,
   /rgbd_image0,
   /rgbd_image1,
   /combined_cloud
[ INFO] [1489764493.546962029]: rtabmap 0.12.3 started...

cheers,
Mathieu

[adr_arroyo]

Hi, thanks for the update.
About the last part of your comment, as far as i understand what you sent me is kinetic version of rtabmap_ros, and i am currently working in indigo version, so I am not able to use the param "suscribe_rgbd" and this way not allowed to use "depth_cameras=2" and "suscribe_scan_cloud=true".
I have already the pointclouds of the two cameras and the pointcloud of the laser mergued into one "/cloud" and now they are in the same frame (however the framerate looks low), how can i tell rtabmap to use this pointcloud as the one for the mapping if i am not allowed to set the scan_cloud="/cloud"?

Cheers

[matlabbe]

Hi,

I recommend to use a voxel filter to downsample the camera clouds before merging them. This would fix the low framerate.

Multiple cameras + scan/cloud interface was not implemented in 0.11.8. It is new stuff from 0.11.13. I suggest to build rtabmap and rtabmap_ros from source in your catkin workspace.

I didn't update yet indigo/jade binaries as I didn't have time to test the new interface (0.11.13) on these distributions and make sure there are no regressions.

cheers,
Mathieu

[adr_arroyo]

Hello Mathieu, thanks for previous answers.
I tried the vozel filter and it works better.
About the build of rtabmap, I already have it from source in my catkin_ws. Are you going to update it or is it better to move to the new version?

I am currently doing some maps and I would like to benchmark them, not (just) the odometry but everything, like number of points, number of loop closures, colors, depth, etc. Is there an already done way to compare maps in rtabmap? or is it neccesary to look for a pointcloud comparing external tool?

Cheers

[matlabbe]

Hi,

If you are using the git master branch from source, you have already the latest version. To get latest changes, just follow Update to new version.

By default all statistics of rtabmap are saved in the database. You can get/plot them with rtabmap-databaseViewer, by opening Statistics view. Other info about the map can be shown on Info view.

To compare point clouds, you may use CloudCompare, though never tried it (only saw videos of comparison results).

cheers,
Mathieu

[adr_arroyo]

Hello,
I am still a bit confused about the versions. I am now in branch indigo-devel, but when i tried to be in master there were some compilation erros as some files from rtabmap wouldnt mach with files from rtabmap_ros. Do you mean that it is possible to follow the master branch from the ros-indigo version?

About the databaseViewer, when I open a database where I have a map, there is no statistics view (at least in this version). Then, if I open the rtabmap software there is a view of it, however when I open it, it doesnt show anything; I mean the value of the parameters dont change. If I am not mistaken, this statistics view only works in the rtabmap software (not in rtabmap-databaseViewer) when the actual mapping process (or localization) is working, and not afterwards, doesnt it? Is it possible to show the values for this statistics view after performing the map?

Cheers

[matlabbe]

Hi,

indigo-devel branch matches the release Indigo binaries. The branch names should match between rtabmap and rtabmap_ros projects (e.g., rtabmap:master=rtabmap_ros:master,  rtabmap:indigo_devel=rtabmap_ros:indigo_devel).

When using master branch of rtabmap_ros, you should use master branch of rtabmap. Normally they should be compatible (rtabmap_ros:master should build against rtabmap:master), otherwise it is a bug. See Update to new version section to make sure to update both projects. Make sure you don't have ros-indigo-rtabmap installed if source is used.

Just verified that statistics are not saved in current indigo version (and there is no Statistics panel in database viewer). You should use master branch.


Yes, with the new Statistics tab in database viewer we can see all statistics recorded in the database. They can be plotted like in rtabmap app.

cheers,
Mathieu

[adr_arroyo]

Hi Mathieu,
I updated to master branch, but when i open the database viewer the statistics view does not show anything:



Is this a problem only in my computer?
Cheers

[matlabbe]

Hi,

It is because the database may be recorded with a previous version. Is this happen with a database recorded from the latest version?

cheers

[adr_arroyo]

Hi,
yes the database was recorded with previous version. I will try to make a new one from the latest version.

Thanks

[adr_arroyo]

Hi,
so i did a fast map to check that everything was working and it is, thanks. Even the statistics view.
I have a question regarding the info obtained by rtabmap, the blue line I know that it is the trajectory of the robot (the other blue lines are the items detected by laser) but, what are the points that are placed in the blue trajectory line? Do they represent that rtabmap detects a loop closure?



Cheers


----

I edit this post cause I am stuying a bit the information showed in the statistics view; I have a few questions:

- I am guessing that the yellow circles in the images of database-editor are the keypoints, and the blue lines that connect two circles of each panel tells us that it is the same keypoint, am I right? Moreover, regarding this keypoint information, what does exactly the dictionary size tell us? The size of the dictionary that keeps the keypoints? Is it accesible somehow?

- does Loop panel information have the information about the loop closure detections? is it possible to know where was detected a loop closure, after doing the map, in the database-editor or somewhere?

- In the memory panel, what is exactly the odometry variance? A measure of the error? A mean value of the variance of the odometry for all the time the robot was moving?

- What does Proximity and NeighbourLinkRefining panels refer to?

Thanks in advanced and sorry for the long posts and questioning!

[adr_arroyo]

Hello,
I have my configuration of rtabmap as follows: https://drive.google.com/file/d/0BzDVfAKVWMsPcUY3Q2F0emxuWnM/view?usp=sharing

As you can see, I am trying to use the pcl agregator to have just one pointcloud for both cameras, then I want to use the voxel_grid filter to downsample the resulting pointcloud.
I want to use this plc as the pointcloud that rtabmap uses for mapping (for the /cloud_map, etc....) and as i understood I need to remap it to /scan_cloud topic (/scan_cloud to /voxel_grid/output). Am I doing this right? Because the /cloud_map topic doesnt work or it works with the pcl of the cameras if I remove the comments of the: from="rgbd_image0"       to="/camera1/rgbd_image" from="rgbd_image1" to="/camera2/rgbd_image" but not with the output of the filter.

Cheers

[matlabbe]

Hi,

you should set "Grid/FromDepth" to false to construct cloud_map from scan_cloud:

<param name="Grid/FromDepth" type="string" value="true"/>

cheers,
Mathieu

[matlabbe]

Hi,

The blue dots on the blue trajectory are the poses of the nodes in the map's graph.

Yes, in databaseViewer, the yellow circles are keypoints extracted, and the cyan lines between two views are matching keypoints (same word in dictionary). The dictionary size depends on many factors. For example, for the same number of images processed, a smaller dictionary would tell that the environment has a lot of common features while a large dictionary would tell that the environment has low common features. In particular, the lower Kp/NndrRatio parameter is, the larger will be the dictionary as words will be more unique. By default, rtabmap sets it to 0.8. See figure 11 of this paper: https://www.cs.ubc.ca/~lowe/papers/ijcv04.pdf

Yes, just open the Graph View and you will see the loop closure links shown in red and the proximity links in yellow. In statistics, you can also show them (matching ID) under Loop->Accepted_hypothesis_id:


For odometry variance under Memory panel, if you show the graph, it is the odometry variance of the link added at that time. A high variance means that the link has poor accuracy. The linear variance is max(var_x,var_y,var_z) and angular variance is max (var_roll, var_pitch, var_yaw) of the diagonal of 6x6 covariance matrix.

Proximity statistics are about Proximty detection of rtabmap: local loop closures found by comparing locations under a fixed radius (using odometry poses). Neighbor link refining statistics appear when RGBD/NeighborLinkRefining is true, which is used to refine (generally by ICP and laser scans) the odometry received.

cheers,
Mathieu

[adr_arroyo]

Hello,
thanks for the information and the paper, now I understand more clearly the view.
I am performing some test with the voxel filter and the configuration previously mentioned.
Is it normal that I ferquently get this warnings:
- Cannot compute transform
- Less signatures transfered (0) than added (2)

In addition, there appear a lot of blue points in the rtabmap view of the 3D map:

Are they normally there when we use the scan_cloud?

Also, is there a way to avoid the "duplicated" textures once we have the map? With that I mean, for example, the left part of the shelving that looks duplicated in the map. I know that loop closure is supposed to avoid this but it happens frequently.

Cheers

[matlabbe]

Hi,

the blue points are the scan_cloud. If you don't want to see them, uncheck Preferences->3D Rendering->Show scans.

These warnings are ok, they are more informative.

Yes, normally loop closures would decrease this effect. Is seems to be a simulated environment, appearance-based loop closure detection generally doesn't work well in this kind of environment, unless visual features are not repetitive. If you are going to use scan_cloud from kinects, I recommend to set "RGBD/ProximityPathMaxNeighbors" to 0 to avoid high latencies (in latest code version, default of RGBD/ProximityPathMaxNeighbors is now 0).

You may also create 2d laser scans from the assembled kinect clouds, that way set "RGBD/ProximityPathMaxNeighbors" at 10 so more loop closures are found using only geometry (scan matching). Well, if you can put a 2D laser scanner directly in your simulation, it would be even better.

cheers,
Mathieu

[adr_arroyo]

Hi,
thanks for the information, I am now doing some maps in real world but I will definitely check those parameters out.
Cheers