Open bmorawska opened 3 years ago
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):
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
. -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
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).
@matlabbe Can you please give the link for "oak-d driver for RTAB-Map standalone " . I am unable to google it. Thanks
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
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]
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
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.
@matlabbe your changes were merged in Depthai-ros Noetic branch, now IT WORKS IN ROS XD
@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.
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.
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.
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.
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.
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
@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.
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.
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.
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
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:
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!!
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.
`
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
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 :)
@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!
Hi @viveksood97, were you able to fix the issues and get rtabmap-ros up and running?
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
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...
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.
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!
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:
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:
and launch file is:
Can anyone have faced similar problem or can help me with that?