Trajectory jumps with visual odometry

[osbjo]

Hi!

I am using the part of the rawseeds dataset and RTAB-Map to investigate the robustness of different sensor configurations when doing SLAM, and localization in a known map. I Have posted earlier concerning weird map behavior which I experienced due to subpar stereo image rectification. It is still not perfect, and I cannot redo the camera calibration since I use a dataset, but now my maps look visually good.

The issue at hand now is what can be seen in the following image. When using stereo odometry, much of the estimated trajectory is shifted away from the GT in these discrete jumps. This does not happen when using external wheel encoder odometry (see the second figure). I wonder what might cause this, and what I can do to rectify the issue. I have tried running with both the default parameters Viz/MinInliers and Odom/MinInliers, and with modified values (larger and smaller values), but to no avail.

 

[matlabbe]

It feels like it is jumping between two TF published on same name. If the bag has already odometry TF in it and you want to try stereo odometry, two options:
* Remove the odom tf from the bag (with a script similar to this if you are on ros1), and make sure you are not also using the same odometry topic name than in the bag.
* set "guess_frame_id" parameter in stereo_odometry node and set it to your wheel odometry TF frame. stereo_odometry will then publish the correction between VO and wheel odom in the TF tree to avoid breaking the TF tree (final tf tree would look like this: map->vo->odom->base_link).

Can you link which sequence in rawseeds dataset you are testing with?

[osbjo]

Thank you for directing me toward the issue of overwriting /tf. That was the issue. My bag published a the wheel odometry transform from /odom -> /base_link, and removing this when running in VO mode solved the issue.

I have produced the bag myself from the raw data found in Rawseeds dataset Dropbox, but I don't suppose this will be of interest any more.

[matlabbe]

Thanks for sharing, I tried the dataset in the standalone version (directory of stereo images + rectification enabled) with this calibration:

"bicocca_calib_left.yaml"

%YAML:1.0
---
camera_name: bicocca_calib_left
image_width: 640
image_height: 480
camera_matrix:
   rows: 3
   cols: 3
   data: [ 6.6047200699999996e+02, 0., 3.1609229900000003e+02, 0.,
       6.6047200699999996e+02, 2.1427980900000000e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 5
   data: [ -2.7007730485883452e-01, 0., -1.3143024456984870e-04, 0.,
       1.9823091285521721e-01 ]
distortion_model: plumb_bob
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9947380074257286e-01, 2.8028135809627101e-02,
       1.6326213040173919e-02, -2.8013763049520503e-02,
       9.9960692298331477e-01, -1.1084239418636529e-03,
       -1.6350862637835521e-02, 6.5048202640513532e-04,
       9.9986610411801236e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 6.8930182551354210e+02, 0., 3.0540074539184570e+02, 0., 0.,
       6.8930182551354210e+02, 2.2811547279357910e+02, 0., 0., 0., 1.,
       0. ]
local_transform:
   rows: 3
   cols: 4
   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]

"bicocca_calib_right.yaml"

%YAML:1.0
---
camera_name: bicocca_calib_right
image_width: 640
image_height: 480
camera_matrix:
   rows: 3
   cols: 3
   data: [ 6.6476301500000000e+02, 0., 3.2734811100000002e+02, 0.,
       6.6476301500000000e+02, 2.4536088400000000e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 5
   data: [ -2.9385724883178782e-01, 1.4126458924224591e-01, 0.,
       1.7949559052512960e-03, 0. ]
distortion_model: plumb_bob
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9913931020706770e-01, 3.3554143770965016e-02,
       2.4387665627166735e-02, -3.3575586030339631e-02,
       9.9943607047475580e-01, 4.7016667066965512e-04,
       -2.4358136662403836e-02, -1.2885921683592969e-03,
       9.9970246609106617e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 6.8930182551354210e+02, 0., 3.0540074539184570e+02,
       -1.2476243955923299e+02, 0., 6.8930182551354210e+02,
       2.2811547279357910e+02, 0., 0., 0., 1., 0. ]
local_transform:
   rows: 3
   cols: 4
   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]

"bicocca_calib_pose.yaml"

%YAML:1.0
---
camera_name: bicocca_calib
rotation_matrix:
   rows: 3
   cols: 3
   data: [ 9.9995243549346924e-01, -5.5742207914590836e-03,
       -8.0054979771375656e-03, 5.5595920421183109e-03,
       9.9998289346694946e-01, -1.8484065076336265e-03,
       8.0156642943620682e-03, 1.8038111738860607e-03,
       9.9996626377105713e-01 ]
translation_matrix:
   rows: 3
   cols: 1
   data: [ -1.8084248900413513e-01, -6.0732420533895493e-03,
       -4.4141253456473351e-03 ]

I had difficulty to find documentation about the format used for the calibration, so I guessed what meant the data inside the *.mat files. The stereo rectification looks ok:
 

and the visual odometry...


till the camera rotates in front of a textureless wall:



There is also noticeable motion blur at the same time, making visual odometry being lost. With rtabmap_ros, one thing that can be done to recover from this is, if there is also wheel odometry, set guess_frame_id to wheel odometry TF, set publish_null_when_lost to false (to avoid triggering new map) and set Odom/ResetCountdown > 0. That way, visual odometry can be reset after a couple of frames being lost, then restart accordingly to wheel odometry motion while vo was lost, while not triggering a new session on rtabmap side.

[osbjo]

Thank you Mathieu for taking the time to look at this.

I managed to obtain similar stereo rectification to the one you got, so that is good for me!

And I will try to use the guess_frame_id setting to se if it resolves the issues when VO is lost. I will, however, have to put this project on the side for the time being as I have more pressing matters to attend to, but if I have the time, I will report back on how it went!