Wiki

Note: This tutorial assumes that you have completed the previous tutorials: RGB-D Handheld Mapping With a Kinect.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Stereo Handheld Mapping

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

Tutorial Level: BEGINNER

Next Tutorial: Remote Mapping

Contents

  1. Note
  2. Bring-up the camera
    1. Bring-up example for a Bumblebee2 camera
    2. Bring-up example with Zed/Zed-Mini/Zed2 cameras
    3. Bring-up example with MYNT EYE S camera
    4. Bring-up example with RealSense cameras
    5. Bring-up example with DepthAI OAK-D
  3. Mapping mode
    1. Not synchronized stereo messages
    2. Use external odometry
    3. Where is the map saved?
  4. Localization mode
    1. Switching online from Mapping to Localization
    2. Starting in Localization mode from a saved map
    3. Coordinate frames

Note

For the Zed stereo camera, see RGB-D Mapping tutorial instead as the node publishes already a depth image. However, if you want to use left and right images, skip to Section 2.2 below (images should be already rectified with zed_ros_wrapper).

Bring-up the camera

Here are the main steps to configure your camera to be used with RTAB-Map.

  1. I assume that your stereo camera node publishes these messages and that header.frame_id of the messages is /camera_link:

  2. Your camera should be calibrated, if not, you can do it following this tutorial.

  3. Rectify the images using stereo_image_proc. This will generate these topics:

  4. The messages are in the /camera_link frame (x-axis right, y-axis down and z-axis forward), but RTAB-Map needs them in the /world frame (x-axis forward, y-axis left, z-axis up). To do that, we use a static_transform_publisher to add a transform between /base_link and /camera_link: 

    •    <arg name="pi/2" value="1.5707963267948966" />
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) base_link camera_link 100" />
  5. We are now ready to start RTAB-Map.

Bring-up example for a Bumblebee2 camera

The camera1394stereo node is used. Note that this node uses /stereo_camera as the camera frame and put the messages in the stereo_camera namespace. The camera was calibrated using this tutorial. 

<launch>
  <node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node">
    <param name="video_mode" value="format7_mode3" />
    <param name="format7_color_coding" value="raw16" />
    <param name="bayer_pattern" value="bggr" />
    <param name="bayer_method" value="" />
    <param name="stereo_method" value="Interlaced" />
    <param name="camera_info_url_left" value="" />
    <param name="camera_info_url_right" value="" />
  </node>
    
  <!-- Rotate the camera frame. -->
  <arg name="pi/2" value="1.5707963267948966" />
  <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
  <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) base_link stereo_camera 100" />  
    
  <!-- Use direclty "stereo_camera" namespace instead of remappping each message. -->
  <group ns="/stereo_camera" >   
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
  </group>
</launch>

Bring-up example with Zed/Zed-Mini/Zed2 cameras

Bring-up example with MYNT EYE S camera

Issues:

Launch:

Bring-up example with RealSense cameras

With D400 cameras, it is recommended to disable IR emitter after starting them below, otherwise visual odometry will get confused with the IR points not moving from the projector: 

rosrun rqt_reconfigure rqt_reconfigure
# select Camera->stereo_module, set emitter_enabled to OFF

Bring-up example with DepthAI OAK-D

Mapping mode

There are two choices for online visualization: rtabmap_viz or rviz. With the RVIZ plugin rtabmap_rviz_plugins/MapCloud, the clouds can be incrementally added to RVIZ. However using rtabmap_viz, the interface should look like the tutorials of the standalone version. Following the launch file of the Bumblebee bring-up example above:

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 :

Not synchronized stereo messages

If your camera messages are not synchronized (e.g., the left and right images don't have exactly the same timestamp header.stamp), you should add this argument: "approximate_sync:=true". However, stereo with not exactly synchronized images is bad, so it would better to check on the camera driver node so that it sends left and right images with the same timestamp.

Use external odometry

If you want to use external odometry (e.g., you want to use odometry from your robot or another stereo odometry approaches like viso2_ros or fovis_ros), 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

Switching online from Mapping to Localization

In localization mode, a map large enough (>30 locations) must be already created (using stereo_mapping.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:

Starting in Localization mode from a saved map

If you followed the steps in Mapping mode section above, just kill stereo_mapping.launch terminal, then relaunch it with localization:=true parameter and without --delete_db_on_start argument. This will reuse the map saved by default in ~/.ros/rtabmap.db:

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.

Coordinate frames

RTAB-Map follows standard from REP 105 for coordinate frames. At the initialization, the origin of coordinate frame "odom" is set to the location where the camera is at that moment. The TF odom->camera_link will always be with respect to this origin. Once RTAB-Map has relocalized itself in the prebuilt map, the tf map->odom will change from an identity transform to the transform from the prebuilt map's origin to the new odom coordinate system.

Wiki: rtabmap_ros/Tutorials/StereoHandHeldMapping (last edited 2023-04-19 05:03:17 by MathieuLabbe)