Fusion on zed and imu, but the map drifted immediately

[Yuli]

Dear Mathieu,
I have been trying Fusion on zed and imu, but the map drifted immediately,wonder if you can
give me some suggestions?Thank you!


tf between imu and camera should be right

 


Here is the tf tree

[Yuli]

CONTENTS DELETED

The author has deleted this message.

[Yuli]

Launch file
<launch> 


  <arg name="frame_id"                default="zed_center" />
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth/depth_registered" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="imu_topic"               default="/imu/data" />
  <arg name="imu_ignore_acc"          default="true" />
  <arg name="imu_remove_gravitational_acceleration" default="false" />
  <arg name="rtabmap_args"  default="--delete_db_on_start --Vis/EstimationType 1 --Kp/FeatureType 6 --Odom/ResetCountdown 2"/> 
   

      
  <arg name="stereo"          default="false"/>
 
 
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
 
 
  <arg name="localization"            default="false"/>
 
 
  <arg name="use_sim_time"            default="false"/>
 
 
 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
 
   
  <arg name="odom_frame_id"           default=""/>               
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     
  <arg name="ground_truth_base_frame_id" default=""/> 
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>             
 
  <arg name="launch_prefix"           default=""/>             
  <arg name="output"                  default="screen"/>       

 
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
   
 
   
   
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
 
 
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />     
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
 
 
  <arg name="rgbd_sync"               default="false"/>         
  <arg name="approx_rgbd_sync"        default="true"/>         
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />
 
  <arg name="compressed"              default="false"/>         
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="depth_image_transport"   default="compressedDepth"/> 
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="scan_normal_k"           default="0"/>
   
  <arg name="visual_odometry"          default="true"/>         
  <arg name="icp_odometry"             default="false"/>         
  <arg name="odom_topic"               default="odom"/>         
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> 
  <arg name="odom_tf_angular_variance" default="1"/>             
  <arg name="odom_tf_linear_variance"  default="1"/>             
  <arg name="odom_args"                default=""/>             
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/> 
 
  <arg name="subscribe_user_data"      default="false"/>             
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> 
 
 
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>



 <group ns="camera">
     <include file="$(find zed_wrapper)/launch/zed_camera.launch"/>
 </group>


 <node pkg="imu" type="imu_mpu6050" name="imu_mpu6050" required="true" output="screen">
       
       
       
       
       
       
       
       
 </node>
 
  <group ns="rtabmap">
   
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/vo"/>

     
     
     
    </node>


   
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
     
 
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/odometry/filtered"/>     
    </node>


   
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
     
     
     
     
   
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap     if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
      <remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="/odometry/filtered"/>
    </node>

  </group>

 
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

     
     
     
     
     
     
     
     
       
     
     


     
      <rosparam param="odom0_config">[true, true, true,
                                      false, false, false,
                                      true, true, true,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>
      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true] </rosparam> 
     
     
     

     
     

     

     

     
     
       

     
      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>

     
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

</node>
 





</launch>

[Yuli]

[Yuli]

if I use camera odom directly, map looks better,at least not drifted for a whille

and rqt_graph for this is

[matlabbe]

When using /camera/odom, you don't need to use visual_odometry node. rtabmap should be subscribed to /odometry/flltered if you want to use odometry output from IMU and /camera/odometry fusion, otherwise IMU is ignored if rtabmap is subscribed directly to /camera/odom.

To get a good fusion between IMU and vo, the transform between the sensors should be very precise. You can use kalibr tool to get this information and get more precise TF.

cheers,
Mathieu

[Yuli]

Dear Mathieu
The loop in blue is /vo , which is the real trajectory of zed, but the
odometry/filtered  goes straight all the way down and the map is
built along it. I still couldn't figure out why. Do you have any idea?
Thank you.

[Yuli]

plus, I took away these three lines in your example,
<node pkg="tf" type="static_transform_publisher" name="camera_imu" args=" -0.025 0.0 0.015 0 0 0  $(arg frame_id) imu_link 100" />
<node pkg="tf" type="static_transform_publisher" name="camera_zed" args=" 0 0 0 -1.5707963267948966 0 -1.5707963267948966  $(arg frame_id) zed_initial_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="zed_left" args=" 0 0 0 0 0 0 zed_current_frame ZED_left_camera 100" />

[matlabbe]

Hi,

The drift of /odometry/filtered is more an issue with robot_localization as it is not rtabmap related. I recommend to read their documentation more in depth, or ask the question on ROS answers about Zed's odometry and IMU fusion.

cheers,
Mathieu

[Yuli]

Dear Mathieu,
Indeed.
I set imu0_differential to true instead of odom0_differential, then
/odometry/filtered turned back to a loop again. But while I tried to
load db, the below message showed, and the map still didn't look no drifted.
 


















Thank you for your advice. I am trying to go deeper in EKF localization.

[matlabbe]

Hi,

Can you print out some messages from /odometry/filtered topic? The covariances seem not supported by g2o.

A workaround is to manually set a fixed covariance for odometry coming from robot_localization. This can be done by making rtabmap using TF to get odometry instead of the topic. This can be done by setting parameter odom_frame_id to odometry frame. Then to set covariance, set also parameters odom_tf_angular_variance (e.g., 0.005) and odom_tf_linear_variance (e.g., 0.0005).

cheers,
Mathieu

[Yuli]

Dear Mathieu,
Thank you.
Here you go.

header:
  seq: 4959
  stamp:
    secs: 1527821086
    nsecs: 339719534
  frame_id: "odom"
child_frame_id: "zed_center"
pose:
  pose:
    position:
      x: 4.97042465117
      y: 0.855350399345
      z: -0.0744909809759
    orientation:
      x: -0.380878367851
      y: 0.0440517921946
      z: -0.011168207463
      w: 0.923507650022
  covariance: [0.0034271044317294368, 5.518612152658068e-05, 2.6141804319629494e-05, -5.8460089094787045e-06, -7.825591159745411e-05, 0.00017840402552667665, 5.518612152658066e-05, 0.004505493033166105, 1.0658438212167947e-05, -0.00011608893678193164, -1.8895202121137824e-05, 0.0034349893641131986, 2.61418043196295e-05, 1.0658438212167962e-05, 0.004493493478350276, -0.0002705351427027966, -0.002429719232669587, -1.3668112542489383e-05, -5.846008909478827e-06, -0.00011608893678193335, -0.00027053514270279703, 0.0527767518006934, 0.0010294351221327093, 0.003019548540801398, -7.825591159745425e-05, -1.889520212113782e-05, -0.00242971923266959, 0.0010294351221327078, 0.008611947244348949, 3.044246684484786e-05, 0.00017840402552667684, 0.003434989364113199, -1.3668112542489476e-05, 0.0030195485408014023, 3.044246684484803e-05, 0.012613802557143715]
twist:
  twist:
    linear:
      x: 0.611961073777
      y: -0.03471843838
      z: 0.0214183857814
    angular:
      x: -0.0265472898676
      y: -0.0293380879058
      z: 0.072377605212
  covariance: [0.0021073319131095117, 1.5303781860314442e-07, -5.264338214094726e-07, 7.156446360635717e-45, -4.3817328266607114e-44, -1.2933614840104644e-45, 1.5303781860314484e-07, 0.0021190231949057697, 7.1484097284714e-07, 1.3261100959584828e-43, -2.1944340678282925e-44, -1.680190090039377e-44, -5.264338214094731e-07, 7.148409728471386e-07, 0.002637220022658556, 8.608264099909615e-44, 1.2237308038975466e-42, 3.203542856536319e-46, 7.156446360635715e-45, 1.3261100959584832e-43, 8.608264099909616e-44, 4.999750185555667e-10, -8.105446970990487e-80, 2.1724101968760056e-81, -4.3817328266607094e-44, -2.194434067828293e-44, 1.2237308038975473e-42, -8.105446970990484e-80, 4.999750185555667e-10, -1.1984512052022033e-82, -1.2933614840104632e-45, -1.6801900900393755e-44, 3.203542856536459e-46, 2.1724101968760056e-81, -1.1984512052022641e-82, 4.999875086537033e-10]
---

After I tried the workaround, the message became
header:
  seq: 1362
  stamp:
    secs: 1527821013
    nsecs: 364721537
  frame_id: "odom"
child_frame_id: "zed_center"
pose:
  pose:
    position:
      x: 14.5868603797
      y: -0.497043708796
      z: 0.854988310526
    orientation:
      x: 0.100525568978
      y: -0.00474168177941
      z: -0.0235284301349
      w: 0.994644931325
  covariance: [0.007256748586234843, 0.00025371730994290287, -0.00018277083685829172, 4.724127443343903e-05, 0.00045191309890259507, 0.0006253471440057287, 0.00025371730994290265, 0.009383799173821821, 2.522734791465354e-05, -0.0011722894724471408, -2.4642980288928135e-05, 0.00533864621983907, -0.00018277083685829155, 2.5227347914653393e-05, 0.009206397862292876, -0.00139270836679194, -0.00377776445219577, -8.732745023262492e-05, 4.724127443343955e-05, -0.0011722894724471404, -0.001392708366791944, 0.07815322678689272, -0.0011688521835528632, 0.004639832972567471, 0.0004519130989025951, -2.4642980288927935e-05, -0.003777764452195771, -0.0011688521835528742, 0.010583640946052356, -9.561085337631926e-05, 0.0006253471440057268, 0.005338646219839068, -8.73274502326252e-05, 0.0046398329725674675, -9.561085337631856e-05, 0.016095041468645044]
twist:
  twist:
    linear:
      x: 0.554848697331
      y: 0.00774478637123
      z: 0.0949887265746
    angular:
      x: 0.0393229633529
      y: -0.0777916040891
      z: 0.0758514775571
  covariance: [0.004169240304337178, 6.376480011342887e-06, -8.191856947482772e-06, 1.3324028156068525e-44, -6.236117901072805e-43, 3.358864555599465e-45, 6.376480011342863e-06, 0.004291264348773109, -2.1085156453339712e-06, 5.0410231280561655e-43, 4.755572822797831e-44, -5.187271630659658e-44, -8.191856947482757e-06, -2.1085156453339484e-06, 0.0050191069880976946, -2.744821221597513e-43, 3.7597818352266634e-42, -4.110183434787748e-46, 1.3324028156068615e-44, 5.041023128056161e-43, -2.7448212215975113e-43, 4.999750167699537e-10, 7.412474294715098e-80, 2.387318320723103e-81, -6.236117901072808e-43, 4.755572822797776e-44, 3.759781835226663e-42, 7.412474294715095e-80, 4.999750167699537e-10, 1.8712283399604235e-82, 3.3588645555994595e-45, -5.187271630659655e-44, -4.11018343478796e-46, 2.3873183207231042e-81, 1.8712283399604284e-82, 4.999875077608073e-10]
---

and the picture in odometry window became like oil painting







































plus, when I tried to load db, it showed

acid@acid:~$ rtabmap ~/.ros/rtabmap.db
0x3be0c60 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be28b0 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be2e00 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be32e0 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be36e0 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be3b20 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be3ea0 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be4320 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be4760 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3be0740 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0x25d5100) ): Attempt to set a screen on a child window.
0x3bdfee0


Ming

[matlabbe]

Hi Ming,

When using the workaround, this doesn't change odometry topic, it just makes rtabmap using TF to get odometry instead of the topic. The QWindowPrivate logs are Qt related stuff, maybe related to platform used and Qt version. The odometry view is showing the depth image over the RGB image, to see only rgb image, right-click and uncheck depth image.

For the covariance, they seem ok. I think the very small values are causing approximation problems, which may make that line fail. I updated rtabmap code in this commit with a new parameter ignoring by default the off diagonal values of the covariance matrix. This may also fix all other problems with g2o matrix verifications.

cheers,
Mathieu

[Yuli]

Dear Mathieu,
Now db can be loaded without any problems.
Thanks a lot!


Ming

[Yuli]

Dear Mathieu,
I compared trajectories produced from VO and VIO. It looks fusion did nothing special.
I don't have ground truth to verify whether VIO improves anything, though.

Plus, even trajectory from VIO looks "normal",
the map generated is pretty bad

This is before a loop closure is found

Here's what it looks when it was generated from VO




Did you have similar experience in this?Thanks!!

Ming

[matlabbe]

From my experience, VIO doesn't always mean better odometry, but helps a lot to have a continuous odometry estimation when environments don't have a lot of visual features or on some fast motions. Without ground truth, it is indeed difficult to get a really good comparison between both approaches.

The map before loop closure seems okay, is the first screenshot after a loop closure has been detected? Can you share the databases (VO and VIO)?

[Yuli]

Dear Mathieu,
Here you go. Thank you! What a big favor!
https://drive.google.com/file/d/1qkwFtjr3AYdOLg3IHuuSJ_OvAn_LFpBM/view?usp=sharing
https://drive.google.com/file/d/1GDfc5iCk64Iyj0KkeBbkCz1eYBJcWuHq/view?usp=sharing


Yes, the first screenshot is after a loop closure has been detected.
Btw, everytime trajectories generated from VO and VIO were not the same and
the comparison could be very different too.



Ming

[Yuli]

Dear Mathieu,
Is it a must that
we  feed odometry/filtered  to rtabmapviz?
I don't understand why it can effect 3D Map.
Does it mean rtabmapviz show map with respect to odom frame?
Thank you!






Ming

[Yuli]

Dear Mathieu,
I did fusion on ZED live+imu,
though the   map  drifted when I saw it in "live",
it didn't drift when I saw it by loading db in rtabmap standalone.
So does this count  as map is built correctly?

Thank you.



Ming

[matlabbe]

Hi,

the variance on angles looks underestimated. When exporting poses from VIO in g2o format:

EDGE_SE3:QUAT 4 5 0.631175 -0.040330 0.038267 0.005885 -0.008198 -0.014605 0.999842 300.796331 0.000000 0.000000 0.000000 0.000000 0.000000 298.789572 0.000000 0.000000 0.000000 0.000000 237.479907 0.000000 0.000000 0.000000 2000100042.797642 0.000000 0.000000 2000100042.797642 0.000000 2000050022.649828
EDGE_SE3:QUAT 4 93 -0.084220 -0.058437 -0.002275 -0.000696 0.000530 -0.026301 0.999654 36.602417 0.000000 0.000000 0.000000 0.000000 0.000000 36.602417 0.000000 0.000000 0.000000 0.000000 36.602417 0.000000 0.000000 0.000000 209.051583 0.000000 0.000000 209.051583 0.000000 209.051583

The first example is an odometry edge, covariance taken from odometry. The second example is the loop closure transformation computed by rtabmap. Note how the information values for the angles of the odometry are huge 2000100042.797642 (variance is thus very small 5x10^-10).

Are there parameters in robot_localization configuration that can be used to change the covariance error for angles? Otherwise, you may try the odom_frame_id workaround, by fixing variances like show in my previous post.

Visually, here is a comparison of the optimization between VO and VIO versions (using rtabmap-databaseViewer):

rtabmap_VO.db before and after graph optimization


rtabmap_VIO.db before and after graph optimization


Note how g2o cannot optimize the graph, as variance of angles on odometry links are a lot smaller than those in loop closure links.