How to improve the test result on a car?

[Userpc1010]

I did several test runs on the car in open areas and the private sector. Moving in the private sector does not cause problems, except for the periodic distortion of the camera's roll orientation on a turn. I believe this is caused by the distortion of the gravity vector (see in test 2) in the Mudgwick filter and perhaps a simple way to solve this problem is to stop the combination of the gyroscope and the accelerometer if the length of the gravity vector is greater than or noticeably less than its standard value. I also noticed that the odometery disappears when leaving for an open area (in a planar scene), is there a way to fix this? Putting the camera on a lower vehicle such as an ATV can improve visual odometry in open area by having lower plants in the field of view that appear flatter from above? A possible solution I see is to add GNSS data and wheel odometery how can this be done in the current version app? Does it make sense to use a special version of the UBLOX M8L GNSS receiver that combines the combination of an accelerometer, gyroscope and wheel odometery to use its navigation solution in RtabMap?

Test CPU 1 https://youtu.be/os4Q9WzbcsE

Test with GPU 2 https://youtu.be/AXuscRb9n5o

[matlabbe]

The "not moving" effect at 2:10 on the first video is similar to KITTI dataset sequence 1 on the highway. To avoid this, We should reduce Vis/CorGuessWinSize to 20. Based on KITTI benchmark here, the Vis/CorNNDR has been also reduced to 0.6 for better matches.

On second video, are you using SURF for odometry? I would use SURF only for loop closure detection, not visual odometry. For visual odometry, the default features should be okay.

For the wrong gravity vector when the car is on a long curve is interesting, that could be indeed an issue of using VIO that way. Not integrating IMU in vo bundle adjustment when this happens (checking length of gravity vector like you said) could be indeed be done (with some code modification). EDIT: a paper describing this problem:

As mentioned there, the [madgwick] filter only estimates the correct attitude, if no external acceleration is actuating the vehicle. The calculated roll and pitch angles are actually just valid while standing still, not while accelerating/braking or cornering.

VO from rtabmap won't work only with IMU measurements (accelerometer is not used for pose estimation, vo will only care about the orientation of the IMU), in the second video when the 3D map was updated and not the 2D visualization, I think it is just an UI bug, with too much data to show. For large-scale environment, I would disable the 3D rendering of the Map cloud to keep the UI smooth.

You could give an external odometry pose (computed by your UBLOX) as guess to visual odometry (guess_frame_id parameter), then when not enough visual features are detected, visual odometry can just output the guess odometry, then reset from there. However, ideally visual doometry should be always able to track some features, otherwise there could be some discontinuities.

If you recorded rosbags, I could be interested to give it a try.

cheers, Mathieu

[Userpc1010]

I think I found a possible solution to the problem with the gravity vector in the INAV 1.0 autopilot code since the Mahoney filter uses a similar principle of integrating data with an accelerometer, I think this might work.

RosBug file at start and gravity distortion

RosBug file at running slam planar scene & close the loop

[matlabbe]

Hi,

The large orientation drift could be reduced by switching to IR stereo mode, because the disparity computed by realsense is missing many far visual features that are super useful to estimate rotation.

I have also doubt about registration of depth to RGB, in particular for accurate time synchronization. Stereo IR mode would make sure that there is no error included by time sync issues between different cameras.

[Userpc1010]

I turned off the IR projector because it could create glare but when installing the camera on the outside of the car or test wheel platform I use the IR projector because it is very useful at night & indoor (under a canopy). I believed that if the RGB camera is equipped with a global shutter in the D455, then there synchronize the cameras easier using the external triger input of the camera matrix when operating at the same frequencies 30-60Hz cameras, I also believed that the camera uses one clock generator for all matrices, which together should give an ideal synchronization of 3 cameras with global shutter (according to Table 3-19. Color Sensor Properties - D450 & Table 3-14. Wide Left and Right Imager Properties - D450 uses one type of sensor OmniVision Technologies OV9782 although the left and right cameras are monochrome, the sensors are color). If use the D455 monochrome cameras, then probably the IR projector will have to be constantly turned off so as not to interfere with odometry?

[matlabbe]

To test if RGB camera is indeed exactly sync with IR cameras, you can do fast rotations, then check how good is the depth registration. If there is a time sync issue, the depth won't align correctly with corresponding RGB pixels.

For stereo cameras, you should disable the IR projector: see also this link for stereo mode I was talking about to be able to use very far features.

[Userpc1010]

I made several rotation tests in 3D mode of overlaying rgb textures on polygons formed by a three-dimensional cloud of measurements from the obtained depth in order to understand from the rgb picture what depth should be present at the current point in space and did not notice the problem.

I also did a test again on the car recording all the topics from the camera without ir projector, could you retest and compare the drift in both modes?

Rosbag_all_topic_d455

Short_tf_static_rqt

[matlabbe]

Hi, using stereo mode is a lot better, we can see that features can be extracted very far:

The resulting trajectory looks more realistic, though I don't know exactly what it should like. Did you close the big loop closure in that bag? That would be easier to estimate the actual drift.

Commands I used (based on this tutorial for D435i):

roslaunch rtabmap_launch rtabmap.launch \
   rtabmap_args:="--delete_db_on_start" \
   left_image_topic:=/camera/infra1/image_rect_raw \
   right_image_topic:=/camera/infra2/image_rect_raw \
   left_camera_info_topic:=/camera/infra1/camera_info \
   right_camera_info_topic:=/camera/infra2/camera_info \
   stereo:=true \
   wait_imu_to_init:=true \
   imu_topic:=/rtabmap/imu \
   use_sim_time:=true

rosrun tf static_transform_publisher 0 0 0 -1.57 0 -1.57 camera_link camera_infra1_optical_frame 100
rosrun tf static_transform_publisher 0 0 0 0 0 0 camera_infra1_optical_frame camera_imu_optical_frame 100

rosrun imu_filter_madgwick imu_filter_node \
    _use_mag:=false \
    _publish_tf:=false \
    _world_frame:="enu" \
    /imu/data_raw:=/camera/imu \
    /imu/data:=/rtabmap/imu

rosbag play --clock --pause 2023-06-26-13-00-39.bag

Tips: I recommend to record /tf and /tf_static when your record a bag, otherwise I should guess what are the transforms between the sensors.

[Userpc1010]

The resulting trajectory looks much better! Here is the actual trajectory on the map:

Because corridor scan is used for better visualization, the real distance 5400m in the screenshot True, it’s not entirely clear to me why, with the same baseline between cameras and lens focus, the realsense algorithm works worse, as far as I remember, the camera D455 uses a modified SGM, baseline 95mm, focusing lenses 1.9mm. What matching algorithm was used in this test? In my tests with a homemade stereo camera, I used SBM & SGBM, they were far inferior in accuracy to the realsense solution, although the baseline was more than 300mm focusing lenses 2.8mm.

I tried to close a large 1km+ loop without going out into the open area and got good accuracy in rgb-d mode, the tracks matched before the loop was closed

Thank you for your time!

[matlabbe]

Some things to check for your custom setup: Are images from your custom stereo camera exactly captured at the same time? How good is the calibration?

In stereo mode, rtabmap's stereo correspondences are computed with a optical flow approach between left and right images. For very far features (very small disparity close to 0), they would still be used like mono observations in local BA. This will improve orientation estimation. However, I'll need to double-check if it is actually the case, as in my animated gif, points on the clouds are never inliers (green).

Note that if you provide a /gps/fix topic to rtabmap, it will be easier to overlay the estimated trajectory on google maps for ground truth comparison.