search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.69k stars 773 forks source link

RTABMap on Euroc V02_03 #1028

Open rachelzh9 opened 1 year ago

rachelzh9 commented 1 year ago

I'm running RTABMap on the Euroc V02_03, and OdometryF2M fails to find a transformation. When I try to run with VINS_Fusion as the VIO method, it is able to generate a trajectory estimate for a couple seconds but then the estimated pose starts to run away once there is rapid movement. When I run VINS_Fusion by itself, it is able to generate a good estimated trajectory. Any ideas on why VINS + RTAB fails, but VINS standalone works?

matlabbe commented 1 year ago

Are you using https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/euroc_datasets.launch ?

rachelzh9 commented 1 year ago

Yes I am

matlabbe commented 1 year ago

I cannot reproduce your problem. You should not enable ground truth though, it seems there is an issue with timestamps and it makes lagging odometry node and vins diverge.

[ WARN] [1682914484.037688684, 1413394884.092363794]: Could not get transform from world to base_link_gt after 0.100000 seconds (for stamp=1413394884.355761)! Error="Lookup would require extrapolation 0.081478638s into the future.  Requested time 1413394884.355760574 but the latest data is at time 1413394884.274281979, when looking up transform from frame [base_link_gt] to frame [world]. canTransform returned after 0.100582 timeout was 0.1.".

The only thing I can see is that the trajectory looks slightly less accurate than VINS node. I looked at the data received and in which order, and it seems that vins node can process IMUs with stamp larger than image stamp before processing the image. Adding some warnings in the callbacks:

rosrun vins vins_node ~/catkin_ws/src/tmp/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml

[ WARN] [1682913552.769086230, 1413394977.857865090]: process imu 1413394978.225760
[ WARN] [1682913552.769114843, 1413394977.857865090]: process imu 1413394978.230761
[ WARN] [1682913552.769184065, 1413394977.857865090]: process imu 1413394978.235760
[ WARN] [1682913552.769415422, 1413394977.857865090]: process imu 1413394978.240761
[ WARN] [1682913552.769514274, 1413394977.857865090]: process imu 1413394978.245760
[ WARN] [1682913552.769525802, 1413394977.857865090]: process imu 1413394978.250761
[ WARN] [1682913552.769578557, 1413394977.857865090]: process imu 1413394978.255760
[ WARN] [1682913552.769589423, 1413394977.857865090]: process imu 1413394978.260761
[ WARN] [1682913552.769990604, 1413394977.857865090]: process imu 1413394978.265760
[ WARN] [1682913552.770001981, 1413394977.857865090]: process imu 1413394978.270761
[ WARN] [1682913552.799288901, 1413394977.888041566]: process image 1413394978.255760

In rtabmap's odometry, only imus up to equal or greater than image stamp are processed before the image. 

roslaunch rtabmap_examples euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=false raw_images_for_odom:=true
rosservice call /rtabmap/stereo_odometry/log_debug

[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.160760
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.165761
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.170760
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.175761
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.180760
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.185760
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.190761
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.195760
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.200761
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:357::computeTransform() IMU update stamp=1413394886.205760
[DEBUG] (2023-04-30 21:04:45.474) OdometryVINS.cpp:386::computeTransform() Image update stamp=1413394886.205760

When changing this line to:

std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.end();

it makes the odometry node behave more vins node, passing imu data as soon as possible to estimator even if we receive an image afterwards with stmap in the past. It seems to improve a little the trajectory, but just looking at rviz it is difficult to say. Ideally, exporting the poses and compare to ground truth would give a better idea if it indeed works better. I could add a parameter to odometry to always forward all data to underlaying odometry approach.

rachelzh9 commented 1 year ago

I tried not recording ground truth, and that seemed to work for me, thank you. Additionally, for some difficult sequences like MH_05, I have observed that vanilla rtabmap VO outperforms VIO methods like VINS_fusion even though the sequence contains more motion blur where VIO methods should provide a theoretical boost in performance. Do you have any intuition on why this might be happening?

matlabbe commented 1 year ago

MH sequences are in larger spaces, thus local bundle adjustment done in rtabmap's F2M approach could help to better estimate farther features. I don't remember how much motion blur has MH_05, but definitely for V02_03, VIO would outperform standard VO because exposure time between left and right images may be different, and there are very fast motions (causing lot of motion blur) in small space, where sometimes no visual feature matching possible and thus has to rely only on IMU for short period of time.