RGB-D Handheld Mapping

Description: This tutorial shows how to use rtabmap_ros out-of-the-box with a Kinect-like sensor in mapping mode or localization mode.

Tutorial Level: BEGINNER

Next Tutorial: Stereo Handheld Mapping

RGB and depth sensors

The RGB-D sensors which are OpenNI and OpenNI2 compliant would work out-of-the-box using openni_launch, openni2_launch, freenect_launch, iai_kinect2 or Zed packages. Sensors tested:

• Kinect for Xbox 360

  • $ roslaunch openni_launch openni.launch depth_registration:=true

    or

  • $ roslaunch freenect_launch freenect.launch depth_registration:=true

• XtionPRO Live

  • $ roslaunch openni2_launch openni2.launch depth_registration:=true

• Kinect v2 (Xbox One)

  • Make sure it is calibrated: see kinect2_calibration.

  • $ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true

• Stereolabs ZED (make sure you have this pull request to avoid synchronization issues) Updated April 29 2020

  • Choose between A or B:

  • // A) With rtabmap odometry:
    roslaunch zed_wrapper zed_no_tf.launch
    rosrun dynamic_reconfigure dynparam set zed_node depth_confidence 99
    rosrun dynamic_reconfigure dynparam set zed_node depth_texture_conf 90
    rosrun dynamic_reconfigure dynparam set zed_node depth_confidence 100
    
    // B) With zed odometry:
    roslaunch zed_wrapper zed2.launch
    rosrun dynamic_reconfigure dynparam set /zed2/zed_node depth_confidence 99
    rosrun dynamic_reconfigure dynparam set /zed2/zed_node depth_texture_conf 90
    rosrun dynamic_reconfigure dynparam set /zed2/zed_node depth_confidence 100
    // To avoid map -> odom TF published by zed, set tracking/publish_map_tf to false for zed_wrapper_node
    
    // Note: the repeating depth_confidence is not an error, it seems on the first call nothing happens, we need to redo second time with a different value.

• RealSense R200 and ZR300

  • #R200
    $ roslaunch realsense_camera r200_nodelet_rgbd.launch
    
    #ZR300
    $ roslaunch realsense_camera zr300_nodelet_rgdb.launch
    
    // To get registration without empty lines
    $ rosrun rtabmap_util pointcloud_to_depthimage \
        cloud:=/camera/depth/points \
        camera_info:=/camera/rgb/camera_info \
        image_raw:=/camera/depth/points/image_raw \
        image:=/camera/depth/points/image \
        _approx:=false \
        _fill_holes_size:=2

• RealSense D415/D435

  • $ roslaunch realsense2_camera rs_camera.launch align_depth:=true

• RealSense D435i/L515

  • # ISSUE: Use unite_imu_method:="copy" if imu topics keep stopping
    $ roslaunch realsense2_camera rs_camera.launch \
        align_depth:=true \
        unite_imu_method:="linear_interpolation" \
        enable_gyro:=true \
         enable_accel:=true
    
    $ rosrun imu_filter_madgwick imu_filter_node \
        _use_mag:=false \
        _publish_tf:=false \
        _world_frame:="enu" \
        /imu/data_raw:=/camera/imu \
        /imu/data:=/rtabmap/imu

• RealSense D400/L515 + T265

  • Based on your setup, make sure to edit the static transform between the cameras in that launch file.

  • $ roslaunch realsense2_camera rs_d400_and_t265.launch

• MYNT EYE S

  • $ roslaunch mynt_eye_ros_wrapper mynteye.launch
    $ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966  base_link mynteye_link 100

• Kinect for Azure: see Mapping mode below.

• DepthAI OAK-D RGB-D Mode:

  • $ roslaunch depthai_examples stereo_inertial_node.launch
    
    # Estimate quaternion of the IMU data
    $ rosrun imu_filter_madgwick imu_filter_node \
       imu/data_raw:=/stereo_inertial_publisher/imu \
       imu/data:=/stereo_inertial_publisher/imu/data  \
       _use_mag:=false \
       _publish_tf:=false

• DepthAI OAK-D Right IR-Depth Mode:

  • $ roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
    
    # Estimate quaternion of the IMU data
    $ rosrun imu_filter_madgwick imu_filter_node \
       imu/data_raw:=/stereo_inertial_publisher/imu \
       imu/data:=/stereo_inertial_publisher/imu/data  \
       _use_mag:=false \
       _publish_tf:=false

Mapping mode

There are two choices for online visualization: rtabmap_viz or rviz. With the RVIZ plugin rtabmap/MapCloud, the clouds can be incrementally added to RVIZ. However using rtabmap_viz, the interface should look like the tutorials of the standalone version. With the launch file rtabmap.launch below, set rviz:=true to open rviz and rtabmap_viz:=true to open rtabmap_viz (default true) for visualization.

• For freenect, openni and openni2:

  •  $ roslaunch rtabmap_launch rtabmap.launch rtabmap_args:="--delete_db_on_start"

• For kinect2_bridge:

  •  $ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link kinect2_link 100
     $ roslaunch rtabmap_launch rtabmap.launch \
        rtabmap_args:="--delete_db_on_start" \
        rgb_topic:=/kinect2/qhd/image_color_rect \
        depth_topic:=/kinect2/qhd/image_depth_rect \
        camera_info_topic:=/kinect2/qhd/camera_info \
        approx_sync:=false

  or for convenience: rgbd_mapping_kinect2.launch

  •  $ roslaunch rtabmap_legacy rgbd_mapping_kinect2.launch resolution:=qhd

• For Stereolabs ZED and ZED Mini

  • Choose between A or B (depending of the one chosen above):

     // A) With rtabmap odometry
     $ roslaunch rtabmap_launch rtabmap.launch \
        rtabmap_args:="--delete_db_on_start" \
        rgb_topic:=/zed_node/rgb/image_rect_color \
        depth_topic:=/zed_node/depth/depth_registered \
        camera_info_topic:=/zed_node/rgb/camera_info \
        frame_id:=base_link \
        approx_sync:=false \
        wait_imu_to_init:=true \
        imu_topic:=/zed_node/imu/data
     # Remove wait_imu_to_init if using ZED without imu
    
     // B) With zed odometry
     $ roslaunch rtabmap_launch rtabmap.launch \
        rtabmap_args:="--delete_db_on_start" \
        rgb_topic:=/zed2/zed_node/rgb/image_rect_color \
        depth_topic:=/zed2/zed_node/depth/depth_registered \
        camera_info_topic:=/zed2/zed_node/depth/camera_info \
        odom_topic:=/zed2/zed_node/odom \
        imu_topic:=/zed2/zed_node/imu/data \
        visual_odometry:=false \
        frame_id:=base_link \
        approx_sync:=false \
        rgbd_sync:=true \
        approx_rgbd_sync:=false

  • An example package is provided by Stereolabs to directly launch a 3D mapping process using RTAB-map from the zed-ros-wrapper:

  •  $ roslaunch zed_rtabmap_example zed_rtabmap.launch

• For RealSense R200 and ZR300

  •  $ roslaunch rtabmap_launch rtabmap.launch \
        rtabmap_args:="--delete_db_on_start" \
        depth_topic:=/camera/depth_registered/sw_registered/image_rect_raw
    
     // Using depth registered from `rtabmap_util/pointcloud_to_depthimage`
     $ roslaunch rtabmap_launch rtabmap.launch \
        rtabmap_args:="--delete_db_on_start" \
        depth_topic:=/camera/depth/points/image_raw

• For RealSense D415/D435

  •  $ roslaunch rtabmap_launch rtabmap.launch \
        rtabmap_args:="--delete_db_on_start" \
        depth_topic:=/camera/aligned_depth_to_color/image_raw \
        rgb_topic:=/camera/color/image_raw \
        camera_info_topic:=/camera/color/camera_info \
        approx_sync:=false

• For RealSense D435i/L515

  •  $ roslaunch rtabmap_launch rtabmap.launch \
        rtabmap_args:="--delete_db_on_start --Optimizer/GravitySigma 0.3" \
        depth_topic:=/camera/aligned_depth_to_color/image_raw \
        rgb_topic:=/camera/color/image_raw \
        camera_info_topic:=/camera/color/camera_info \
        approx_sync:=false \
        wait_imu_to_init:=true \
        imu_topic:=/rtabmap/imu

• For RealSense D400/L515 + T265

  •  $ roslaunch rtabmap_launch rtabmap.launch \
       args:="-d --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
       odom_topic:=/t265/odom/sample \
       frame_id:=t265_link \
       rgbd_sync:=true \
       depth_topic:=/d400/aligned_depth_to_color/image_raw \
       rgb_topic:=/d400/color/image_raw \
       camera_info_topic:=/d400/color/camera_info \
       approx_rgbd_sync:=false \
       visual_odometry:=false \
       queue_size:=30

• For RealSense L515: ICP odometry example(ref). See D435i example above for visual odometry.

  • $ rosrun nodelet nodelet standalone rtabmap_util/point_cloud_xyz \
        _approx_sync:=false  \
        /depth/image:=/camera/depth/image_rect_raw \
        /depth/camera_info:=/camera/depth/camera_info \
        _decimation:=4
    
    $ roslaunch rtabmap_launch rtabmap.launch\
        rtabmap_args:="\
          --delete_db_on_start \
          --Icp/VoxelSize 0.05 \
          --Icp/PointToPlaneRadius 0 \
          --Icp/PointToPlaneK 20 \
          --Icp/CorrespondenceRatio 0.2 \
          --Icp/PMOutlierRatio 0.65 \
          --Icp/Epsilon 0.005 \
          --Icp/PointToPlaneMinComplexity 0 \
          --Odom/ScanKeyFrameThr 0.7 \
          --OdomF2M/ScanMaxSize 15000 \
          --Optimizer/GravitySigma 0.3 \
          --RGBD/ProximityPathMaxNeighbors 1 \
          --Reg/Strategy 1" \
        icp_odometry:=true \
        scan_cloud_topic:=/cloud \
        subscribe_scan_cloud:=true \
        depth_topic:=/camera/aligned_depth_to_color/image_raw \
        rgb_topic:=/camera/color/image_raw \
        camera_info_topic:=/camera/color/camera_info \
        approx_sync:=false \
        wait_imu_to_init:=true \
        imu_topic:=/rtabmap/imu 

• For MYNT EYE S

  •  $ roslaunch rtabmap_launch rtabmap.launch \
       rgb_topic:=/mynteye/left_rect/image_rect \
       camera_info_topic:=/mynteye/left_rect/camera_info \
       depth_topic:=/mynteye/depth/image_raw \
       frame_id:=base_link \
       rtabmap_args:="-d"

• For Kinect for Azure (make sure you have this pull request)

  •  $ cd ~/catkin_ws/src/Azure_Kinect_ROS_Driver
     $ git pull origin pull/166/head
    
     # Color mode:
     $ roslaunch azure_kinect_ros_driver slam_rtabmap.launch
     # To avoid black borders in the 3D Map, set "0.05 0.05 0.05 0.05" 
     # in Preferences->3D Rendering->ROI Ratios, for both Map and Odom columns.
    
     # IR mode:
     $ roslaunch azure_kinect_ros_driver slam_rtabmap.launch color_enabled:=false

• For DepthAI OAK-D RGB-D Mode

  •  $ roslaunch rtabmap_launch rtabmap.launch \
        args:="--delete_db_on_start" \
        rgb_topic:=/stereo_inertial_publisher/color/image \
        depth_topic:=/stereo_inertial_publisher/stereo/depth \
        camera_info_topic:=/stereo_inertial_publisher/color/camera_info \
        imu_topic:=/stereo_inertial_publisher/imu/data \
        frame_id:=oak-d_frame \
        approx_sync:=true \
        wait_imu_to_init:=true

• For DepthAI OAK-D Right IR-Depth Mode

  •  $ roslaunch rtabmap_launch rtabmap.launch \
        args:="--delete_db_on_start" \
        rgb_topic:=/stereo_inertial_publisher/right/image_rect \
        depth_topic:=/stereo_inertial_publisher/stereo/depth \
        camera_info_topic:=/stereo_inertial_publisher/right/camera_info \
        imu_topic:=/stereo_inertial_publisher/imu/data \
        frame_id:=oak-d_frame \
        approx_sync:=true \
        approx_sync_max_interval:=0.001 \
        wait_imu_to_init:=true

The rtabmap argument "--delete_db_on_start" is used to start mapping from a clean database. You can also reset the memory from rtabmapviz using the action "Edit->Delete memory", or by using this service :

•  $ rosservice call /rtabmap/reset

You should see something like this:

• If the background of the map turns RED, it is because the odometry is lost. Visit this page under Lost Odometry (RED screens!) section to know why this happens! To track again the odometry, replace the camera to the last position before the RED appeared and it should be able to recompute the odometry. Another way is to reset the odometry using the menu option "Edit->Reset odometry" (note that a new map is created each time the odometry is reset). See Odometry Auto-Reset for another way to handle odometry lost (reset and continue the same map).

Use external odometry

If you want to use external odometry (e.g., you want to use odometry from your robot), you can set these arguments: "visual_odometry:=false odom_topic:=/my_odometry".

Where is the map saved?

~/.ros/rtabmap.db

RTAB-Map provides a tool to browse data in the database:

$ rtabmap-databaseViewer ~/.ros/rtabmap.db

Localization mode

In localization mode, a map large enough (>30 locations) must be already created (using rtabmap.launch above).

In rtabmapviz (GUI), click on "Localization" in the "Detection" menu. By looking over locations in the map, RTAB-Map would localize itself in it by finding a loop closure. Once a loop closure is found, the odometry is corrected and the current cloud will be aligned to the map.

You can play with the relocation behaviour by resetting the odometry. You can use form the GUI the Detection->"Reset odometry" action or call the service on the terminal like:

• $ rosservice call /rtabmap/reset_odom

Advanced

You can also start the launch file above directly in Localization mode with a previously built database. Set the parameter "Mem/IncrementalMemory" to "false". Also, when starting the rtabmap node, the argument "--delete_db_on_start" must not be set!

• <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="">
     ...
     <param name="Mem/IncrementalMemory" type="string" value="false"/>
  </node>

Since recent versions, rtabmap.launch has "localization" argument for convenience:

• $ roslaunch rtabmap_launch rtabmap.launch localization:=true

In the GUI, do Edit->"Download map" to download the map from the rtabmap node. At the start, you should see that the current displayed cloud is not aligned to the map, it is normal because the odometry is reset on each start. By looking over locations in the map, RTAB-Map would localize itself in it by finding a loop closure. Once a loop closure is found, the odometry is corrected and the map will appear with the current cloud aligned to it.