using vins fusion slam

[youngjae41]

@matlabbe Hello, I am trying to use vio with vins fusion and mapping with rtabmap using this good code.

I downloaded the rtabmap source and set vinsFusion=On to complete the installation. But can you tell me the launch file to use this vins fusion as odom?

I made settings related to vins and odometry in arg, but it doesn't work

[matlabbe]

You can look at this example: https://github.com/introlab/rtabmap_ros/blob/c1d1c01d5d732784ceb6b225ebf6eebdb700ceb0/rtabmap_examples/launch/euroc_datasets.launch#L26-L32

Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml

To make sure if rtabmap did build with VINS, you can do rtabmap --version. If you see With VINS-Fusion: true, the parameters in the example above should work.

[youngjae41]

@matlabbe T

here is no problem with Vins and Rtabmap working normally.

The problem is that tf related errors occur.

[matlabbe]

Which tf errors?

[youngjae41]

@matlabbe I am testing the above algorithm on a real drone. I am currently using orangepi5, and at first it outputs 30hz odom and then gradually goes down. Are there options available for less powerful computers?

I also tried Vins fusion, but it takes a lot of resources

[matlabbe]

For VIO, you may try msckf_vio (see Table 7 of this paper), which is made to use less computation power (i.e., higher framerate on less powerful computer) and accuracy is still relatively good. If you want to use it inside rtabmap, you may have to follow this: https://github.com/introlab/rtabmap/blob/64f79813cd2c3a7e3f916a7ef2637e93f1f13a5f/docker/jfr2018/latest/Dockerfile#L140-L141 to build msckf in a catkin workspace, then do this:

cd rtabmap/build
cmake -DWITH_MSCKF_VIO=ON ..

[youngjae41]

@matlabbe Thanks matlabbe

Right now I'm trying to use the existing rtabmap slam and I'm testing it, but I've noticed something strange.

camera : d435i imu : /mavros/imu/data

I am using it as described above, and the tf of the base frame are all showing different directions.

my launch file

<arg name="rtabmap_viz"   default="false"/>
<arg name="ground_truth" default="false"/>

<arg     if="$(arg localization)" name="pre_args" default=""/>
<arg unless="$(arg localization)" name="pre_args" default="-d"/>

<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="standalone rtabmap_util/imu_to_tf">
  <remap from="imu/data" to="/mavros/imu/data"/>
  <param name="fixed_frame_id" value="base_link_stabilized"/>
  <param name="base_frame_id" value="base_link"/>
</node>

<!-- To connect rtabmap planning stuff with move_base below -->
<param name="/rtabmap/rtabmap/use_action_for_goal" value="true"/>
<remap from="/rtabmap/move_base" to="/move_base"/>

<!-- VSLAM -->
<param name="/rtabmap/rtabmap/latch" value="false"/> <!-- For some reason, if we latch grid_map, the global costmap inflation layer will create holes on robot path. To avoid hol>
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
  <arg name="localization"      value="$(arg localization)"/>
  <arg name="args"              value="$(arg pre_args)  --delete_db_on_start --Odom/Strategy 1 --Vis/MaxFeatures 600 --OdomF2M/MaxSize 1000 --Optimizer/GravitySigma 0.1 --Vis/Fe>
  <arg name="rtabmap_viz"        value="$(arg rtabmap_viz)" />
  <arg name="frame_id"          value="base_link" />
  <arg name="odom_guess_frame_id" value="base_link_stabilized" />
  <arg name="rgb_topic"         value="/camera/color/image_raw" />
  <arg name="depth_topic"       value="/camera/depth/image_rect_raw" />
  <arg name="camera_info_topic" value="/camera/color/camera_info" />
  <arg name="imu_topic"         value="/mavros/imu/data"/>
  <arg name="wait_imu_to_init"  value="true"/>
  <arg name="approx_sync"       value="true"/>

  <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/>
  <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
</include>

<!-- Costmap -->
  <node pkg="nodelet" type="nodelet" name="camera_points_xyz" args="standalone rtabmap_util/point_cloud_xyz">
  <remap from="depth/image"       to="/camera/depth/image_rect_raw"/>
  <remap from="depth/camera_info" to="/camera/depth/camera_info"/>
  <remap from="cloud"             to="camera_cloud" />

  <param name="decimation"  type="double" value="4"/>
  <param name="voxel_size"  type="double" value="0.1"/>
  <param name="approx_sync" type="bool"   value="true"/>
</node>

<!-- navigation -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
   <remap from="map" to="/rtabmap/grid_map"/>
   <remap from="odom" to="/rtabmap/odom"/>
   <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
   <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
   <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
   <rosparam file="$(find rtabmap_drone_example)/param/global_costmap_params.yaml" command="load" />
   <rosparam file="$(find rtabmap_drone_example)/param/local_costmap_params.yaml" command="load" />
   <rosparam file="$(find rtabmap_drone_example)/param/base_local_planner_params.yaml" command="load" />
</node>

Can you tell me what's wrong? thank you

[youngjae41]

@matlabbe

my tf tree

[matlabbe]

What is the frame_id in /mavros/imu/data? If different than base_link, the 180 deg may come from base_link to imu_link, see imu_to_tf code here: https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_util/src/nodelets/imu_to_tf.cpp

You may also check the quaternion inside /mavros/imu/data

[youngjae41]

@matlabbe thanks matlabbe I am using the px4.config file provided by rtabmap_drone_example and imu is written as base_link

And as recommended by matlabbe, I am using it after changing it to MSCKF_VIO. When base_link TF rotates on the map, base_link_stabilized TF is fixed instead of rotating along with it.

sorry. I checked again with matlabbe and found that base_link_stabilized was in a fixed state in the simulation.

1. I have another question. --Grid/MaxGroundHeight doesn't seem to work when using msckf_vio. The aircraft is flying over 1m, but obstacles appearing below 1m are displayed.

2. Generally R_imu_cam0 t_imu_cam0 Write a config file in VIO such that the conversion of camera and imu is R_imu_cam0 0, 0, -1; 1, 0, 0]

etc. are generated automatically but they are not accurate, so I think it will affect the performance on VIO. What do you think of matlabbe?

[matlabbe]

1. Grid/MaxGroundHeight is based on map frame (Grid/MapFrameProjection), assuming the drone is taking off from the floor. Did you compare the odometry values between both visual odometry approaches?
2. Well, for how much it will affect VIO you would need to ask on MSCKF repo.

[youngjae41]

@matlabbe

When testing all three of orb-slam , vins and msckf_vio , msckf_vio performed the best as recommended.

As with vins, I thought it would be a good way to achieve a more accurate vio to set each cam, imu, gyro, and acc using a config file.

Due to lack of CPU performance, Z is using altitude lidar. Is there any way to integrate a separate lidar sensor?

Thank you for your help. When the code is complete, we will publish the code with a thank you note

[matlabbe]

Is there any way to integrate a separate lidar sensor?

Not sure where you want to integrate it. rtabmap node can subscribe to a lidar or Pointcloud2 topic.

[youngjae41]

@matlabbe thanks