Rtabmap failing frequently!

[asabet]

I'm testing rtabmap with a bumblebee2 stereo camera mounted on a clearpath jackal ugv in an indoor environment, with the stereo_img_proc and stereo_odometry nodes publishing to rtabmap. Rtabmap seems to be failing frequently with the following error:

[FATAL] (2018-08-02 13:10:38.767) OptimizerG2O.cpp:533::optimize() Condition (optimizer.verifyInformationMatrices()) not met!
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2018-08-02 13:10:38.767) OptimizerG2O.cpp:533::optimize() Condition (optimizer.verifyInformationMatrices()) not met!
[rtabmap-1] process has died [pid 9352, exit code -6, cmd /opt/ros/kinetic/lib/rtabmap_ros/rtabmap --delete_db_on_start left/image_rect:=/camera/left/image_rect_color right/image_rect:=/camera/right/image_rect left/camera_info:=/camera/left/camera_info right/camera_info:=/camera/right/camera_info rgbd_image:=/camera/rgbd_image odom:=/odometry/filtered __name:=rtabmap __log:=/home/administrator/.ros/log/4b4b82e8-967d-11e8-8a19-e1e488c6b69e/rtabmap-1.log].
log file: /home/administrator/.ros/log/4b4b82e8-967d-11e8-8a19-e1e488c6b69e/rtabmap-1*.log

I'm running Rtabmap with the following settings:

process[rtabmap-1]: started with pid [9352]
[ INFO] [1533233334.844499216]: Starting node...
[ INFO] [1533233334.875005704]: Initializing nodelet with 4 worker threads.
[ INFO] [1533233335.223490905]: /rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1533233335.223565852]: /rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1533233335.223636544]: /rtabmap(maps): map_cleanup                = true
[ INFO] [1533233335.223770065]: /rtabmap(maps): map_negative_poses_ignored = true
[ INFO] [1533233335.223868440]: /rtabmap(maps): map_negative_scan_ray_tracing = true
[ INFO] [1533233335.223944981]: /rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1533233335.224043998]: /rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1533233335.224146842]: /rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1533233335.227971519]: /rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1533233335.275938507]: rtabmap: frame_id      = base_link
[ INFO] [1533233335.276040754]: rtabmap: map_frame_id  = map
[ INFO] [1533233335.276123368]: rtabmap: tf_delay      = 0.050000
[ INFO] [1533233335.276197819]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1533233335.276266700]: rtabmap: odom_sensor_sync   = false
[ INFO] [1533233335.277333392]: rtabmap: stereo_to_depth = false
[ INFO] [1533233335.426698081]: Setting RTAB-Map parameter "Grid/DepthDecimation"="4"
[ INFO] [1533233335.429287494]: Setting RTAB-Map parameter "Grid/FlatObstacleDetected"="true"
[ INFO] [1533233335.606062877]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="6"
[ INFO] [1533233335.623989518]: Setting RTAB-Map parameter "Kp/MaxDepth"="0"
[ INFO] [1533233336.044132359]: Setting RTAB-Map parameter "RGBD/CreateOccupancyGrid"="true"
[ INFO] [1533233336.182978437]: Setting RTAB-Map parameter "Rtabmap/TimeThr"="700"
[ INFO] [1533233336.301921212]: Setting RTAB-Map parameter "Vis/EstimationType"="1"
[ INFO] [1533233336.325502177]: Setting RTAB-Map parameter "Vis/MaxDepth"="0"
[ INFO] [1533233337.064177998]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1533233337.082313342]: rtabmap: Deleted database "/home/administrator/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1533233337.082369911]: rtabmap: Using database from "/home/administrator/.ros/rtabmap.db" (0 MB).
[ INFO] [1533233337.464908838]: rtabmap: Database version = "0.17.1".
[ INFO] [1533233337.494774280]: /rtabmap: queue_size    = 30
[ INFO] [1533233337.494835657]: /rtabmap: rgbd_cameras = 1
[ INFO] [1533233337.494866145]: /rtabmap: approx_sync   = true
[ INFO] [1533233337.494947736]: Setup stereo callback
[ INFO] [1533233337.535102450]:

Stereo_img_proc:

<launch>
   <arg name="stereo_sync" default="true" />
   
   <group ns="/camera" >
      <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
         <remap from="left/image_raw"    to="/camera/reft/image_raw"/>
         <remap from="left/camera_info"  to="/camera/left/camera_info"/>
         <remap from="right/image_raw"   to="/camera/right/image_raw"/>
         <remap from="right/camera_info" to="/camera/right/camera_info"/>
         
      </node>
     
      <node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync">
      <remap from="left/image_rect"   to="/camera/left/image_rect_color"/>
      <remap from="right/image_rect"   to="/camera/right/image_rect"/>
      <remap from="left/camera_info"   to="/camera/left/camera_info"/>
      <remap from="right/camera_info"   to="/camera/right/camera_info"/>
    </node>
   </group>
</launch>

and stereo_odometry:

<launch>
   
   <arg name="rviz" default="true" />
   <arg name="rtabmapviz" default="false" />
   <arg name="local_bundle" default="true" />
   <arg name="stereo_sync" default="false" />
   
   
      
   <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
      <remap from="left/image_rect"       to="/camera/left/image_rect"/>
      <remap from="right/image_rect"      to="/camera/right/image_rect"/>
      <remap from="left/camera_info"      to="/camera/left/camera_info"/>
      <remap from="right/camera_info"     to="/camera/right/camera_info"/>
      <remap from="rgbd_image"            to="/rgbd_image"/>
      <remap from="odom"                  to="/odometry/filtered"/>

     
     
     

       
       
     
     
     
     
       
     
       
     

     
     
   </node>
</launch>

If the issue's with the robot's odometry covariance, what parameters/files do I modify to fix this?

[matlabbe]

Hi,

is /odometry/filtered already published by jackal? You remap odometry output of stereo_odometry to /odometry/filtered, you may have 2 nodes publishing under the same name. Normally, odometry covariance coming from rtabmap_ros/stereo_odometry should be already compatible with g2o. The only thing I see is that rtabmap is receiving both /odometry/filtered from jackal and stereo_odometry, where covariance from jackal's odometry may not be compatible with g2o. Can you remove the remap of stereo_odometry? and set rtabmap to subscribe to odometry from stereo_odometry alone?

For TF, can you show your TF tree when everything is started?

$ rosrun tf view_frames
$ evince frames.pdf

cheers,
Mathieu