RGB-D Mapping with Laser scanner

[Kazu]

Hi, I'm very impressed your work!

In my project, I need point clouds of the environment, so I'm using Rtabmap with Kinect.  The problem is the environment (hallway) has less landmarks, and Rtabmap becomes lost quite often.  The reason I guess is the range Kinect can measure is limited (up to 4 m; angle of view is 57 deg in horizontal).  Rtabmap works fine in the small.
Thus I added laser scanner (UTM LX-30) to improve the precision; however, the precision hasn't improved and I couldn't export the point clouds after I added laser scanner.

My question is
1) whether possible for RGB-D mapping to add laser scanner to improve the precision
2) if possible, then how I should add it

Thanks in advance,
Kazu

[matlabbe]

Hallways are always challenging for any systems that cannot "view" very far. This demo uses an UTM LX-30 if I remember correctly. The configuration used assumes that there is a relatively reliable odometry available. The odometry is computed from the wheel encoders of the base, so it is why the robot gets never lost, even in hallways. If you don't have any odometry available on your platform, the rtabmap/rgbd_odometry works reliably only if there are enough visual features under 4 meters in the environment. You could use the laser to generate an odometry instead (like in the "Kinect+2D laser" configuration). I don't provide an "icp_odometry" node built in rtabmap, but a google search tells that packages like laser_scan_matcher or more robust hector_slam could generate such odometry as input to RTAB-Map.

[Kazu]

Thanks for the reply; we don't have any odometry source but have a laser scanner, so I configured rtabmap with the suggested settings.

The laser scanner seems to work; at least Hector SLAM created 2D map successfully.  However, nothing is shown in Rivz when I started rtabmap.  The voxel clouds were shown only when I ran rgbd_odometry; moreover, I couldn't see assembled clouds published by map_assembler when I ran it.  How do you think I can fix it?

Our purpose is to have point cloud (.ply file) of the environment, by the way.

[matlabbe]

I've made a new launch file to show how to integrate hector_mapping with rtabmap  if you have a laser scanner and no odometry. You can download it here: demo_hector_mapping.launch. The important thing about hector_mapping is that parameter pub_odometry should be true in order to publish the odometry topic "/scanmatch_odom".

Example with the demo_mapping.bag (you should remove the "/odom" tf from the bag first):

$ rosbag filter demo_mapping.bag demo_mapping_no_odom.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom"'
$ roslaunch rtabmap_ros demo_hector_mapping.launch rviz:=true rtabmapviz:=false
$ rosbag play --clock demo_mapping_no_odom.bag

You can choose visualization between rviz and rtabmapviz with the according argument.

To generate a (.ply), if you use the rtabmapviz, you can directly export using the menu. Another way is to open the database in rtabmap standalone application and export the cloud, see Export cloud section of this page .

[Kazu]

I appreciate your quick response and thanks for providing demo file as well as bag file!  I confirmed that the launch file worked with the bag file you uploaded, but it didn't work when I switched to my system.  

What I've tried was to:
change Ln 57 - 59, 81 - 83 and 96 - 98 as I don't use data throttle and,
change "scan topic" from "/jn0/base_scan" to "scan"
Unfortunately, it still didn't work; nothing was shown in Rviz; it said fixed frame "map" does not exist.

I guess the topic name (including 'remap' topic configuration) was inappropriate, so will examine again; probably I need to change some parameters in Freenect launch file.
Do you have any other idea about the problem?  

Kazu

P.S. I think you missed a hyphen ("-") in the example launch file; " ->" should be "-->" at the end of Ln 8.

[matlabbe]

You don't have to change anything in Freenect launch files. For the lines you mentioned, it should look like like that with Freenect:

<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>

Don't forget to start freenect.launch with "depth_registration:=true" argument. Verify that these topics are sent:

$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ rostopic hz /camera/rgb/image_rect_color
$ rostopic hz /camera/depth_registered/image_raw
$ rostopic hz /camera/depth_registered/camera_info

The base frame id may be needed to be modified. The actual is set to "base_footprint". Don't need the "use_sim_time" too and the "image_transport" parameters. You should set "/jn0/base_can" to "/scan" (with '/') for rtabmap node.

The "/map" frame is only published after the first image is processed by rtabmap, so the inputs should be correctly set. Here the actual rqt_graph of the demo above:

[Kazu]

I finally could make it!

First, I changed "/jn0/base_can" to "/scan" and image parameters (incl. rgb and depth) as you mentioned, then I looked rqt_graph to examine if each node/topic was working; it was what I expected.
Second, as you suggested, I checked if the topics were sent, but they weren't so I restarted the system.  After restarting my system, the topics were sent, but a map didn't show up.  It was when I realized that the tf transform I set was not proper; I forgot to add some so modified tf transforms.

Eventually, the map was built in rviz; so I ran
$ roslaunch rtabmap_ros demo_hector_mapping.launch rviz:=false rtabmapviz:=true
in our hallway and could export .ply file with high accuracy of dimension; that will definitely be useful on our project.

I greatly appreciate your help!


May I confirm about citation; it is what described here (2. Citing), isn't it?
http://wiki.ros.org/rtabmap

Thanks,
Kazu

[matlabbe]

I'm glad that you could make it. The citation is correct. Thanks!

[consoante]

How does one align both sensors? Is there a calibration process?

[matlabbe]

Hi,

There is no calibration process to align the RGB-D camera and the laser scanner. The URDF or static transforms are manually set. I can use RVIZ to show up the laser scans and point cloud of the RGB-D camera at the same time, then I refine TF manually so that points of the laser match with those of the RGB-D sensor.

You can also look at this approach to find the RGB-D sensor's position on the robot: http://wiki.ros.org/openni/Contests/ROS%203D/Automatic%20Calibration%20of%20Extrinsic%20Parameters

cheers

[consoante]

Mathieu first of all many thanks for the reply and for your good work.

I understand the solution and suggestion to solve for the geometry but what about time syncronization?

Is there a description on how the laser is used to improve the solution?

Thanks again,

//RO

[matlabbe]

Hi,

Time synchronization is done with an ApproximateTime Synchronizer callback from ROS. The transforms for the laser and the camera are refined with TF using the timestamp, if different, from the odometry message (to actually align sensor data to odometry). Note that for the laser scan, the stamp is the stamp of the message + scan time (see here for more info).

The laser scanner is used to refine odometry between each map update, to refine loop closure / localization transformation and to detect loop closures / to localize using only geometry. For example, if you have a long-range LiDAR like 30 meters indoor, you will have a very accurate map. The parameters "RGBD/NeighborLinkRefining" (true) and "Reg/Strategy" (1 or 2) are used to make use of the laser scans.

In case you do 2D mapping, you may want to set these parameters too: "Optimizer/Slam2D" (true) and "Reg/Force3DoF" (true).

cheers

[consoante]

Super, i have a Hokuyo lidar whose max distance is 30m...I do want to see if it is possible to get accurate maps! Would this area be written in a paper or so? I guess this would make use of an ICP implementation? Also, the vertical baseline between camera and sensor would have to be as close as possible I guess or can this be accounted somehow

Thanks again for your patience

//ro

[consoante]

Have been testing the standalone version with freenect2 and getting quite good results.

Would like to test the a href="http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#Kinect_.2B-_2D_laser">Kinect + 2D Laser, however I am totally new to ROS.

Do I need to record a bag with the data to be able to use the demo_hector_mapping.launch, or can it be done in real time modifying the launch file?

Thanks again

[consoante]

Is the case of having to combine the demo_hector_mapping.launch with the rgbd_mapping_kinect2_test.launch ?

[matlabbe]

Hi,

you don't need to record a bag. You have to remap the input topics, remove "use_sim_time", remove "image_transport" compressed parameters. demo_hector_mapping.launch has already rtabmap node. If you want to use kinect2 topics, change them under rtabmap and rtabmapviz nodes (similar to rgbd_mapping_kinect2.launch).

cheers

[consoante]

I think I am making progress: both datasets are visible and I see the hector_slam map being built in rviz.

When I try to run  rtabmapviz I am getting an error and the window does not appear...Any advice?

Is it better to record a bag and process it afterwards?

My graph and tf frames are bellow for reference and so is my error in rtabmapviz

consoante wrote

Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ThinkPad-T440p:54374/

SUMMARY
========

PARAMETERS
 * /hector_mapping/base_frame: base_footprint
 * /hector_mapping/map_frame: hector_map
 * /hector_mapping/map_multi_res_levels: 2
 * /hector_mapping/map_resolution: 0.05
 * /hector_mapping/map_size: 2048
 * /hector_mapping/map_update_angle_thresh: 0.06
 * /hector_mapping/odom_frame: odom
 * /hector_mapping/pub_map_odom_transform: False
 * /hector_mapping/pub_map_scanmatch_transform: True
 * /hector_mapping/pub_odometry: True
 * /hector_mapping/scan_topic: /scan
 * /points_xyzrgb/voxel_size: 0.01
 * /rosdistro: indigo
 * /rosversion: 1.11.20
 * /rtabmap/rtabmap/Optimizer/Slam2D: true
 * /rtabmap/rtabmap/Reg/Force3DoF: true
 * /rtabmap/rtabmap/Reg/Strategy: 1
 * /rtabmap/rtabmap/Vis/InlierDistance: 0.1
 * /rtabmap/rtabmap/Vis/MaxDepth: 10.0
 * /rtabmap/rtabmap/frame_id: base_footprint
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_scan: True
 * /rtabmap/rtabmapviz/frame_id: base_footprint
 * /rtabmap/rtabmapviz/subscribe_depth: True
 * /rtabmap/rtabmapviz/subscribe_laserScan: True
 * /urg_node/ip_address: 192.168.0.10

NODES
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)
  /
    hector_mapping (hector_mapping/hector_mapping)
    kinect2_2_base_link (tf/static_transform_publisher)
    laser_2_base_link (tf/static_transform_publisher)
    points_xyzrgb (nodelet/nodelet)
    scanmatcher_to_base_footprint (tf/static_transform_publisher)
    urg_node (urg_node/urg_node)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[urg_node-1]: started with pid [13483]
process[kinect2_2_base_link-2]: started with pid [13484]
process[laser_2_base_link-3]: started with pid [13485]
process[scanmatcher_to_base_footprint-4]: started with pid [13493]
process[hector_mapping-5]: started with pid [13503]
process[rtabmap/rtabmap-6]: started with pid [13528]
process[rtabmap/rtabmapviz-7]: started with pid [13533]
process[points_xyzrgb-8]: started with pid [13545]
HectorSM map lvl 0: cellLength: 0.05 res x:2048 res y: 2048
HectorSM map lvl 1: cellLength: 0.1 res x:1024 res y: 1024
[ INFO] [1473536958.202990687]: HectorSM p_base_frame_: base_footprint
[ INFO] [1473536958.203072087]: HectorSM p_map_frame_: hector_map
[ INFO] [1473536958.203099437]: HectorSM p_odom_frame_: odom
[ INFO] [1473536958.203127054]: HectorSM p_scan_topic_: /scan
[ INFO] [1473536958.203156302]: HectorSM p_use_tf_scan_transformation_: true
[ INFO] [1473536958.203181625]: HectorSM p_pub_map_odom_transform_: false
[ INFO] [1473536958.203219208]: HectorSM p_scan_subscriber_queue_size_: 5
[ INFO] [1473536958.203259972]: HectorSM p_map_pub_period_: 2.000000
[ INFO] [1473536958.203283108]: HectorSM p_update_factor_free_: 0.400000
[ INFO] [1473536958.203305883]: HectorSM p_update_factor_occupied_: 0.900000
[ INFO] [1473536958.203328951]: HectorSM p_map_update_distance_threshold_: 0.400000
[ INFO] [1473536958.203351503]: HectorSM p_map_update_angle_threshold_: 0.060000
[ INFO] [1473536958.203372769]: HectorSM p_laser_z_min_value_: -1.000000
[ INFO] [1473536958.203394604]: HectorSM p_laser_z_max_value_: 1.000000
[ INFO] [1473536958.314057716]: Starting node...
[ INFO] [1473536958.351224953]: Starting node...
[ INFO] [1473536958.373440533]: rtabmapviz: Using configuration from "/opt/ros/indigo/share/rtabmap_ros/launch/config/rgbd_gui.ini"
[ INFO] [1473536958.386297790]: rtabmap: frame_id = base_footprint
[ INFO] [1473536958.386335200]: rtabmap: map_frame_id = map
[ INFO] [1473536958.386345369]: rtabmap: queue_size = 10
[ INFO] [1473536958.386356496]: rtabmap: tf_delay = 0.050000
[ INFO] [1473536958.386366020]: rtabmap: tf_tolerance = 0.100000
[ INFO] [1473536958.386374780]: rtabmap: depth_cameras = 1
[ INFO] [1473536958.386384101]: rtabmap: approx_sync = true
[ INFO] [1473536958.597805405]: Setting RTAB-Map parameter "Optimizer/Slam2D"="true"
[ INFO] [1473536958.646841753]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1473536958.647350898]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1473536958.752345851]: Setting RTAB-Map parameter "Vis/InlierDistance"="0.1"
[ INFO] [1473536958.754747810]: Setting RTAB-Map parameter "Vis/MaxDepth"="10.0"
[ INFO] [1473536958.931730056]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1473536958.931823355]: rtabmap: Deleted database "/root/.ros/rtabmap.db" (--delete_db_on_start is set).
[ INFO] [1473536958.931840624]: rtabmap: Using database from "/root/.ros/rtabmap.db".
[rtabmap/rtabmapviz-7] process has died [pid 13533, exit code -11, cmd /opt/ros/indigo/lib/rtabmap_ros/rtabmapviz -d /opt/ros/indigo/share/rtabmap_ros/launch/config/rgbd_gui.ini rgb/image:=/kinect2/sd/image_color_rect depth/image:=/kinect2/sd/image_depth_rect rgb/camera_info:=/kinect2/sd/camera_info scan:=/scan odom:=/scanmatch_odom __name:=rtabmapviz __log:=/root/.ros/log/69c33e9c-778e-11e6-a6e6-5c514ffb9c71/rtabmap-rtabmapviz-7.log].
log file: /root/.ros/log/69c33e9c-778e-11e6-a6e6-5c514ffb9c71/rtabmap-rtabmapviz-7*.log
[ INFO] [1473536959.209306251]: rtabmap: Database version = "0.11.8".
[ INFO] [1473536959.332027875]:
/rtabmap/rtabmap subscribed to:
   /data_throttled_image,
   /data_throttled_image_depth,
   /data_throttled_camera_info,
   /scanmatch_odom,
   /scan
[ INFO] [1473536959.332153542]: rtabmap 0.11.8 started...

^C[points_xyzrgb-8] killing on exit
[rtabmap/rtabmap-6] killing on exit
[hector_mapping-5] killing on exit
[scanmatcher_to_base_footprint-4] killing on exit
[laser_2_base_link-3] killing on exit
[kinect2_2_base_link-2] killing on exit
[urg_node-1] killing on exit
rtabmap: Saving database/long-term memory... (located at /root/.ros/rtabmap.db)
shutting down processing monitor...

Many thanks again

[consoante]

Problem solved after compiling rtabmap_ros from source.

In testing mode now



I noticed the tree shows only ~ +/- 10hz for the laser...the scanner is capable of 40Hz

Thanks!

[You Li]

Good job Cosoante! Did you roslaunch the .launch file and get the result in real time?

[You Li]

Hi,

Thanks for the instructions. I followed the information, but still got the problem when using Kinect v2 and rplidar A2. Neither rviz or rtabmaprviz did not provide the mapping results. Here is what I did:
1. Run my lidar by using $ roslaunch rplidar_ros rplidar.launch
2. Open another terminal and run $  roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
3. Open another terminal and run $ roslaunch rtabmap_ros demo_hector_mapping_v2.launch  resolution:=hd

Here are my .launch files:
demo_hector_mapping_v2.launch
demo_robot_mapping_v1.rviz

Here is my rqt graph:


And here is the frames:


Do you have any suggestions?

Many thanks,
You Li