rtabmap and oakd - Could not get transform from base_link to oak_rgb_camera_optical_frame

[bmorawska]

Hi, I would like to connect my OAK-D camera to rtabmap as RGBD camera but I am not able to make the configuration. The problem is defined in terminal as:

[ WARN] [1624989827.670520212]: odometry: Could not get transform from base_link to oak_rgb_camera_optical_frame (stamp=38412.438123) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="Lookup would require extrapolation 1624951412.358793736s into the past.  Requested time 38412.438123286 but the earliest data is at time 1624989824.796916962, when looking up transform from frame [oak_rgb_camera_optical_frame] to frame [base_link]. canTransform returned after 0.20093 timeout was 0.2."

I've read on the rtabmap forum that if this message appears several times it does not matter, but I have hundreds of them and nothing more.

My topics and frames look as below:

/joint_states
/rgb_stereo_publisher/color/camera_info
/rgb_stereo_publisher/color/image
/rgb_stereo_publisher/stereo/camera_info
/rgb_stereo_publisher/stereo/depth
/rosout
/rosout_agg
/tf
/tf_static

and launch file is:

<?xml version="1.0"?>
<launch>
  
  <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="oak_base_link"
        args="$(arg optical_rotate) camera_link oak-d_frame 100" /> 
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
	args="$(arg optical_rotate) base_link camera_link 100" />  
        
  <group ns="rtabmap">

    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"        to="/rgb_stereo_publisher/color/image"/>
      <remap from="depth/image"      to="/rgb_stereo_publisher/stereo/depth"/>
      <remap from="rgb/camera_info"  to="/rgb_stereo_publisher/color/camera_info"/>
      <remap from="rgbd_image"       to="rgbd_image"/> <!-- output -->
      
      <!-- Should be true for not synchronized camera topics 
           (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
      <param name="approx_sync"       value="true"/> 
    </node>

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="wait_for_transform_duration" type="double" value="0.2"/>
      <param name="subscribe_rgbd" type="bool"   value="true"/>
      <param name="frame_id"       type="string" value="base_link"/>
      <remap from="rgbd_image" to="rgbd_image"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id"        type="string" value="base_link"/>
          <param name="subscribe_depth" type="bool"   value="false"/>
          <param name="subscribe_rgbd"  type="bool"   value="true"/>

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

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

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
    </node>
  </group>
</launch>

Can anyone have faced similar problem or can help me with that?

[matlabbe]

Hi,

There are a couple of issues with the current state of the depthai ros packages https://github.com/luxonis/depthai-ros-examples and https://github.com/luxonis/depthai-ros (for ros1):

1. In depthai_ros, the bridge is using hard-coded calibration file for camera_info topic instead of loading the calibration of your camera from its eeprom: https://github.com/luxonis/depthai-ros/blob/21baff8e923e7ea0df60a2138174e5d14d306fdd/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp#L69-L73
2. Hard-coded right calibration file in depthai_ros_examples has wrong baseline in cm, it should be in meters. For example, with Tx=-9222.846299795649, baseline=-Tx/Fx=-(-9222.846299795649)/852.1042688421006=10.8245, even in cm it is wrong as OAK-D has 7.5 cm baseline. Tx should be around -852.1042688421006*0.075=-63.78.
3. Note that I tried to hard-code to change -9222.846299795649 to -63.78 of issue 2 above, the scale is good, but published left and right images don't look like stereo rectified. Tested with stereo_node.launch and rtabmap launched like this:

   roslaunch rtabmap_ros rtabmap.launch \
       args:="--delete_db_on_start" \
       stereo:=true \
       left_image_topic:=/stereo_publisher/left/image \
       right_image_topic:=/stereo_publisher/right/image \
       left_camera_info_topic:=/stereo_publisher/left/camera_info \
       right_camera_info_topic:=/stereo_publisher/right/camera_info \
       frame_id:=oak-d_frame \
       approx_sync:=true
   

4. With rgb_stereo_mode.launch, the rgb camera info has different resolution than the published rgb image.
5. Hard-coded distortion parameters are larger than 8 coefficients with all zeros after. I updated rtabmap_ros to handle this format.

Because of those issues, I cannot really support oak-d ros right now. However, oak-d driver for RTAB-Map standalone has been recently updated and it works pretty well (even including IMU).

[zainmehdi]

@matlabbe Can you please give the link for "oak-d driver for RTAB-Map standalone " . I am unable to google it. Thanks

[matlabbe]

If you build and install depthai-core in /usr/local, then on rtabmap side you only need to make sure "With DepthAI = Yes" like below in the cmake build status:

$ cd rtabmap/build
$ cmake ..
...
-- --------------------------------------------
-- Info :
--   RTAB-Map Version =     0.20.14
--   CMAKE_VERSION =        3.16.3
--   CMAKE_INSTALL_PREFIX = /home/mathieu/catkin_ws/devel
--   CMAKE_BUILD_TYPE =     Release
--   CMAKE_INSTALL_LIBDIR = lib
--   BUILD_APP =            ON
--   BUILD_TOOLS =          ON
--   BUILD_EXAMPLES =       ON
--   BUILD_SHARED_LIBS =    ON
--   CMAKE_CXX_FLAGS = -fpic -fmessage-length=0  -fopenmp -fopenmp -std=c++14
--   FLANN_KDTREE_MEM_OPT = OFF
--   PCL_DEFINITIONS = -DDISABLE_OPENNI2;-DDISABLE_PCAP;-DDISABLE_PNG;-DDISABLE_LIBUSB_1_0
--   PCL_VERSION = 1.10.0
-- 
-- Optional dependencies ('*' affects some default parameters) :
--  *With OpenCV 4.2.0 xfeatures2d = NO, nonfree = NO (License: BSD)
--   With Qt 5.12.8            = YES (License: Open Source or Commercial)
--   With VTK 7.1              = YES (License: BSD)
--   With external SQLite3     = YES (License: Public Domain)
--   With ORB OcTree           = YES (License: GPLv3)
--   With SupertPoint          = NO (WITH_TORCH=OFF)
--   With Python3              = NO (WITH_PYTHON=OFF)
--   With Madgwick             = YES (License: GPL)
--   With FastCV               = NO (FastCV not found)
--   With PDAL                 = YES (License: BSD)
-- 
--  Solvers:
--   With TORO                 = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])
--  *With g2o                  = YES (License: BSD)
--  *With GTSAM                = YES (License: BSD)
--  *With Ceres                = YES (License: BSD)
--   With VERTIGO              = YES (License: GPLv3)
--   With cvsba                = NO (cvsba not found)
--  *With libpointmatcher      = YES (License: BSD)
--   With CCCoreLib            = NO (WITH_CCCORELIB=OFF)
-- 
--  Reconstruction Approaches:
--   With OCTOMAP              = YES (License: BSD)
--   With CPUTSDF              = NO (CPUTSDF not found)
--   With OpenChisel           = NO (open_chisel not found)
--   With AliceVision 2.4.0    = YES (License: MPLv2)
-- 
--  Camera Drivers:
--   With Freenect             = YES (License: Apache v2 and/or GPLv2)
--   With OpenNI2              = YES (License: Apache v2)
--   With Freenect2            = NO (WITH_FREENECT2=OFF)
--   With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)
--   With Kinect for Azure     = NO (WITH_K4A=OFF)
--   With dc1394               = NO (WITH_DC1394=OFF)
--   With FlyCapture2/Triclops = NO (Point Grey SDK not found)
--   With ZED                  = NO (WITH_ZED=OFF)
--   With ZEDOC                = NO (ZED Open Capture not found)
--   With RealSense            = NO (librealsense not found)
--   With RealSense2           = NO (WITH_REALSENSE2=OFF)
--   With MyntEyeS             = NO (mynteye s sdk not found)
--   With DepthAI              = YES (License: MIT)
-- 
--  Odometry Approaches:
--   With loam_velodyne        = NO (WITH_LOAM=OFF)
--   With floam                = NO (WITH_FLOAM=OFF)
--   With libfovis             = NO (libfovis not found)
--   With libviso2             = NO (libviso2 not found)
--   With dvo_core             = NO (dvo_core not found)
--   With okvis                = NO (WITH_OKVIS=OFF)
--   With msckf_vio            = NO (WITH_MSCKF_VIO=OFF)
--   With VINS-Fusion          = NO (VINS-Fusion not found)
--   With OpenVINS             = NO (ov_msckf not found)
--   With ORB_SLAM             = NO (WITH_G2O should be OFF as ORB_SLAM uses its own g2o version)
-- Show all options with: cmake -LA | grep WITH_
-- --------------------------------------------
-- Configuring done
-- Generating done
...

As mentionned in this post: https://github.com/luxonis/depthai-core/issues/91#issuecomment-804918485, if you build depthai_core in default install directory (in its build directory), do:

$ cmake -Ddepthai_DIR=~/depthai-core/build ..

It should be working with depthai shared library, see also this issue: https://github.com/luxonis/depthai-core/issues/183

[matlabbe]

Update with latest depthai ros packages. To make it work with rtabmap, here are the changes:

• depthai_bridge, switch left and right when getting Tx or P[0,3] (rtabmap assumes that Tx for left camera info is positive and Tx from right camera is negative, here Tx of left camera info was negative):

ndex 04a0858..bfb8027 100644
--- a/depthai_bridge/src/ImageConverter.cpp
+++ b/depthai_bridge/src/ImageConverter.cpp
@@ -292,7 +292,7 @@ sensor_msgs::CameraInfo ImageConverter::calibrationToCameraInfo(
 
             if(calibHandler.getStereoLeftCameraId() == cameraId) {
                 stereoFlatIntrinsics[3] = stereoFlatIntrinsics[0]
-                                          * calibHandler.getCameraExtrinsics(calibHandler.getStereoLeftCameraId(), calibHandler.getStereoRightCameraId())[0][3] / 100.0; // Converting to meters
+                                          * calibHandler.getCameraExtrinsics(calibHandler.getStereoRightCameraId(), calibHandler.getStereoLeftCameraId())[0][3] / 100.0; // Converting to meters
                 rectifiedRotation = calibHandler.getStereoLeftRectificationRotation();
             } else {
                 rectifiedRotation = calibHandler.getStereoRightRectificationRotation();

• depthai_examples, use rectified stereo images instead of the raw ones:

index 5be4831..ba2d4d7 100644
--- a/depthai_examples/src/stereo_publisher.cpp
+++ b/depthai_examples/src/stereo_publisher.cpp
@@ -26,8 +26,8 @@ dai::Pipeline createPipeline(bool withDepth, bool lrcheck, bool extended, bool s
    auto xoutDepth   = pipeline.create<dai::node::XLinkOut>();

    // XLinkOut
-    xoutLeft->setStreamName("left");
-    xoutRight->setStreamName("right");
+    xoutLeft->setStreamName("rectified_left");
+    xoutRight->setStreamName("rectified_right");

    if (withDepth) {
        xoutDepth->setStreamName("depth");
@@ -58,8 +58,8 @@ dai::Pipeline createPipeline(bool withDepth, bool lrcheck, bool extended, bool s
    monoLeft->out.link(stereo->left);
    monoRight->out.link(stereo->right);

-    stereo->syncedLeft.link(xoutLeft->input);
-    stereo->syncedRight.link(xoutRight->input);
+    stereo->rectifiedLeft.link(xoutLeft->input);
+    stereo->rectifiedRight.link(xoutRight->input);

    if(withDepth){
        stereo->depth.link(xoutDepth->input);
@@ -106,8 +106,8 @@ int main(int argc, char** argv){

    dai::Device device(pipeline);

-    auto leftQueue = device.getOutputQueue("left", 30, false);
-    auto rightQueue = device.getOutputQueue("right", 30, false);
+    auto leftQueue = device.getOutputQueue("rectified_left", 30, false);
+    auto rightQueue = device.getOutputQueue("rectified_right", 30, false);
    std::shared_ptr<dai::DataOutputQueue> stereoQueue;
    if (enableDepth) {
        stereoQueue = device.getOutputQueue("depth", 30, false)

Then launch like this:

roslaunch depthai_examples stereo_node.launch

roslaunch rtabmap_ros rtabmap.launch \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/stereo_publisher/left/image \
    right_image_topic:=/stereo_publisher/right/image \
    left_camera_info_topic:=/stereo_publisher/left/camera_info \
    right_camera_info_topic:=/stereo_publisher/right/camera_info \
    frame_id:=oak-d_frame \
    approx_sync:=true

Note: I still don't like having to set approx synchronization to synchronize left and right stereo images and their camera infos, but not sure how to make sure images/camera info are all published with exact same stamp with their ros driver.

[FPSychotic]

@matlabbe your changes were merged in Depthai-ros Noetic branch, now IT WORKS IN ROS XD

[matlabbe]

@FPSychotic indeed, the gray stereo is now working with their latest version:

roslaunch depthai_examples stereo_node.launch

# Stereo Mode
roslaunch rtabmap_ros rtabmap.launch \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/stereo_publisher/left/image \
    right_image_topic:=/stereo_publisher/right/image \
    left_camera_info_topic:=/stereo_publisher/left/camera_info \
    right_camera_info_topic:=/stereo_publisher/right/camera_info \
    frame_id:=oak-d_frame \
    approx_sync:=true

# Gray-D Mode (depth computed on camera)
roslaunch rtabmap_ros rtabmap.launch \
    args:="--delete_db_on_start" \
    rgb_topic:=/stereo_publisher/right/image \
    depth_topic:=/stereo_publisher/stereo/depth \
    camera_info_topic:=/stereo_publisher/right/camera_info \
    frame_id:=oak-d_frame \
    approx_sync:=true

I tried also the RGB-Stereo launch file for RGB camera with depth computed from the camera (from gray stereo cameras):

roslaunch depthai_examples rgb_stereo_node.launch

roslaunch rtabmap_ros rtabmap.launch \
    args:="--delete_db_on_start" \
    rgb_topic:=/rgb_stereo_publisher/color/image \
    depth_topic:=/rgb_stereo_publisher/stereo/depth \
    camera_info_topic:=/rgb_stereo_publisher/color/camera_info \
    frame_id:=oak-d_frame \
    approx_sync:=true

However, the depth image is badly registered to RGB camera:

With some rtabmap's magic, we can register the depth to rgb for convenience:

roslaunch depthai_examples rgb_stereo_node.launch

rosrun nodelet nodelet standalone rtabmap_ros/point_cloud_xyz \
    /depth/image:=/rgb_stereo_publisher/stereo/depth \
    /depth/camera_info:=/rgb_stereo_publisher/stereo/camera_info \
    _approx_sync:=false \
    _decimation:=2

rosrun rtabmap_ros pointcloud_to_depthimage \
    /camera_info:=/rgb_stereo_publisher/color/camera_info \
    _fixed_frame_id:=oak-d_frame \
    _decimation:=4

roslaunch rtabmap_ros rtabmap.launch \
    args:="--delete_db_on_start" \
    rgb_topic:=/rgb_stereo_publisher/color/image \
    depth_topic:=/image \
    camera_info_topic:=/rgb_stereo_publisher/color/camera_info \
    frame_id:=oak-d_frame \
    approx_sync:=true

However, I have some doubts about the accuracy of the rgb calibration and extrinsics, as color camera_info is taken from those hard-coded parameters: https://github.com/luxonis/depthai-ros-examples/blob/noetic-devel/depthai_examples/params/camera/color.yaml. Time synchronization between the RGB and gray stereo camera can be also an issue when we move the camera.

[saching13]

Hello @matlabbe , Sorry we took some time fixing the issues on the stereo. And as of the RGB alignment we still need to optimize the firmware. We are working on it. I Will update here once it is ready. And please feel free to let us know any suggestions or improvement that would make it even better.

[FPSychotic]

Yes, I'm getting rgb pointclouds in the same way from last week. Would be great if you add it to your hand held tutorials and even if you can, make a .launch example , specially interesting if it can be with turtlebot, just an idea. I invited to Luxonis devs to join to this conversation to take any action they decide convenient.

[saching13]

And also roslaunch depthai_examples rgb_stereo_node.launch doesn't have depthAlignment to the RGB. This is addressed in the PR here with a more generic example.

[matlabbe]

Revisiting this issue as it seems that depthai-examples branches have changed names and I have doubts that the changes above followed.

From this post https://github.com/introlab/rtabmap/issues/742#issuecomment-955180106, it seems that my proposed changes have been merged to noetic-devel branch of depthai, and I did verified 13 days after https://github.com/introlab/rtabmap/issues/742#issuecomment-967497082 that it worked.

However to this date looking at the main branches of depthai-ros and depthai-ros-examples, from the changes I proposed in this post https://github.com/introlab/rtabmap/issues/742#issuecomment-938199456, only the change in depthai_bridge actually exist in latest version: https://github.com/luxonis/depthai-ros/commit/bc32785348ea29acbf11c0700da5f708df1ae999

The proposed changes for depthai_examples seem not there. Here is an updated patch for latest version of main branch so that the rtabmap examples in this post https://github.com/introlab/rtabmap/issues/742#issuecomment-967497082 work:

diff --git a/depthai_examples/ros1_src/stereo_publisher.cpp b/depthai_examples/ros1_src/stereo_publisher.cpp
index 60ab661..79c094d 100644
--- a/depthai_examples/ros1_src/stereo_publisher.cpp
+++ b/depthai_examples/ros1_src/stereo_publisher.cpp
@@ -27,8 +27,8 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool withDepth, bool lrcheck,
     auto xoutDepth   = pipeline.create<dai::node::XLinkOut>();
 
     // XLinkOut
-    xoutLeft->setStreamName("left");
-    xoutRight->setStreamName("right");
+    xoutLeft->setStreamName("rectified_left");
+    xoutRight->setStreamName("rectified_right");
 
     if (withDepth) {
         xoutDepth->setStreamName("depth");
@@ -77,8 +77,8 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool withDepth, bool lrcheck,
     monoLeft->out.link(stereo->left);
     monoRight->out.link(stereo->right);
 
-    stereo->syncedLeft.link(xoutLeft->input);
-    stereo->syncedRight.link(xoutRight->input);
+    stereo->rectifiedLeft.link(xoutLeft->input);
+    stereo->rectifiedRight.link(xoutRight->input);
 
     if(withDepth){
         stereo->depth.link(xoutDepth->input);
@@ -131,8 +131,8 @@ int main(int argc, char** argv){
     std::tie(pipeline, monoWidth, monoHeight) = createPipeline(enableDepth, lrcheck, extended, subpixel, confidence, LRchecktresh, monoResolution);
 
     dai::Device device(pipeline);
-    auto leftQueue = device.getOutputQueue("left", 30, false);
-    auto rightQueue = device.getOutputQueue("right", 30, false);
+    auto leftQueue = device.getOutputQueue("rectified_left", 30, false);
+    auto rightQueue = device.getOutputQueue("rectified_right", 30, false);
     std::shared_ptr<dai::DataOutputQueue> stereoQueue;
     if (enableDepth) {
         stereoQueue = device.getOutputQueue("depth", 30, false);

Comparison before and after this patch (note how there is a vertical shift before the patch):

Here is a comparison using depth from depthai (note how the depth doesn't perfectly align with left image before the patch):

I will open a pull request on depthai-ros-examples and refer to this issue.

[saching13]

Hello @matlabbe , Can we use stereo_intertial_publisher example here ? It addresses all the comments you mentioned above. And also adds the option of publishing depth aligned with RGB. Let me know if I'm missing anything. I will address it and update it right away.

And as of the changing branch names. Sorry for that. I moved supporting all the versions to single branch with a plan to make a release.

C++ code of stereo intertial publisher here Launch file of the same is here

[matlabbe]

@saching13 Thank you for the fast answer. I'll test stereo_inertial_publisher example (seems providing a lot of options!). In the mean time, I still opened a pull request for stereo_publisher example.

[matlabbe]

I tested stereo_inertial_publisher, it is working great but to work with the IMU, I had to update the URDF as on my oak-d the imu x-axis is down, not right.

Original (x->right, y->down, z->forward) (look at the acceleration on right, in this orientation, it should have been y=-9.18, not x=-9.81):

New IMU frame based on my oak-d (x->down, y->left, z->forward) (now the acceleration matches the TF frame):

Note that for a static IMU on earth, the acceleration should be positive upwards (to negate gravity acceleration downwards).

The imu urdf patch:

diff --git a/depthai_bridge/urdf/include/depthai_macro.urdf.xacro b/depthai_bridge/urdf/include/depthai_macro.urdf.xacro
index ec56b1c..0838193 100644
--- a/depthai_bridge/urdf/include/depthai_macro.urdf.xacro
+++ b/depthai_bridge/urdf/include/depthai_macro.urdf.xacro
@@ -43,7 +43,7 @@
     <joint name="${camera_name}_imu_joint" type="fixed">
         <parent link="${base_frame}"/>
         <child link="${camera_name}_imu_frame"/>
-        <origin xyz="0 0 0" rpy="-${M_PI/2} 0 -${M_PI/2}" />
+        <origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
     </joint>

With the URDF fixed with the patch above, those examples are working correctly:

# RGB-D Mode
roslaunch depthai_examples stereo_inertial_node.launch
roslaunch rtabmap_ros 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 \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    wait_imu_to_init:=true
    
# Stereo Mode
roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
roslaunch rtabmap_ros rtabmap.launch \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/stereo_inertial_publisher/left/image_rect \
    right_image_topic:=/stereo_inertial_publisher/right/image_rect \
    left_camera_info_topic:=/stereo_inertial_publisher/left/camera_info \
    right_camera_info_topic:=/stereo_inertial_publisher/right/camera_info \
    imu_topic:=/stereo_inertial_publisher/imu \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    wait_imu_to_init:=true

# Gray-D Mode (depth computed on camera)
roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
roslaunch rtabmap_ros 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 \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    wait_imu_to_init:=true

@saching13 As I am not sure if it is camera specific, I didn't open a pull request on depthai-bridge with the imu urdf correction. If for all oak cameras with imu the imu should be oriented the same, I would recommend to apply the patch to fix the urdf.

[saching13]

I know the IMU position is not updated on the existing URDF. I'm working on the same as we speak along with IMU interpolation update. It is WIP in the imu-interpolation branch. https://github.com/luxonis/depthai-ros/tree/imu-interpolation

Should have that merged to main by next week.

[matlabbe]

A little update to my previous post. For the required approximate time synchronization problem, I added a new parameter called approx_sync_max_interval on rtabmap side in this commit: https://github.com/introlab/rtabmap_ros/commit/07bf207ec3696f63b112eeb5de402ad78f610fa0

A new warning is shown when we use approx_sync:=true and we detect a large time difference between the left and right images. Here is an example: [ INFO] [1646435749.740379499]: Odom: quality=174, std dev=0.006691m|0.017944rad, update time=0.100353s [ WARN] [1646435749.741419123]: The time difference between left and right frames is high (diff=0.033304s, left=1646435749.671594s, right=1646435749.638290s). If your left and right cameras are hardware synchronized, use approx_sync:=false. Otherwise, you may want to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations. [ INFO] [1646435749.827452694]: Odom: quality=47, std dev=0.170837m|0.029695rad, update time=0.085642s [ INFO] [1646435749.917822806]: Odom: quality=95, std dev=0.007719m|0.020216rad, update time=0.078818s [ INFO] [1646435750.009813045]: Odom: quality=212, std dev=0.005460m|0.016851rad, update time=0.069911s

33 ms means that a left image has been synchronized with a right image one frame later. When this happens, it is likely that rtabmap's odometry get lost if the camera is moving at the same time (disparity between left and right images will be wrong). Here the odometry quality (number of features matched) dropped during that event.

So now we can use approx_sync_max_interval to skip such bad synchronizations. Here are the same examples as above with this parameter set:

# RGB-D Mode
roslaunch depthai_examples stereo_inertial_node.launch
roslaunch rtabmap_ros 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 \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true
    
# Stereo Mode
roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
roslaunch rtabmap_ros rtabmap.launch \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/stereo_inertial_publisher/left/image_rect \
    right_image_topic:=/stereo_inertial_publisher/right/image_rect \
    left_camera_info_topic:=/stereo_inertial_publisher/left/camera_info \
    right_camera_info_topic:=/stereo_inertial_publisher/right/camera_info \
    imu_topic:=/stereo_inertial_publisher/imu \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true

# Gray-D Mode (depth computed on camera)
roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
roslaunch rtabmap_ros 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 \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true

[FPSychotic]

I could try this last fixes in my drone with a OAK-D after fix a problem with it, it is used with RPi4 and 20.04/noetic. What I found is a really low performance (OAK-D at 400p).I have similar setups with R200,d435/i, d435+t265 in RPi4,Aaeon UP,Nano (all of them same CPU power approx.), This in any of the 3 different possibilities given above, is with difference , the slower and totally unusable for real-time VSLAM. Issues I noted:

• IMU is not recognized always. Needs reboot camera and Rtabmap to be recognized.
• When IMU is used, the TF is wrong, if it is fine for you, or my Depthai branch have different UDRF or IMUs position in the PCB. If Rtabmap dont use IMU, TF is OK.
• Low performance shoots with Rtabmap, (no rviz or rtabviz), it looks rtabmap needs to do some extra (and heavy) process that my R200/D400 don't need to do, Do you have any clue why this difference of performance? I think rectification and depth is done in camera, no very different to a Realsense I think, they make pointclouds onboard, but it is not used ini ROS or Rtabmap I think, isn't it? In Realsenses I use 320p , OAK-D use 400p, in case could be that.
• IMU works at random Hz between 120 and 170 (didn't check many, just noted that finishing the test, so I would need test better, but didn't look good. Any opinion to make me a idea if these issues will be fixable or just they are part of a limitation with not easy solution would be very helpful in my case.

Edit: Time past fast, my setup is from February so there is new versions of depthai and rtabmap, so I will check, but I guess some questions are still valid, specially the performance ones, the IMU issues probably are fixed.

Edit2: Everything updated show this error with the IMU, I tried 1,2,3 mode, similar message, and have not accel, [ERROR] (2022-04-29 01:36:02.003) CoreWrapper.cpp:2506::imuAsyncCallback() IMU received doesn't have orientation set, it is ignored.

Thanks!!

[FPSychotic]

I'm receiving this error from Rtabmap and main branch of depthai, I guess due to the imu interpolation update. it send me different erros in diffterent modes, 0,1 or 2. i.e. `[ERROR] (2022-04-29 12:18:10.218) CoreWrapper.cpp:2506::imuAsyncCallback() IMU received doesn't have orientation set, it is ignored.

`

[matlabbe]

Just tried latest dephai code, it seems they don't compute the imu quaternion anymore. To recompute it, you can use madgwick filter. Here the modified Gray-D mode from my previous post:

# Gray-D Mode (depth computed on camera)
$ roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
$ 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
$ roslaunch rtabmap_ros 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

[viveksood97]

Hi, I am trying to use RTAB-Map with OAK-D S2 and ROS2.
I can see rviz properly when I run:

ros2 launch depthai_examples stereo_inertial_node.launch.py depth_aligned:=false

But I am unable to see anything on the RTAB-Map gui when I run:

ros2 launch rtabmap_ros rtabmap.launch.py \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/stereo_inertial_publisher/left/image_rect \
    right_image_topic:=/stereo_inertial_publisher/right/image_rect \
    left_camera_info_topic:=/stereo_inertial_publisher/left/camera_info \
    right_camera_info_topic:=/stereo_inertial_publisher/right/camera_info \
    imu_topic:=/stereo_inertial_publisher/imu \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true

.

I see this on the terminal where I run the RTAB-Map:
.

I can see all the topics listed when I run ros2 topic list but RTAB-Map cant them find them.

Can someone please help me find the issue? Thanks :)

[samialperen]

@matlabbe did you have a chance to check the latest depthai code? Specifially right now, they have a version with active IR (OAK-D-Pro), and I am wondering what might be the most optimized way to use it with rtabmap. Thanks!

[alvaropascuaI]

Hi @viveksood97, were you able to fix the issues and get rtabmap-ros up and running?

[an99990]

hey @alvaropascuaI , i was not able to launch the rtabmap_ros with the parameters from @viveksood97 , but it worked with this For some reason, all topics from stereo_publisher are not publishing anything for me. hope this helps

ros2 launch rtabmap_ros rtabmap.launch.py \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/left/image_rect \
    right_image_topic:=/right/image_rect \
    left_camera_info_topic:=/left/camera_info \
    right_camera_info_topic:=/right/camera_info \
    imu_topic:=/imu \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true

[matlabbe]

As of today, with ros-noetic-rtabmap-ros or rtabmap_ros latest from source, and with ros-noetic-depthai-ros, the example for OAK-D still work: http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping (see also https://github.com/luxonis/depthai-ros/issues/128#issuecomment-1258778360)

For ROS2, to use imu, madgwick filter should be started like in the ros1 example:

ros2 launch depthai_examples stereo_inertial_node.launch.py depth_aligned:=false

ros2 run imu_filter_madgwick imu_filter_madgwick_node \
   --ros-args \
   -p use_mag:=false \
   -p publish_tf:=false \
   -p world_frame:="enu" \
   -r /imu/data_raw:=/imu \
   -r /imu/data:=/rtabmap/imu

ros2 launch rtabmap_ros rtabmap.launch.py \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/left/image_rect \
    right_image_topic:=/right/image_rect \
    left_camera_info_topic:=/left/camera_info \
    right_camera_info_topic:=/right/camera_info \
    imu_topic:=/rtabmap/imu \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true \
    qos:=1

Note that qos should be 1 (reliable) to be compatible with qos from depthai topics. You can verify qos of a topic with ros2 topic info -v /left/image_rect, and make sure all publishers and subscribers are using the same qos.

However, I tested on galactic and while stereo_odometry seems working, rtabmap and rtabmapviz have still difficulty to synchronize input topics, not sure why yet...

[matlabbe]

For ROS2, I think I've found a workaround to make rtabmap and rtabmapviz correctly receiving odom topics, by setting rclcpp::QoS(1) instead of rclcpp::QoS(queueSize_) here: https://github.com/introlab/rtabmap_ros/blob/c0259f31a22f81d3f56c271448394c13fbe4afe3/src/nodelets/stereo_odometry.cpp#L85 then launching with rgbd_sync:=true:

ros2 launch rtabmap_ros rtabmap.launch.py \
    args:="--delete_db_on_start" \
    stereo:=true \
    left_image_topic:=/left/image_rect \
    right_image_topic:=/right/image_rect \
    left_camera_info_topic:=/left/camera_info \
    right_camera_info_topic:=/right/camera_info \
    imu_topic:=/rtabmap/imu \
    frame_id:=oak-d_frame \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true \
    rgbd_sync:=true \
    qos:=1

Not sure why yet, but increasing the queue_size argument of stereo_odometry node increases the delay of the output odom even if the frame rate is normal. However, for stereo_sync node, there is no delay added even with very large queue_size?!

When /rtabmap/odom has a large delay, rtabmap and rtabmapviz nodes cannot correctly synchronize the input topics.

ros2 topic delay /rtabmap/odom

For example, setting a queue_size of 100:

ros2 topic delay /rtabmap/odom
average delay: 3.587
	min: 3.571s max: 3.598s std dev: 0.00898s window: 12

then with queue_size of 1:

ros2 topic delay /rtabmap/odom
average delay: 0.235
	min: 0.209s max: 0.313s std dev: 0.03244s window: 11

There is then something wrong on how approximate sync is setup in stereo_odometry, though it looks exactly like in stereo_sync.

[matlabbe]

The delay issue on ROS2 has been fixed in this commit: https://github.com/introlab/rtabmap_ros/commit/4fb73b4279f56b1b173f0f5f0c26f41b4db8f659

Example of https://github.com/introlab/rtabmap/issues/742#issuecomment-1259965385 should work as expected!