RGB-D odometry using turtlebot4

[IacopoFornai]

hi! i'm running

ros2 run rtabmap_odom rgbd_odometry --ros-args
-r /rgb/image:=/color/video/image
-r /depth/image:=/stereo/depth
-r /rgb/camera_info:=/color/video/camera_info
-p approx_sync_max_interval:=0.02

i need the remappings because i've launched a driver to get a depth image topic (otherwise by defalut the turtlebot doesn't have one).
/color/video/image is a bgr image topic (bgr8 encoding)
/stereo/depth is a depth image (16UC1 encoding)

i need to set approx_sync_max_interval:=0.02 to avoid some warnings

the problem is that i'm not able to see anything but warnings:

[ WARN] (2024-11-27 11:20:32.863) MsgConversion.cpp:1994::getTransform() (can transform base_link -> oak_rgb_camera_optical_frame?) Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist. canTransform returned after 0.101187 timeout was 0.1. (wait_for_transform=0.100000)

how can i solve this problem? Thank you in advance

[AnhHazz]

does your robot have base_link link? As the warning said it doesn't exist

[IacopoFornai]

i think it doesn't have a base_link link because i launch driver connecting the camera to my PC through usb connection. So maybe i can see just camera's frame. Running
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link oak_rgb_camera_optical_frame
i don't have these warnings and odometry seems to work. But i don't know if it is a good solution.

[AnhHazz]

I think it's good because every robot has base_link.
Or you can remap base_link to oak_rgb_camera_optical_frame. The launch file should has that

[matlabbe]

You should use the base frame on the robot/camera of your TF tree (ros2 run tf2_tools view_frames.py). If you don't have a base_link, you may have another base frame for the camera that is not the optical frame.

You can add -p frame_id:=oak_rgb_camera_optical_frame to rgbd_odometry node directly if you want.

i need the remappings because i've launched a driver to get a depth image topic (otherwise by default the turtlebot doesn't have one).

What kind of driver? You should make sure that the depth image is correctly registered to your RGB camera. If you are using an OAK camera, they would have already depth computed on the camera.

[IacopoFornai]

i'm running this driver:
ros2 launch depthai_examples stereo_inertial_node.launch.py
because it's the only way to obtain a depth image topic with type of message "Image" (and not compressedImage) and with images of the right size (i need rgb and depth images to be the same size , otherwise odometry is not working). i need to connect my pc to let this driver work, if i run without usb connection i'm getting an error.

i tried also to change parameter setting in ssh (without usb connection):
ros2 param set /turtlebot4_1/oakd camera.i_pipeline_type RGBD
but in this way i have these topics:
/turtlebot4_1/oakd/rgb/preview/camera_info
/turtlebot4_1/oakd/rgb/preview/image_raw
/turtlebot4_1/oakd/rgb/preview/image_raw/compressed
/turtlebot4_1/oakd/rgb/preview/image_raw/compressedDepth
/turtlebot4_1/oakd/rgb/preview/image_raw/theora
/turtlebot4_1/oakd/stereo/camera_info
/turtlebot4_1/oakd/stereo/image_raw
/turtlebot4_1/oakd/stereo/image_raw/compressed
/turtlebot4_1/oakd/stereo/image_raw/compressedDepth
/turtlebot4_1/oakd/stereo/image_raw/theora

is /turtlebot4_1/oakd/stereo/image_raw a ros2 topic for depth image? i'm asking because the encoding is 16UC1 but when i check the parameter:

ros2 param get /turtlebot4_1/oakd .oakd.stereo.image_raw.format
String value is: jpeg

[matlabbe]

When doing:

ros2 launch depthai_examples stereo_inertial_node.launch.py

I see this:

ros2 topic list
/clicked_point
/color/camera_info
/color/image
/color/image/compressed
/color/image/compressedDepth
/color/image/theora
/color/preview/camera_info
/color/preview/image
/color/preview/image/compressed
/color/preview/image/compressedDepth
/color/preview/image/theora
/color/yolov4_Spatial_detections
/image/compressed
/image/compressedDepth
/image/theora
/imu
/initialpose
/joint_states
/move_base_simple/goal
/parameter_events
/robot_description
/rosout
/stereo/camera_info
/stereo/converted_depth
/stereo/depth
/stereo/depth/compressed
/stereo/depth/compressedDepth
/stereo/depth/theora
/stereo/points
/tf
/tf_static

Tf tree looks like this:

So for a quick rtabmap test, you can do:

ros2 launch rtabmap_launch rtabmap.launch.py \
   args:="-d" \
   depth_topic:=/stereo/depth \
   rgb_topic:=/color/image \
   camera_info_topic:=/color/camera_info \
   approx_sync_max_interval:=0.02 \
   frame_id:=oak-d-base-frame \
   rgbd_sync:=true

On my machine and my camera (OAK-D), the result is kinda laggy because the camera has huge delay up to 800 ms!

ros2 topic delay /stereo/depth
average delay: 0.727
	min: 0.568s max: 0.865s std dev: 0.08305s window: 22
average delay: 0.672
	min: 0.512s max: 0.865s std dev: 0.10181s window: 42
average delay: 0.706
	min: 0.512s max: 0.865s std dev: 0.09819s window: 63

There is may a bug on depthai example.

[IacopoFornai]

there is a weird problem: if i launch
ros2 launch depthai_examples stereo_inertial_node.launch.py
i cannot see any data with echo , for example using:
ros2 topic echo /color/image
ros2 topic echo /stereo/converted_depth
i cannot see any data on the terminal

[IacopoFornai]

i tried another configuration using:
ros2 launch depthai_examples rgb_stereo_node.launch.py usePreview:=True previewHeight:=720 previewWidth:=1280
(in this way i have rgb and depth image of same dimensions)

then if i launch:
ros2 run rtabmap_odom rgbd_odometry --ros-args
-r /rgb/image:=/color/preview/image
-r /depth/image:=/stereo/depth
-r /rgb/camera_info:=/color/preview/camera_info
-p approx_sync_max_interval:=0.02

i have this warnings:
@

and i'm solving it using this command:
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link oak_rgb_camera_optical_frame

now everything seems to work well, i just need to check the quality of odometry, is there a way to do it?

[matlabbe]

The frame rate is not constant, the camera driver stops publishing the images for a couple of seconds.

You can debug the frame rates with ros2 topic hz tool. You can do it on each topic to make sure their publishing rate is constant and similar. If not, you may ask the question on depthai ros github.

[IacopoFornai]

launching:
ros2 launch turtlebot4_bringup oakd_example.launch.py previewWidth:=1280 previewHeight:=720
i obtain the lowest delay (i need to change previewWidth and Preview height to match stereo/image_raw topic that publish 720x1280 images otherwise t RGBD odometry is not working):

do you think the odometry will work well with this value of delay? there are some parameter settings i can follow for having good results?

[IacopoFornai]

using the same configuration of previous issue i run RGBD odometry, when robot is not moving it seems to work but when i move it just a bit i have this warning:
[ WARN] (2024-12-06 10:35:13.705) OdometryF2M.cpp:569::computeTransform() Registration failed: "Not enough inliers 10/20 (matches=105) between -1 and 47" (guess=xyz=0.000899,-0.000626,-0.001164 rpy=-0.000165,-0.002129,0.000777)

i attach some screen of my terminal:

is this problem related to the delay i have( posted in the previous issue)?

[matlabbe]

Are the preview image and depth correctly registered together? Can you share a rosbag of the three input topics? The update time looks also very long, too long (>400 ms), the robot cannot move very fast or it will lose track of odometry. You may try starting the camera driver with smaller resolution, or set Odom/ImageDecimation to 2 or 4.

[IacopoFornai]

i think preview images and depth images are not registered together but i don't know a way to inspect it, how can i share you a rosbag?I cannot send it here beacuse file type is not supported.
i will try also to run the camera with smaller resolution to see if the problem is due to a weakness of the hardware.

[matlabbe]

If you have a dropbox or google drive, you can share a public link to that file.