g2o error on loop closure

[kemfic1]

I'm trying to run the sensor_fusion example in rtabmap, but I keep getting this error upon loop closure:


[FATAL] (2018-06-27 09:49:35.288) OptimizerG2O.cpp:538::optimize() Condition (optimizer.verifyInformationMatrices()) not met!

I've tried ros-kinetic-libg2o, and building rtabmap, rtabmap_ros, and g2o from scratch, but I keep getting the error. How can I fix this?

Thanks,
Kemal

[matlabbe]

Hi Kemal,

Can you output the odometry message that is used as input to rtabmap?

$ rostopic echo /my_odom_msg

The error is caused by bad covariance set in odometry messages.

cheers,
Mathieu

[joshcorn]

Hi Mathieu and Kemal,
I am having the same issue. If I map a space using G2O or even using TORO but then try to replay the database in rtabmap with G2O, I get an error at loop closure. Here is an example of my odometry message if it helps to debug:

header:
  seq: 5025
  stamp:
    secs: 1530616088
    nsecs: 912664413
  frame_id: "odom"
child_frame_id: "base_link"
pose:
  pose:
    position:
      x: -3.74060212997
      y: -3.46450097923
      z: 5.08594993175e-41
    orientation:
      x: -8.06924906635e-34
      y: 5.88928756199e-34
      z: -0.20862903246
      w: 0.977994850096
  covariance: [12.686810358243818, -1.3968613001501316e-06, 7.668319755579451e-43, -6.5808757363258135e-47, 9.31472426219147e-47, -1.7100305706904937e-14, -1.3968613001503e-06, 12.686808804312687, 6.868672508437312e-43, -5.615928846216479e-47, -2.203720934045757e-47, -9.06107774075948e-16, 7.668319755880418e-43, 6.868672508453273e-43, 9.983904260409865e-07, 3.9230978399995335e-17, 2.0090168903201882e-17, -5.078303782800176e-45, -3.1128810526816167e-47, -5.432052783009098e-47, 3.923097839999533e-17, 9.96788590037476e-07, -4.896701411726453e-25, 9.690892032796565e-37, 7.764795840279529e-47, -2.285899473087892e-47, 2.0090168903201888e-17, -4.896700744084517e-25, 9.967885900374757e-07, -4.321171516054147e-37, -1.7100305706904937e-14, -9.06107774075948e-16, 1.2929379301643271e-44, -9.690909294489868e-37, 4.341237177896264e-37, 0.0006197844144668082]
twist:
  twist:
    linear:
      x: -0.000427685481123
      y: 0.000835242621314
      z: -6.30205947146e-46
    angular:
      x: -5.80673370376e-31
      y: -3.014060155e-44
      z: -0.00421867148938
  covariance: [0.0005067836282552057, 1.8237883693895204e-26, -1.0949524850939264e-46, 7.237649690742052e-54, 9.001358964848324e-54, 4.746299381057628e-75, 5.303690085370432e-26, 0.0005067836282552057, -2.4500233396079856e-46, 1.6194679286875402e-53, 2.014108554759372e-53, 1.0620131695632636e-74, -1.094952485093926e-46, -2.450023339607984e-46, 9.975885452089326e-07, 1.6493334990319415e-26, 2.0512512855579715e-26, 1.0823262121141407e-47, 7.237649690742052e-54, 1.61946792868754e-53, 1.6493334990319426e-26, 9.904572080410505e-07, 4.087063625809987e-28, 2.9699838788951874e-32, 9.001358964848325e-54, 2.0141085547593725e-53, 2.0512512855579726e-26, 4.087058720643747e-28, 9.904572080410505e-07, -3.450481650127278e-49, -4.746299383035191e-75, -1.0620131700057561e-74, -1.0823262125743454e-47, -2.969983878895204e-32, 1.5829175702708016e-48, 0.00020671608637670805]
 
Thanks for the help!

[joshcorn]

I realized I had some IMU values that were being fused in robot_localization which I didn't need. This odometry output is still crashing G2O for me:
header:
  seq: 716
  stamp:
    secs: 1530631932
    nsecs:  68974733
  frame_id: "odom"
child_frame_id: "base_link"
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.000785427135805
      w: 0.999999691552
  covariance: [4030.6397772351525, 4.4236634483534737e-16, 0.0, 0.0, 0.0, 0.0, -4.493052387392546e-16, 4030.6397772351534, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.998698597376548e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.997398549128209e-07, -1.913326252639207e-34, 0.0, 0.0, 0.0, 0.0, 1.913326252639207e-34, 4.997398549128209e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.994797095830563e-07]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.00111235205013
  covariance: [46.219113711096966, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 46.219113711096966, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.998048404120562e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.992211834837847e-07, 1.6807491848305652e-37, 0.0, 0.0, 0.0, 0.0, -1.6807491848305652e-37, 4.992211834837847e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.2182377884746984e-07]

[matlabbe]

See the g2o::OptimizableGraph::verifyInformationMatrices() method: https://github.com/RainerKuemmerle/g2o/blob/8564e1e365d4e719dceca6269a0ba203f7f43dec/g2o/core/optimizable_graph.cpp#L981-L1013

There are some conditions that the matrices should have to be valid. You may look if your covariance matrices respect those conditions. The information matrix is the inverse of the covariance. In rtabmap, the covariance is taken from twist covariance of odometry messages.

A covariance of 46.219113711096966 is quite huge. In general, it should be in order of 10-3 to 10-6 for the linear part. Also, note that non-null diagonal-only covariance matrices should work with g2o.

cheers,
Mathieu

[joshcorn]

Thanks Mathieu, I'll look into my odometry a bit closer to figure it out!

[asabet]

How can we reduce the odometry's covariance in this case?

[matlabbe]

By careful tuning of robot_localization, see their documentation.

On rtabmap side, we can manually fix a covariance if fixing on robot_localization side is not possible. Set odom_frame_id of rtabmap node and set also odom_tf_angular_variance and odom_tf_linear_variance parameters.

cheers,
Mathieu

[asabet]

Thank you for the reference and suggestions. Set odom_frame_id to what however?

[matlabbe]

See http://wiki.ros.org/rtabmap_ros, this is the name of the odometry frame (e.g., "odom").