Extreme Odometry Drift - Githubissues

zippyzoo77 commented 2 months ago

Description: When I run any kimera nodes, the odometry that comes out is way off of what it should be. It drifts a huge amount, enough that I almost suspect it is putting out positions in mm instead of m. Here is a link to a video of the rviz window when I run the euroc example from the main page. I haven't changed any of the config files. I changed the 'use_lcd' parameter to true and added the rviz node to the launch file (rather than in a separate window), and that is it.

Command:

roslaunch kimera_vio_ros kimera_vio_ros_euroc.launch

second terminal:
rosbag play --clock V1_01_easy.bag

Console output: (only the latest three instances, but these are put out with similar data throughout)


-----------                                  #  Log Hz  {avg     +- std    }    [min,max]
Display [ms]                                 1353   9.97554 {12.1212 +- 12.4769}    [0,50]
Lcd [ms]                                      677   4.99150 {55.7563 +- 29.6810}    [10,133]
Mesher [ms]                                   677   4.99269 {4.05022 +- 0.894653}   [1,8]
PGO Update/Optimization Timing [ms]           260   2.19986 {4.15000 +- 1.15798}    [0,10]
Stereo Data Provider [ms]                       0   
VioBackend [ms]                               677   4.99282 {3.45052 +- 0.672324}   [0,99]
VioFrontend Frame Rate [ms]                  2061   15.0663 {1.19893 +- 0.544857}   [1,5]
VioFrontend Keyframe Rate [ms]                676   4.99316 {32.8047 +- 2.76740}    [23,43]
VioFrontend [ms]                             2738   19.9851 {9.03397 +- 13.9105}    [1,43]
Visualizer [ms]                               677   4.99146 {0.423929 +- 0.238683}  [0,39]
backend_input_queue Size [#]                  677   4.98824 {1.00000 +- 0.00000}    [1,1]
data_provider_left_frame_queue Size [#]      2740   19.9823 {1.00000 +- 0.00000}    [1,1]
data_provider_right_frame_queue Size [#]     2740   19.9823 {1.00109 +- 0.00000}    [1,3]
display_input_queue Size [#]                 1353   9.96612 {1.00000 +- 0.00000}    [1,1]
frontend_input_queue Size [#]                2738   19.9668 {1.00000 +- 0.00000}    [1,1]
lcd_backend_queue Size [#]                    677   4.99208 {1.00000 +- 0.00000}    [1,1]
lcd_frontend_queue Size [#]                  2738   19.9822 {2.64500 +- 7.29536}    [1,29]
mesher_backend Size [#]                       677   4.98838 {1.00000 +- 0.00000}    [1,1]
mesher_frontend Size [#]                     2738   19.9675 {2.64354 +- 7.29536}    [1,29]
visualizer_backend_queue Size [#]             677   4.99272 {1.00000 +- 0.00000}    [1,1]
visualizer_frontend_queue Size [#]           2738   19.9844 {2.64500 +- 7.29536}    [1,29]
visualizer_lcd_queue Size [#]                 677   4.99149 {1.00000 +- 0.00000}    [1,1]
visualizer_mesher_queue Size [#]              677   4.99268 {1.00000 +- 0.00000}    [1,1]
I0429 17:21:04.650614 23695 KimeraVioRos.cpp:215] Statistics
-----------                                  #  Log Hz  {avg     +- std    }    [min,max]
Display [ms]                                 1353   9.97554 {12.1212 +- 12.4769}    [0,50]
Lcd [ms]                                      677   4.99150 {55.7563 +- 29.6810}    [10,133]
Mesher [ms]                                   677   4.99269 {4.05022 +- 0.894653}   [1,8]
PGO Update/Optimization Timing [ms]           260   2.19986 {4.15000 +- 1.15798}    [0,10]
Stereo Data Provider [ms]                       0   
VioBackend [ms]                               677   4.99282 {3.45052 +- 0.672324}   [0,99]
VioFrontend Frame Rate [ms]                  2061   15.0663 {1.19893 +- 0.544857}   [1,5]
VioFrontend Keyframe Rate [ms]                676   4.99316 {32.8047 +- 2.76740}    [23,43]
VioFrontend [ms]                             2738   19.9851 {9.03397 +- 13.9105}    [1,43]
Visualizer [ms]                               677   4.99146 {0.423929 +- 0.238683}  [0,39]
backend_input_queue Size [#]                  677   4.98824 {1.00000 +- 0.00000}    [1,1]
data_provider_left_frame_queue Size [#]      2740   19.9823 {1.00000 +- 0.00000}    [1,1]
data_provider_right_frame_queue Size [#]     2740   19.9823 {1.00109 +- 0.00000}    [1,3]
display_input_queue Size [#]                 1353   9.96612 {1.00000 +- 0.00000}    [1,1]
frontend_input_queue Size [#]                2738   19.9668 {1.00000 +- 0.00000}    [1,1]
lcd_backend_queue Size [#]                    677   4.99208 {1.00000 +- 0.00000}    [1,1]
lcd_frontend_queue Size [#]                  2738   19.9822 {2.64500 +- 7.29536}    [1,29]
mesher_backend Size [#]                       677   4.98838 {1.00000 +- 0.00000}    [1,1]
mesher_frontend Size [#]                     2738   19.9675 {2.64354 +- 7.29536}    [1,29]
visualizer_backend_queue Size [#]             677   4.99272 {1.00000 +- 0.00000}    [1,1]
visualizer_frontend_queue Size [#]           2738   19.9844 {2.64500 +- 7.29536}    [1,29]
visualizer_lcd_queue Size [#]                 677   4.99149 {1.00000 +- 0.00000}    [1,1]
visualizer_mesher_queue Size [#]              677   4.99268 {1.00000 +- 0.00000}    [1,1]
^C[rvizself-4] killing on exit
[kimera_vio_ros/posegraph_viewer-3] killing on exit
[kimera_vio_ros/kimera_vio_ros_node-2] killing on exit
I0429 17:21:05.650602 23695 KimeraVioRos.cpp:215] Statistics
-----------                                  #  Log Hz  {avg     +- std    }    [min,max]
Display [ms]                                 1353   9.97554 {12.1212 +- 12.4769}    [0,50]
Lcd [ms]                                      677   4.99150 {55.7563 +- 29.6810}    [10,133]
Mesher [ms]                                   677   4.99269 {4.05022 +- 0.894653}   [1,8]
PGO Update/Optimization Timing [ms]           260   2.19986 {4.15000 +- 1.15798}    [0,10]
Stereo Data Provider [ms]                       0   
VioBackend [ms]                               677   4.99282 {3.45052 +- 0.672324}   [0,99]
VioFrontend Frame Rate [ms]                  2061   15.0663 {1.19893 +- 0.544857}   [1,5]
VioFrontend Keyframe Rate [ms]                676   4.99316 {32.8047 +- 2.76740}    [23,43]
VioFrontend [ms]                             2738   19.9851 {9.03397 +- 13.9105}    [1,43]
Visualizer [ms]                               677   4.99146 {0.423929 +- 0.238683}  [0,39]
backend_input_queue Size [#]                  677   4.98824 {1.00000 +- 0.00000}    [1,1]
data_provider_left_frame_queue Size [#]      2740   19.9823 {1.00000 +- 0.00000}    [1,1]
data_provider_right_frame_queue Size [#]     2740   19.9823 {1.00109 +- 0.00000}    [1,3]
display_input_queue Size [#]                 1353   9.96612 {1.00000 +- 0.00000}    [1,1]
frontend_input_queue Size [#]                2738   19.9668 {1.00000 +- 0.00000}    [1,1]
lcd_backend_queue Size [#]                    677   4.99208 {1.00000 +- 0.00000}    [1,1]
lcd_frontend_queue Size [#]                  2738   19.9822 {2.64500 +- 7.29536}    [1,29]
mesher_backend Size [#]                       677   4.98838 {1.00000 +- 0.00000}    [1,1]
mesher_frontend Size [#]                     2738   19.9675 {2.64354 +- 7.29536}    [1,29]
visualizer_backend_queue Size [#]             677   4.99272 {1.00000 +- 0.00000}    [1,1]
visualizer_frontend_queue Size [#]           2738   19.9844 {2.64500 +- 7.29536}    [1,29]
visualizer_lcd_queue Size [#]                 677   4.99149 {1.00000 +- 0.00000}    [1,1]
visualizer_mesher_queue Size [#]              677   4.99268 {1.00000 +- 0.00000}    [1,1]

with rostopic echo /kimera_vio_ros/odometry after about 60s of the euroc rosbag:

header: 
  seq: 252
  stamp: 
    secs: 1403715332
    nsecs: 412143000
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 1307.0291992928474
      y: -2.300996351900729
      z: 341.19589624784726
    orientation: 
      x: 0.7091589371389581
      y: 0.3898710327851008
      z: 0.5013452691822693
      w: -0.30618148333872347
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 19.03902073562167
      y: 38.23343385280038
      z: 7.482924191340842
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
header: 
  seq: 253
  stamp: 
    secs: 1403715332
    nsecs: 612143000
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 1315.0511493014933
      y: -4.021416470102114
      z: 344.02561924363806
    orientation: 
      x: 0.6798732315460381
      y: 0.44362727719272554
      z: 0.48031604424765784
      w: -0.3320598223150586
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 16.8158001194226
      y: 39.96577363266999
      z: 2.308057709206576
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
header: 
  seq: 254
  stamp: 
    secs: 1403715332
    nsecs: 812143000
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 1323.0764862993126
      y: -5.747677163589862
      z: 346.87398143841426
    orientation: 
      x: 0.6408592459495908
      y: 0.4918358400056318
      z: 0.46627929096335513
      w: -0.36052816281878985
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 14.327384852386913
      y: 40.94736167403652
      z: -2.5711977618058537
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Additional files: Please attach all the files needed to reproduce the error.

Please give also the following information:

KimeraVIO branch, tag or commit used noetic branch
GTSAM version used: 3.2.12 (I think?)
OpenGV version used: 4.2.0 (I think?)
OpenCV version used: 4.2.0
Operating system and version (e.g. Ubuntu 16.04 or Windows 10): Ubuntu 20.04
Did you change the source code? (yes / no): yes- I have changed the verbosity level of several warnings in attempts to debug, but no changes to anything other than those (so nothing that should affect functionality, only what it prints to the screen)

zippyzoo77 commented 5 days ago

Just an update, I'm still seeing this issue. I recently did a fresh install from the OS up on a new computer, and I also see this issue on my new computer.

zippyzoo77 commented 5 days ago

Ok, another update, I ran just the kimera-VIO example (without the ros part) on MH-01-easy data, and on the same data in the ros version, and compared the output odometry. This is what I got: The kimera-ros output: traj_vio_ros.csv The kimera only output: traj_vio.csv The gt data logged with kimera only: traj_gt.csv

From that, I can see that the kimera only part seems to be working fine, it doesn't deviate much from the gt data, but the ros data is really far off, so wherever the problem is, I think it's somewhere in the ros stuff. Also noticed that the data runs much, much faster in the kimera only version, not sure if that's a difference between the csv v. bag files, or delays from the ros wrapper stuff. Also the mesh was much denser.

MIT-SPARK / Kimera-VIO-ROS

Extreme Odometry Drift #204