Information Matrices - Odometry fusion

[german]

Hi Mathieu!

I am trying to run rtabmap fusing wheel odometry, IMU and visual odometry using the robot_localization package. In this experiment I leave the robot static, and let rtabmap run. Rtabmap is crashing with the following message:

[FATAL] (2017-11-21 19:16:36.867) OptimizerG2O.cpp:482::optimize() Condition (optimizer.verifyInformationMatrices()) not met!
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2017-11-21 19:16:36.867) OptimizerG2O.cpp:482::optimize() Condition (optimizer.verifyInformationMatrices()) not met!

The filtered odometry messages look like this:

---
header:
  seq: 275
  stamp:
    secs: 1511290429
    nsecs: 403695107
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: -0.0014129343564
      y: -0.00527140735641
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.00333560307984
      w: 0.999994436861
  covariance: [0.00825685013891545, -4.525089183241261e-09, 0.0, 0.0, 0.0, 9.446447303384692e-12, -4.525089183241261e-09, 0.00825752839888843, 0.0, 0.0, 0.0, -4.588860241236351e-12, 0.0, 0.0, 4.39221239320484e-07, -3.618116429136773e-19, 1.5771560133333614e-20, 0.0, 0.0, 0.0, -3.6181164291367726e-19, 4.021614392931562e-07, 1.793347207199633e-31, 0.0, 0.0, 0.0, 1.577156013333361e-20, 1.8418374172304484e-31, 4.021614392931562e-07, 0.0, 9.446447303384689e-12, -4.588860241236351e-12, 0.0, 0.0, 0.0, 0.00026489410619986954]
twist:
  twist:
    linear:
      x: -4.87059989449e-09
      y: -6.28886795539e-08
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -3.85080132791e-17
  covariance: [1.2990264307452079e-06, 2.4602445549483044e-24, 0.0, 0.0, 0.0, 1.0147866430969203e-23, 2.4602445822386573e-24, 9.493234125589882e-06, 0.0, 0.0, 0.0, -1.6024072064554406e-22, 0.0, 0.0, 4.186921758942164e-07, -5.44638282867501e-27, 4.385556847799991e-28, 0.0, 0.0, 0.0, -5.44638282867501e-27, 3.348171067498431e-07, 2.6433251563728697e-33, 0.0, 0.0, 0.0, 4.385556847799991e-28, -2.643325156372856e-33, 3.348171067498431e-07, 0.0, 1.0147866430969216e-23, -1.602407206455441e-22, 0.0, 0.0, 0.0, 0.0001856083848748251]
---

This is the launch file I'm using:

fusion.launch

Do you have an idea where should I look at? I guess the problem has to do with the filtered odometry, since if I use only visual odometry everything works fine. Does the filtered odometry covariance look reasonable to you? Is there a way to debug what the information matrices of the links look like? (that doesn't involve adding printouts in the code :D )
Thanks a lot!

[matlabbe]

We would have to check what exactly g2o wants, but in general a covariance with values set on diagonal only should pass the test. The information matrix is pretty much the inverse of the odometry's covariance you are seeing.

cheers,
Mathieu

[german]

Thanks for your reply! I just made a test with TORO and it didn't complain.