Scaling issue Zed2 odometry

[qvoortman]

Hello,

We have been using rtabmap to obtain live odometry in a project with a zed2 camera attached to a drone.

The picture below shows the resulting odometry from rtabmap as well as the ground truth. Clearly, something is off in the scaling of the odometry. How can this be fixed?

Thanks in advance.

QVoortman

[matlabbe]

Hi,

To make sure, this is the trajectory computed by rtabmap's odometry, not zed's odometry. The scale of the trajectory will depend one the camera calibration (focal distance) or baseline between the cameras (in stereo mode) or scaled depth values (rgb-d mode).

Can you evaluate the scale error? I tried all zed examples from those pages (http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping, http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping) and the scale seems okay.

Here are the camera info on my zed2 camera:

$ roslaunch zed_wrapper zed_no_tf.launch

$ rostopic echo /zed_node/left/camera_info
header: 
  seq: 193
  stamp: 
    secs: 1635951065
    nsecs:    500313
  frame_id: "zed2_left_camera_optical_frame"
height: 376
width: 672
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [263.1452941894531, 0.0, 374.1166687011719, 0.0, 263.1452941894531, 188.2142333984375, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [263.1452941894531, 0.0, 374.1166687011719, 0.0, 0.0, 263.1452941894531, 188.2142333984375, 0.0, 0.0, 0.0, 1.0, 0.0]

$ rostopic echo /zed_node/right/camera_info
header: 
  seq: 77
  stamp: 
    secs: 1635950993
    nsecs: 444169313
  frame_id: "zed2_right_camera_optical_frame"
height: 376
width: 672
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [263.1452941894531, 0.0, 374.1166687011719, 0.0, 263.1452941894531, 188.2142333984375, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [263.1452941894531, 0.0, 374.1166687011719, -31.415287017822266, 0.0, 263.1452941894531, 188.2142333984375, 0.0, 0.0, 0.0, 1.0, 0.0]

To compute the baseline, do -P[0,3]/P[0,0] on right camera_info, which in this case is -(-31.415)/263.145 = 0.119 (it should be close to 12 cm)

[qvoortman]

To obtain the odometry of rtabmap, we use the launch options:
        <arg name="frame_id"                value="base_link"/>
        <arg name="map_frame_id"       value="rtabmap_map"/>
        <arg name="odom_topic"            value="rtabmap_odom"/>
and a custom node that remaps messages on the /tf topic from rtabmap_odom->base_link to rtabmap_odom->rtabmap_link. The trajectory displayed in the above figure under then name traj_rtabmap_72 is then simply the transform from rtabmap_map->rtabmap_odom->rtabmap_link in real time.

Here is what I obtain on both topics:

rostopic echo /zed2/zed2/zed_node/left/camera_info
header:
  seq: 94
  stamp:
    secs: 1626945384
    nsecs: 564205182
  frame_id: "zed2_left_camera_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [521.0396728515625, 0.0, 631.9817504882812, 0.0, 521.0396728515625, 353.6788024902344, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [521.0396728515625, 0.0, 631.9817504882812, 0.0, 0.0, 521.0396728515625, 353.6788024902344, 0.0, 0.0, 0.0, 1.0, 0.0]

rostopic echo /zed2/zed2/zed_node/right/camera_info
Header:
  seq: 145
  stamp:
    secs: 1626945386
    nsecs: 494957182
  frame_id: "zed2_right_camera_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [521.0396728515625, 0.0, 631.9817504882812, 0.0, 521.0396728515625, 353.6788024902344, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [521.0396728515625, 0.0, 631.9817504882812, -62.505550384521484, 0.0, 521.0396728515625, 353.6788024902344, 0.0, 0.0, 0.0, 1.0, 0.0]

The ratio you are talking about is verified for the above code.

Do you see another reason why the scaling might be off ?

[matlabbe]

Hi,

the calibration looks fine, the baseline is 12 cm. It could be that the depth image has some scale issue. Can you share a database?

cheers,
Mathieu

[qvoortman]

Yes, this would be great.

Would it be possible to share the bag files via a link to your email address? (The bag files are quite large)

[matlabbe]

Yes it is possible, you can send me a private email when clicking on my username.

[matlabbe]

Calibration parameters seem okay (similar to mine). However, the right and left images are not correctly stereo rectified. There is a vertical shift between the left and right images. Here is a zoomed section of the overlapped left and right images:


I checked with my Zed2, and I have similar issue and the disparity can be negative depending on the side of the image.
Left: disparity is negative, right appears on right as it should appear on left. There is also a vertical shift.

When showing the disparity image, StereoBM cannot estimate disparity of the tree when it is on left of the image (because of a negative disparity), while it can when the tree is on the right region of the image:



I reproduced the same issue with Zed SDK (v3.5.5):



It seems there is a calibration tool for ZED cameras: https://www.stereolabs.com/docs/video/camera-calibration/#calibration-tool, however:

However, we do not recommend this for ZED 2 cameras. They go through extensive and rigorous multi-step factory calibration (including thermal measurements), and a manual calibration might degrade its calibration parameters.

The best bet would be to check with stereolabs for this issue.