Angular information roll should not be null

[flabrosse]

I have been playing for a while with rtabmap using a simulated robot and environment (in gazebo using the kinect plugin) and am now moving onto the real robot. When I start rtabmap I get, shortly after starting:

[FATAL] (2021-07-13 16:52:52.576) Link.cpp:139::setInfMatrix() Condition (uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0) not met! [Angular information roll should not be null! Value=inf (set to 1 if unknown or <=1/9999 to be ignored in some 
computations).] 

shortly followed by

[rtabmap/rtabmap-2] process has died [pid 6848, exit code -6, cmd /opt/ros/noetic/lib/rtabmap_ros/rtabmap --delete_db_on_start rgb/image:=/tryfan/zed/rgb/image_rect_color depth/image:=/tryfan/zed/depth/depth_registered rgb/camera_info:=/tryfan/zed/rgb/camera_info odom:=/tryfan/odom scan:=/tryfan/front_laser/scan rgbd_image:=rgbd_image gps/fix:=/tryfan/gps/fix imu:=/tryfan/imu/data grid_map:=/map __name:=rtabmap __log:=/home/ffl/.ros/log/6dd8e7e4-e3e9-11eb-8016-d7840c42cc3d/rtabmap-rtabmap-2.log].computations).] 

The mentioned log file does not exist.

rtabmap is still running after that (rtabmapviz still shows a pointcloud being constructed) but there are no loop closures being detected (I think not even attempts at detecting any, so I think this is what crashed).

Anybody can tell me what is wrong? I can provide info if needed.

Cheers,

Fred

[matlabbe]

Hi,

It feels like your odometry topic has "inf" values in it. rtabmap asserts the error and exit, so no map is created anymore.

You can do "rostopic echo /your/odom/topic" to see the covariance values. Either modify the node publishing the odometry with 9999 instead of inf, or make a middle node to convert and republish the covariance with 9999 instead of the inf values.

EDIT: In this new commit, I check if there are inf values in z,roll and pitch if Reg/Force3DoF is enabled, then change them to be compatible down the line for convenience. Solution above should still work without recompiling rtabmap.

cheers,
Mathieu

[flabrosse]

Hi Mathieu,

Thanks for the answer.  I checked and none of the values in my odom messages are inf (or Inf or any combination).  Here is a typical example while the robot is static and where I get the problem:

header: 
  seq: 37243
  stamp: 
    secs: 1628780962
    nsecs: 695691021
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 37.39194540035548
      y: 29.11935107467684
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.8336782641320923
      w: 0.5522504431086509
  covariance: [0.005, 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.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.005, 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.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.08]

Maybe my covariances are not correct?

Fred

[matlabbe]

Hi, if Reg/Force3DoF is false, those covariances would indeed create an assert. The covariance should be invertible, so put 1 on the diagonal of the matrix where values are 0:

covariance: [0.005, 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.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]

[flabrosse]

Ah, this fixed it.  Thank you!