Stereocamera with RTAB-map: no features extracted in odometry and invalid camera model

[bramth]

Dear Mathieu,

First of all thank you for keeping this forum up and running for the many questions of us, the users :-).

Now let us dive in. This might be a big one, I’ll try to be as complete as possible. I’m trying to get this stereo camera by e-consystems working with ROS and RTAB-map on a Jetson TX2 (later on the Xavier), but having a very difficult time:
https://www.e-consystems.com/nvidia-cameras/jetson-agx-xavier-cameras/stereo-camera.asp
https://github.com/econsystems/taraxl-ros-package/blob/master/taraxl_ros_package/launch/taraxl.launch
As can be seen in the second link, it does have a factory ROS package, but unfortunately this did not quite work as intended. As can be seen in src/taraxl.cpp, the messages were not synchronized (time-stamp) based at all, so I fixed that. I also removed some of the published topics for speed reasons and removed the thread based execution. Also the published topics are now /taraxl/left/image_rect and /taraxl/left/camera_info instead of the camera parameters within the /taraxl/left/calib/ group. The camera is factory calibrated, so it contains both the /taraxl/left/image_raw and /taraxl/left/image_rect. It also publishes a depth image in /taraxl/depth/image. I’ll make sure to post the edited package with this post. (The synchronization of time-stamps was not only necessary for rtabmap_ros (I think I could have used approx_sync=true), but apriltag_ros also requires this (which does works with this edit)).

Next, the launch file contains a static transform from /map to /taraxl, which I changed from /base_link to /camera_link (standalone, not being on a robot just yet).

Now I tried to run rtabmap with visual (stereo) odometry, according to http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping. The first error we get is that there exists no transform between /camera_link and /camera_left, which I need to insert into the launch file as a static transform below the base_link to camera_link, with a “guess” distance of 10 cm:

<node name="camera_transform" pkg="tf" type="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /base_link /camera_link 10" />

<node name="camera_transformL" pkg="tf" type="static_transform_publisher" args=" 0 0 0 0 0 /camera_link /camera_left 10" />
<node name="camera_transformR" pkg="tf" type="static_transform_publisher" args="-0.1 0 0 0 0 0 /camera_link /camera_right 10" />

Without these transforms, we get the error:

[ WARN] [1600694703.279360110]: odometry: Could not get transform from camera_link to camera_left (stamp=1600694702.988393) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="canTransform: source_frame camera_left does not exist.. canTransform returned after 0.202086 timeout was 0.2."

I would guess it should be able to get this information from the camera_msgs, or does the camera node publish this transform usually?

The next error after adding the transforms is that the the D (distortion) vector does not pass some assertion, being that it is a vector of length 14 and not the CV_64FC1 format. Since it (factory-wise) uses a plump-bob model, we slice the first 5 elements and .convertTo the CV_64FC1 format. It is still weird that the D vector is an entirely zero-valued vector, but hey, the camera is also quite good (no distortion visible by eye when keeping objects very close).

Next we checked the /left & /right camera_info messages and saw in some of your other answers that a very large value for P(0,3) is unrealistic. We had a value of 14592 there, so it is very likely that this value is in centimeters, and thus needs to be divided by 100, which we did (a value 1000 didn't change things either). It is also very strange that both the left and right camera have exactly the same camera parameters (yes I checked, I did not mistype left for right or vice versa somewhere).

We created a new launch file that only runs the stereo odometry node, and keep getting the following message

[ WARN] (2020-09-21 15:22:27.083) OdometryF2M.cpp:1256::computeTransform() 10 visual features required to initialize the odometry (only 0 extracted).
[ INFO] [1600694547.083737335]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.256637s

And yes, the image is not a white wall, anything but, so it should be able to find features.


We then tried to only use RGBD odometry, but got the following error immediately*: “No valid camera model!”, which can be retraced in the source code of RTAB-map:
https://github.com/introlab/rtabmap/blob/master/corelib/src/odometry/OdometryF2M.cpp#L642
https://github.com/introlab/rtabmap/blob/master/corelib/src/odometry/OdometryF2M.cpp#L1192
(* Well actually we first had to convert the depthmap to 16bit (CV_16UC1) because rtabmap does not allow a mono8 depth map.) Here, we find the isValidForProjection() constraint is violated, found in
https://github.com/introlab/rtabmap/blob/e0973fea92bf6de9495ac44b5bf639288c32e533/corelib/include/rtabmap/core/CameraModel.h#L87
Apparently our K matrix does not adhere to all(K > 0), but as visible in the camera_info, it is:

K: [1459.6129976783102, 0.0, 802.8454055786133, 0.0, 1459.6129976783102, 608.2273559570312, 0.0, 0.0, 1.0]

This is what the camera_info msgs for both left and right look like:

---
header: 
  seq: 5423
  stamp: 
    secs: 1600695061
    nsecs: 224249028
  frame_id: "camera_left"
height: 1300
width: 3200
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [1459.6129976783102, 0.0, 802.8454055786133, 0.0, 1459.6129976783102, 608.2273559570312, 0.0, 0.0, 1.0]
R: [0.9999962320195279, 0.0018907703081777391, -0.001990209634262717, -0.0018908545510503377, 0.9999982115149181, -4.0447891859735255e-05, 0.0019901295971295205, 4.421093639732365e-05, 0.9999980187128271]
P: [1459.6129976783102, 0.0, 802.8454055786133, 0.0, 1459.6129976783102, 608.2273559570312, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

---
header: 
  seq: 5845
  stamp: 
    secs: 1600695091
    nsecs: 368014789
  frame_id: "camera_right"
height: 1300
width: 3200
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [1459.6129976783102, 0.0, 802.8454055786133, 0.0, 1459.6129976783102, 608.2273559570312, 0.0, 0.0, 1.0]
R: [0.9999962320195279, 0.0018907703081777391, -0.001990209634262717, -0.0018908545510503377, 0.9999982115149181, -4.0447891859735255e-05, 0.0019901295971295205, 4.421093639732365e-05, 0.9999980187128271]
P: [1459.6129976783102, 0.0, 802.8454055786133, -145.9251079094203, 1459.6129976783102, 608.2273559570312, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

I will add a rosbag with the stereo_odometry node enabled. See this link: https://bramth1995.stackstorage.com/s/Anfq1EXgLJH8ld5c
Do you have any clue how to solve these issues? Thank you!

Kind regards, Bram

[bramth]

First problem found: the P matrix is incorrect. At position P(0,4) a 0 should be placed instead the value for f_y. This is clear from the fact that it uses the same cameraMatrix, being of size (3,3), for both K and P. Going back to the C++ file, we change

rightCalibMsg.P[4]=right.cameraMatrix.at<double>(4);
rightCalibMsg.P[5]=right.cameraMatrix.at<double>(5);

to

rightCalibMsg.P[5]=right.cameraMatrix.at<double>(4);
rightCalibMsg.P[6]=right.cameraMatrix.at<double>(5);

and

leftCalibMsg.P[4]=left.cameraMatrix.at<double>(4);
leftCalibMsg.P[5]=left.cameraMatrix.at<double>(5);

to

leftCalibMsg.P[5]=left.cameraMatrix.at<double>(4);
leftCalibMsg.P[6]=left.cameraMatrix.at<double>(5);

This actually gives a proper odometry for both depth and stereo odometry, but when using stereo we do get a computeCorrespondences warning, telling us that "Stereo.cpp:160::computeCorrespondences() A large number (615/972) of stereo correspondences are rejected! Optical flow may have failed because images are not calibrated, the background is too far (no disparity between the images), maximum disparity may be too small (128.000000) or that exposure between left and right images is too different.".
I highly doubt the calibration is proper, and we might need to redo it based on http://wiki.ros.org/camera_calibration/Tutorials/StereoCalibration.

[matlabbe]

Good catch about the P matrix. For the warning about low valid stereo correspondence, it could also depend on what the camera is looking at. However, if the warning is also shown, there is maybe a problem with the calibration or stereo matching parameters. Can you share a database as you can map now?