RTAB map with vins Fusion

[stress526]

Hi

I tried RGB-D Handheld Mapping with intel realsense 435i. And I am trying to apply Vins Fusion after I know that I can use Use external odometry.

It was executed using:

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=true visual_odometry:=false odom_topic:=/vins_estimator/odometry

After that, 3D map rotates 90 degrees in the direction of the ccw.

Also on the terminal

[ WARN] [1565142019.178442386]: Could not get transform from world to camera_link after 0.200000 seconds (for stamp=1565142018.912843)! Error="Could not find a connection between 'world' and 'camera_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.200332 timeout was 0.2."

The phrase is being repeated.

Any feedback would be appreciated.

Thank you.

Sung

[matlabbe]

Hi,

Can vins_estimator be configured to do only VIO (without relocalization)? External odometry should not contain relocalization to be compatible with rtabmap. The base frame (frame_id) from vins_estimator may be different than one used by rtabmap. What Tf are published by vins_estimator? What contains /vins_estimator/odometry topic?

Another approach to use VINS as odometry in rtabmap is to build rtabmap with VINS-FUSION support (vins would need to be built with this patch so that cmake can find it when build rtabmap). After that, we can select it like in this example:

$ roslaunch rtabmap_ros rtabmap.launch \
   rtabmap_args:="--delete_db_on_start --Odom/Strategy 9 --OdomVINS/ConfigPath ~/stereo_imu_config.yaml --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
   depth_topic:=/camera/aligned_depth_to_color/image_raw \
   rgb_topic:=/camera/color/image_raw \
   camera_info_topic:=/camera/color/camera_info \
   approx_sync:=false \
   frame_id:=camera_link

Note that assuming you are using VIO, we can set Mem/UseOdomGravity to true and Optimizer/GravitySigma so that graph is optimized accordingly to gravity on loop closures.

cheers,
Mathieu

[stress526]

First of all, thank you for giving me a feedback.

- Can vins_estimator be configured to do only VIO (without relocalization)? External odometry should not contain relocalization to be compatible with rtabmap.

First, Vins_estimator does not have relocalization function.I think.
It have a loop detection function, but I didn't use it because it might make errors.

- What Tf are published by vins_estimator?

It seems that the TF of vins_estimator is made up of world, body and camera.
The picture is as follows.

- What contains /vins_estimator/odometry topic?

After checking, all values are taken, except that the twist.twist.angular is zero based on the Nav_msgs/Odometry Message.


I also tried one more attempt.

rosrun tf static_transform_publisher 0 0 0  0 0 1.57  camera camera_link 100 <-Added

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=true visual_odometry:=false odom_topic:=/vins_estimator/odometry


And, two prizes are raised, one of them is still 90 degrees, and the other comes out in the right.

Also There is no error message on the terminal as I said earlier.
[ WARN] [1565142019.178442386]: Could not get transform from world to camera_link after 0.200000 seconds (for stamp=1565142018.912843)! Error="Could not find a connection between 'world' and 'camera_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.200332 timeout was 0.2."

In this case, the TF is as follows.



Thank you for giving me another approach for the last time. I'll give it a try. (using rtabmap with VINS-FUSION support)

[matlabbe]

Hi,

I didn't have much experience with VINS-Fusion ros package to get odometry externally to rtabmap. However, I did update rtabmap+VINS integration to be compatible with D435i (see this commit and this commit). Make sure to update rtabmap and rtabmap_ros to latest version from source, then you should be able to try this example test_d435i_vio.launch.

cheers,
Mathieu

[stress526]

Thank you!

[DronistB]

I'm trying to launch the test_d435i_vio.launch with my d435i.launch
I'm moving it a bit and I'm getting immediate drift without any movement that don't stop
Do I need to create my own calibration files
Or I can use the files in:
~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/left.yaml
~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/right.yaml

If I need to create my own files can you please give me some manual on how to do it?
Thanks

[DronistB]

Have you tried it with the new versions:
RealSense ROS v2.2.15
LibRealSense v2.36.0 ?

[matlabbe]

I did update recently stereo_odometry to support better imu/image topics synchronization from D435i for VINS-Fusion: https://github.com/introlab/rtabmap_ros/commit/ae854dfb4cb8b38299e1abc7b632f178e9e67bb4

You may give a try. Before the change, VINS was drifting almost instantly. Note also the unite_imu_method parameter has been set to copy instead of linear_interpolation. It seems with latest realsense package, linear_interpolation mode is very unstable, sometimes no imu published for a couple of seconds, then VINS automatically loses its tracking. With copy mode, there is a more constant flow of imu topics, though not sure it is ideal.

https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/test_d435i_vio.launch