ZED camera RTABMapping for navigation

[Alexia]

I am part of a project where we use a ZED stereo camera to map an outdoor environment, however we encountered a lot of problems. We're using the latest version of RTABmap with ROS Kinetic and the latest ZED wrapper with 2.0 SDK. We're trying to map according to the tutorial (http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping), however it does not map at all, as well as sometimes it doesn't subscribe to the "left/right image_rect_color" topics, although they are published by the camera.
We had a better result when we were using demo_stereo_outdoor.launch than when we we're using stereo_mapping.launch by which i mean it started mapping, but it mapped upwards instead of on the X/Y-plane. We haven't changed any parametres that would have caused this, and we're not sure what could've caused this. We tried to rotate the tf-frames in ZED_tf.launch however that didn't change the results and it was still mapping upwards. When we use stereo_mapping.launch and we turn on the point cloud coming from the ZED.launch file it is showing the image on the X/Y-plane but the rtabmap does not use the data to map, as well when we turn the camera around the x, Y or Z axis it is standing still in Rviz and the pointcloud is moving instead.

Lastly we get sync errors even though the topics are published at 20Hz+ rate:

[ WARN] [1492762740.823487032]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmap subscribed to (exact sync):
   /rtabmap/odom,
   /camera/left/image_rect_color,
   /camera/right/image_rect_color,
   /camera/left/camera_info,
   /camera/right/camera_info


TL;DR
What specifically causes the upwards mapping?
Which launch file should we focus on? (Stereo_mapping or demo_stereo_outdoor)
RTABmap uses tf dependencies whereas ZED uses tf2 dependencies. Is that causing a problem?

Bellow we've included a screencap from Rviz as well as our two tf-trees






Best Regards

EDIT:
The codes we're using are as follows:

zed_camera.launch:

<launch>
  <arg name="svo_file"/>
  <arg name="zed_id"/>

  <include file="$(find zed_wrapper)/launch/zed_tf.launch" />
  <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen">

    <!-- SVO file path -->
    <param name="svo_filepath" value="$(arg svo_file)" />

    <!-- ZED parameters -->
    <param name="zed_id"                value="$(arg zed_id)" />

    <param name="resolution"            value="2" />
    <param name="quality"               value="1" />
    <param name="sensing_mode"          value="1" />
    <param name="frame_rate"            value="60" />
    <param name="odometry_db"           value="" />
    <param name="openni_depth_mode"     value="0" />
    <param name="gpu_id"                value="-1" />

    <!-- ROS topic names -->
    <param name="rgb_topic"             value="rgb/image_rect_color" />
    <param name="rgb_raw_topic"         value="rgb/image_raw" />
    <param name="rgb_cam_info_topic"    value="rgb/camera_info" />

    <param name="left_topic"            value="left/image_rect_color" />
    <param name="left_raw_topic"        value="left/image_raw" />
    <param name="left_cam_info_topic"   value="left/camera_info" />

    <param name="right_topic"           value="right/image_rect" />
    <param name="right_raw_topic"       value="right/image_raw" />
    <param name="right_cam_info_topic"  value="right/camera_info" />

    <param name="depth_topic"           value="depth/depth_registered" />
    <param name="depth_cam_info_topic"  value="depth/camera_info" />
    <param name="point_cloud_topic"     value="point_cloud/cloud_registered" />

    <param name="odometry_topic"        value="odom" />

  </node>

  <!-- ROS URDF description of the ZED -->
  <param name="zed_description" textfile="$(find zed_wrapper)/urdf/zed.urdf" />
  <node name="zed_state_publisher" pkg="robot_state_publisher" type="state_publisher">
    <remap from="robot_description" to="zed_description" />
  </node>

</launch>

zed_tf.launch:

<launch>
  <arg name="tf_prefix" default="" />
  <arg name="pi/2" value="1.5707963267948966" />
  <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
    
  <node pkg="tf2_ros" type="static_transform_publisher" name="zed_base_link1"
    args="0 0 0 0 0 0 map $(arg tf_prefix)/camera_link" />

  <node pkg="tf2_ros" type="static_transform_publisher" name="zed_base_link3"
    args="0 0 0 0 0 0 $(arg tf_prefix)/zed_current_frame ZED_left_camera" />

  <node pkg="tf2_ros" type="static_transform_publisher" name="zed_base_link4"
    args="0 0 0 -$(arg pi/2) 0 -$(arg pi/2) $(arg tf_prefix)/zed_current_frame zed_depth_frame" />
    
  <node pkg="tf2_ros" type="static_transform_publisher" name="zed_base_link5"
    args="0 0 0 0 0 0 $(arg tf_prefix)/camera_link zed_initial_frame" />

</launch>

stereo_mapping.launch:

<launch>
   <!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->

   <!-- Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="$(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="zed_initial_frame"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="rtabmap_args"            default=""/>   <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>
  <arg name="approx_sync"             default="true"/>         <!-- if timestamps of the input topics are not synchronized -->
  
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  <arg name="compressed"              default="false"/>
   
  <arg name="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
   
  <arg name="subscribe_scan_cloud"    default="false"/>         <!-- Assuming 3D scan if set -->
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  
  <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
  
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.2"/>
  
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="stereo"                  value="true"/>
    <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
    <arg name="rviz"                    value="$(arg rviz)" />
    <arg name="localization"            value="$(arg localization)"/>
    <arg name="gui_cfg"                 value="$(arg rtabmapviz_cfg)" />
    <arg name="rviz_cfg"                value="$(arg rviz_cfg)" />
  
    <arg name="frame_id"                value="$(arg frame_id)"/>
    <arg name="namespace"               value="$(arg namespace)"/>
    <arg name="database_path"           value="$(arg database_path)"/>
    <arg name="wait_for_transform"      value="$(arg wait_for_transform)"/>
    <arg name="rtabmap_args"            value="$(arg rtabmap_args)"/>  
    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>          
    <arg name="approx_sync"             value="$(arg approx_sync)"/>

    <arg name="stereo_namespace"        value="$(arg stereo_namespace)"/>
    <arg name="left_image_topic"        value="$(arg left_image_topic)" />
    <arg name="right_image_topic"       value="$(arg right_image_topic)" />
    <arg name="left_camera_info_topic"  value="$(arg left_camera_info_topic)" />
    <arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
  
    <arg name="compressed"              value="$(arg compressed)"/>                                                                                
   
    <arg name="subscribe_scan"          value="$(arg subscribe_scan)"/>
    <arg name="scan_topic"              value="$(arg scan_topic)"/>
  
    <arg name="subscribe_scan_cloud"    value="$(arg subscribe_scan_cloud)"/>
    <arg name="scan_cloud_topic"        value="$(arg scan_cloud_topic)"/>
   
    <arg name="visual_odometry"         value="$(arg visual_odometry)"/>          
    <arg name="odom_topic"              value="$(arg odom_topic)"/>       
    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>   
    <arg name="odom_args"               value="$(arg rtabmap_args)"/>
  </include>

</launch>

[enthusiastrobotics]

Hello!
I am not a hundred percent sure I can help but I can give it a try (forgive me if my answer is completely wrong)!

I had a similar issue before. It could be that the odometry that is being linked to your camera frame is wrong, causing the location of the camera to be wrong, and thus the resultant pointclouds not being positioned correctly relative to each other (causing weird maps).
Usually, your odometry source can come from different places (you can also fuse several odometry sources to get a better statistical estimate):
1. Visual odometry (rtabmap can do this, which allows you to map with just a handheld camera)
2. Robot wheels, etc..

I'll assume you are using 1., as you are only using a handheld camera.

Visual odometry works by looking at subsequent frames, and finding the relative position between each other. This would then be used by rtabmap in order to place a new set of pointclouds in the correct relative position, and when done consecutively, you get a map.

Are you sure you have visual odometry ON in your zed launch file?
Also, is the /odom TF being published by rtabmap's visual odometry?

Rtabmap's visual odometry would normally publish the location of the /odom frame relative to /map, and then it may be up to you to make a TF publisher from /odom to the camera frame. Since I don't see any /odom frame in your TF tree, nor any connection broadcasted by Rtabmap, this could be the issue. Check roswtf to see if you have any TF errors!

My TF trees when using rtabmap normally show /map->/odom->camera frame!

Hope this helps!

Cheers!

[Alexia]

Hello enthusiast

Thank you for your answer.
With the visual odometry addition the frames started moving but it still didn't map?

Best Regards

[matlabbe]

Hi,

Don't use zed launch files. Follow exactly the instructions from this tutorial (rgb-d version) or this one (stereo version) under ZED subsections. Note how the zed_wrapper_node node is launched directly and static transforms are manually set. Normally, the tutorial should still work with ZED SDK 2.0. When the map is mapped upward, it is because an optical rotation is missing in TF (e.g., see /camera_link -> /zed_initial_frame transform in the referred tutorials).

To use ZED's odometry instead of RTAB-Map's odometry, it is possible, but some fixes to ZED ros wrapper would be required (maybe they are fixed in latest version). See this post.

cheers,
Mathieu

[Alexia]

Hello Mathieu

Thank you very much for your reply. It is nice to know the author of the program is actively helping it's users  it makes it a lot easier to use.

The whole thing works as we wanted when we launch the node. We are fine using RTABMap odom instead of ZED odom as long as it is reliable, which it seems to be.

Just one more question:
In case we use rtabmap with a robot that has it's own odometry should we use rtabmap odom or the robot odom for mapping and obstacle avoidance?

Best Regards
Alexia

[matlabbe]

Hi Alexia,

If the robot has relatively accurate odometry, I would use it instead of rtabmap odom (or any other visual odometry packages). In particular indoor, it is likely that the robot will be facing a featureless area (e.g. white wall or just large empty space), causing visual odometry approaches to fail (or get lost).

For mapping and obstacle avoidance, it is independent of the odometry approach used, but more about mapping approach used. For example, RTAB-Map would provide the global map to move_base (for global planning), and move_base will generate a local map (directly from sensors and odometry, for obstacle avoidance and local planning).

cheers,
Mahieu