Z-axis drift using Ouster OS-0 and ZED2 depth camera in RTABMap ROS

[jaymatsushiba]

Hello,

I have been tinkering with RTABMap, in order to eventually create a 3D mapping system intended for outdoor natural/wilderness settings. In the meantime, I have been testing the program indoors and on my university campus.

My set up includes a GPS sensor (MTK3339), the Ouster OS-0 32, and the ZED2 stereo camera. I think I have managed to set up RTABMap to take in the information from all sources.

I have been seeing a lot of z-axis shift as I move the setup around indoors or outdoors, much more than I see others experience with their projects on these forums. I would really appreciate your help to optimize the parameters to improve the output from RTABMap.

Here are some videos demonstrating:
https://youtu.be/4414Qn6cnqc - Indoor testing with Ouster, ZED2 (No GPS since no fix indoors)
https://youtu.be/Om14Edghysg - Outdoor testing with Ouster, GPS (No ZED2 since I hadn't integrated it yet at this point)

I used the following resources to hobble together a launch file:
https://answers.ros.org/question/402766/loop-closure-with-velodyne-and-camera-using-rtab-map/ 
https://github.com/introlab/rtabmap_ros/issues/922#issuecomment-1657279457
https://github.com/introlab/rtabmap/wiki/Robust-Graph-Optimization#multi-session
https://github.com/introlab/rtabmap_ros/blob/ab676c15a5e5d3d9f1029eaf2f9e52d3fcdb627d/launch/demo/demo_husky.launch
https://github.com/introlab/rtabmap_ros/blob/c1d1c01d5d732784ceb6b225ebf6eebdb700ceb0/rtabmap_examples/launch/test_ouster_gen2.launch 

My launch file is attached here:
230803_ouster_zed_gps_rtabmap.launch 

Thank you!
Jay

[jaymatsushiba]

Here is a bag file with the Ouster and GPS sensor: https://www.dropbox.com/scl/fi/kk2ur8ldc6ej1ujp1a9ub/ouster_gps_2023-08-01-19-43-23.bag?rlkey=abluv1ajbk1ncqcd30ophuf6r&dl=0 

I will be recording another one when weather is better here with Ouster, ZED2, and GPS

[jaymatsushiba]

Taking the depth camera as an input outdoors somehow improved the z-axis drift immensely, and it was able to detect a loop closure doing it. I do wonder why it seems to fail to detect loop closures in indoor environments.

[matlabbe]

If you are not deskewing the lidar, icp_odometry will drift a lot more. Deskewing could be done using the ouster's imu or zed2's imu. I'll try to check your bag this week and see if there is something else that could be improved.

[jaymatsushiba]

Thank you for your response!

Here is the bag file that includes the Ouster, ZED2, and GPS together: https://www.dropbox.com/scl/fi/nhu6wm8ba5d5yy4qbfch5/20230809_ouster_zed2_gps_AQ.bag?rlkey=6s9a717hpsuxl1t0usyjbw96g&dl=0

When I do turn on LiDAR deskewing, it provides an error message that deskewing fails - I will post more details on it on Monday when I am next working on the project.

Jay

[matlabbe]

Hi,

There is something wrong with the TF between os_sensor and os_imu, it feels off. If we remove any usage of IMU, the map is pretty straight (this is without loop closure optimization, only odometry):


I used this simplified launch file just to compare (also tuned some ICP parameters based on that example). I disabled deskewing because IMU stamps are a little off from the lidar scans and the reason above. As the lidar doesn't rotate a lot, in that case deskewing would not change a lot the results.

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="frame_id" default="os_sensor"/>
  <arg name="rtabmap_viz" default="true"/>
  <arg name="slerp" default="false"/>
  <arg name="scan_20_hz" default="true"/>
  <arg name="voxel_size" default="0.2"/>           <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->

  <arg name="scan_topic" default="/ouster/points"/>

  <param name="use_sim_time" value="true"/>

  <group ns="rtabmap">

    <node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
      <remap from="scan_cloud" to="$(arg scan_topic)"/>
      <remap from="odom" to="/icp_odom"/>

      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id" type="string" value="odom"/>
      <param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
      <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>

      <!-- ICP parameters -->
      <param name="Icp/Iterations" type="string" value="10"/>
      <param name="Icp/VoxelSize" type="string" value="$(arg voxel_size)"/>
      <param name="Icp/PointToPlane" type="string" value="false"/>
      <param name="Icp/PointToPlaneK" type="string" value="0"/>
      <param name="Icp/PointToPlaneRadius" type="string" value="0"/>

      <param name="Icp/MaxTranslation" type="string" value="2"/>
      <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval voxel_size*3)"/>
      <param name="Icp/PM" type="string" value="true"/>
      <param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
      <param name="Icp/PMMatcherKnn"        type="string" value="3"/>
      <param name="Icp/PMMatcherEpsilon"    type="string" value="0.001"/>

      <!-- Odom parameters -->
      <param name="Odom/ScanKeyFrameThr" type="string" value="0.5"/>
      <param name="Odom/Strategy" type="string" value="0"/>
      <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
      <param name="OdomF2M/ScanMaxSize" type="string" value="10000"/>
    </node>

    <node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="false"/>
      <param name="subscribe_rgb" type="bool" value="false"/>
      <param name="subscribe_scan_cloud" type="bool" value="true"/>
      <param name="approx_sync" type="bool" value="true"/>

      <remap from="odom" to="/icp_odom"/>
      <param name="subscribe_odom_info" type="bool" value="true"/>

      <remap from="rgbd_image" to="rgbd_image"/>

      <param name="queue_size" type="int" value="10"/>

      <remap from="scan_cloud" to="$(arg scan_topic)"/>

      <!-- RTAB-Map's parameters -->
      <param name="Rtabmap/DetectionRate" type="string" value="1"/>
      <param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
      <param name="RGBD/ProximityBySpace" type="string" value="true"/>
      <param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
      <param name="RGBD/LocalRadius" type="string" value="5"/>
      <param name="RGBD/AngularUpdate" type="string" value="0.05"/>
      <param name="RGBD/LinearUpdate" type="string" value="0.05"/>

      <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
      <param name="Mem/STMSize" type="string" value="30"/>

      <param name="Reg/Strategy" type="string" value="1"/>
      <param name="Optimizer/GravitySigma" type="string" value="0.5"/>
      <param name="Optimizer/Strategy" type="string" value="1"/>

      <param name="Optimizer/PriorsIgnored" type="string" value="true"/>
      <param name="Grid/CellSize" type="string" value="0.1"/>
      <param name="Grid/RangeMax" type="string" value="20"/>
      <param name="Grid/ClusterRadius" type="string" value="1"/>
      <param name="Grid/GroundIsObstacle" type="string" value="true"/>

      <!-- ICP parameters -->
      <param name="Icp/VoxelSize" type="string" value="$(arg voxel_size)"/>
      <param name="Icp/PointToPlane" type="string" value="false"/>
      <param name="Icp/Iterations" type="string" value="30"/>
      <param name="Icp/MaxTranslation" type="string" value="10"/>
      <param name="Icp/MaxCorrespondenceDistance" type="string" value="3"/>
      <param name="Icp/PM" type="string" value="true"/>
      <param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
      <param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
    </node>

    <node name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">

      <param name="frame_id" type="string" value="$(arg frame_id)"/>

      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="false"/>
      <param name="subscribe_rgb" type="bool" value="false"/>
      <param name="subscribe_scan_cloud" type="bool" value="true"/>

      <remap from="odom" to="/icp_odom"/>
      <param name="subscribe_odom_info" type="bool" value="true"/>

      <remap from="scan_cloud" to="/ouster/points"/>
    </node>
  </group>

</launch>

The actual drift error looks like this (before and after loop closure optimization) of two poses of the same location (begin versus end of trajectory):


I think the transform between ouster lidar frame and its imu frame should be calibrated, or use an external imu that you now its frame is aligned with gravity.

Also note that the loop closure shown above is a manual loop closure, as the the one fond automatically was wrong, like 1 meter off this one. Proximity detection is very sensitive to repetitive geometry, so local proximity detection can be found. In that kind of environment, we may need to rely more on global loop closure detection (e.g., combined with camera). I'll download your latest bag and see how much the camera could help for correct loop closure detection. EDIT: I cannot download the second bag, it looks too large (firefox is failing after 38G, tried two times), can you compress it before sharing it? (rosbag compress)

cheers,
Mathieu

[jaymatsushiba]

Hello Mathieu,

Thank you very much for the prompt troubleshooting and support.

Here is the compressed bag file with the ZED2 topics contained: https://www.dropbox.com/scl/fi/s6b19ktr6cwjepqq3qtff/20230809_ouster_zed2_gps_AQ_compressed.bag?rlkey=mbvkb2m5dqjq4wqyjllmue74u&dl=0

I will look into calibrating the OS IMU or acquiring an external IMU to use. The results that you have produced are much better, looking forward to what you can do with the new rosbag.

Jay

[jaymatsushiba]

Hello Mathieu,

Upon closer inspection of the tf of the ZED2, I realized that the urdf provided by the manufacturer was not accurate and caused the depth camera to have a rotation relative to base_link that is not correct. I will produce a fixed rosbag with the correct tf to run.

Jay

[jaymatsushiba]

I am having some issues testing on a new rosbag file (currently being compressed, will upload when ready).

When I set Optimizer/PriorsIgnored to false in order for RTABMap to use the GPS as a prior, the graph goes absolutely nuts - when it maps alright without it (but with drift adding up so loop closure not detected at the end)




When I export the GPS, it looks totally fine


And comparison in RTABmap database viewer:


Jay

[jaymatsushiba]

Here is the new rosbag file (split into 10 GB bag files, can play all at once): https://www.dropbox.com/sh/uzvz2ls3v30mo9c/AAAmfjjECDaaQ16lOlPH_oXea?dl=0

This rosbag file contains the sensor streams during a drive around my campus with the Ouster OS-0, ZED2, and GPS running simultaneously.

Thank you,
Jay

[matlabbe]

Got some issues with those bags:
1) /clock jumps often in the past

clock: 
  secs: 1692300645
  nsecs: 926263117
---
clock: 
  secs: 1692300645
  nsecs: 932719943
---
clock: 
  secs: 1692300645
  nsecs: 936324200  <---------
---
clock: 
  secs: 1692300645
  nsecs: 932719943  <---------
---
clock: 
  secs: 1692300645
  nsecs: 932719943

2) images stop to be published for 1-2 seconds

With lidar using parameters of my previous post it doesn't work, at some point icp_odometry thinks the robot is not moving while it is. The range of that lidar seems small for that kind of application, so I didn't try to push more the lidar approach. I had better results using stereo odometry. Note that RGB-D odometry doesn't work very well because the depth range is pretty small:


Using stereo mode, we can estimate features very far, which is super useful to estimate orientation:


About visual odometry, 5 Hz is too slow, at least 10 Hz is recommended for that kind of experiment. At 5 Hz, it is difficult to correctly track the features between frames, and because of second issue above, the number of inliers often drop very low. I had to lower the threshold to avoid losing visual odometry. Here the launch I used:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="frame_id" default="base_link"/>
  <arg name="rtabmap_viz" default="true"/>

  <param name="use_sim_time" value="false"/>

  <group ns="rtabmap">
  
    <node pkg="rtabmap_sync" type="stereo_sync" name="stereo_sync" output="screen">
      <remap from="left/image_rect" to="/zed2/zed_node/left/image_rect_color"/>
      <remap from="right/image_rect" to="/zed2/zed_node/right/image_rect_color"/>
      <remap from="left/camera_info" to="/zed2/zed_node/left/camera_info"/>
      <remap from="right/camera_info" to="/zed2/zed_node/right/camera_info"/>
      <param name="approx_sync" value="false"/>
    </node>
    
    <node pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" output="screen">
      <remap from="left/image_rect" to="/zed2/zed_node/left/image_rect_color"/>
      <remap from="right/image_rect" to="/zed2/zed_node/right/image_rect_color"/>
      <remap from="left/camera_info" to="/zed2/zed_node/left/camera_info"/>
      <remap from="right/camera_info" to="/zed2/zed_node/right/camera_info"/>
      <remap from="odom" to="odom"/>
      
      <!-- some tuning to avoid odometry lost -->
      <param name="GFTT/MinDistance" type="string" value="5"/>
      <param name="Vis/MinInliers" type="string" value="10"/>
      <param name="Vis/CorGuessWinSize" type="string" value="20"/>

      <param name="frame_id" type="string" value="$(arg frame_id)"/>
    </node>

    <node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="subscribe_rgbd" type="bool" value="true"/>
      <remap from="rgbd_image" to="rgbd_image"/>
      <remap from="gps/fix" to="/fix"/>

      <!-- RTAB-Map's parameters -->
      <param name="Mem/STMSize" type="string" value="30"/>
      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
      <param name="Optimizer/Strategy" type="string" value="2"/>
      <param name="Optimizer/PriorsIgnored" type="string" value="false"/>
    </node>

    <node name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="subscribe_rgbd" type="bool" value="true"/>
      <param name="subscribe_odom_info" type="bool" value="true"/>
    </node>
  </group>

</launch>

I think because of the low framerate and images not published for seconds, there are some constraints added in graph with wrong covariance, and odometry seems drifting sometimes more than the covariance. That makes loop closures all rejected.

[ WARN] (2023-08-18 17:44:52.551) Rtabmap.cpp:3685::process() Rejecting all added loop closures (1, first is 853 <-> 167) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 52.471111 (edge 167->853, type=1, abs error=49.087494 m, stddev=0.935515). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation.
[ WARN] (2023-08-18 17:44:52.551) Rtabmap.cpp:3722::process() Rejecting all added loop closures (1, first is 853 <-> 167) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 7.714176 (edge 166->167, type=0, abs error=10.533203 deg, stddev=0.023831). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation.
[ WARN] (2023-08-18 17:44:52.551) Rtabmap.cpp:3760::process() Loop closure 853->167 rejected!

Well in rtabmap-databaseViewer, setting RGBD/OptimizeMaxError to 0 and adding manually some loop closures, I can get:



Then with GPS priors (two trajectories overlap):




cheers,
Mathieu