search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1.01k stars 558 forks source link

FATAL error occurs.. related to covariance #779

Closed jaykorea closed 2 years ago

jaykorea commented 2 years ago

Hello.

I failed to apply realsense t265 to my robot cause of drifting data.

So I changed the sensor to D455(Imu inside) with wheel odometry

As I run not localization mode, everything works fine, however in localization mode, Fatal error occurs seeing below.

[FATAL] (2022-07-17 22:20:18.216) OptimizerG2O.cpp:958::optimize() Condition (optimizer.verifyInformationMatrices(true)) not met! [This error can be caused by (1) bad covariance matrix set in odometry messages (see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) or that (2) PCL and g2o hadn't been built both with or without "-march=native" compilation flag (if one library is built with this flag and not the other, this is causing Eigen to not work properly, resulting in segmentation faults).]
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2022-07-17 22:20:18.216) OptimizerG2O.cpp:958::optimize() Condition (optimizer.verifyInformationMatrices(true)) not met! [This error can be caused by (1) bad covariance matrix set in odometry messages (see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) or that (2) PCL and g2o hadn't been built both with or without "-march=native" compilation flag (if one library is built with this flag and not the other, this is causing Eigen to not work properly, resulting in segmentation faults).]

I'm using robot localization package following the realsense tutorial

my pkg param is like below

<launch>

    <include file="$(find realsense2_camera)/launch/d400.launch">
      <arg name="camera" value="d400"/>
      <arg name="serial_no_camera" value="213522254416"/>
      <arg name="initial_reset" value="false"/>
      <arg name="align_depth" value="true"/>
      <arg name="enable_sync" value="true"/>
      <arg name="clip_distance" value="-2"/>
      <arg name="linear_accel_cov" value="1.0"/>
      <arg name="unite_imu_method" value="linear_interpolation"/>
      <arg name="enable_gyro"   value="true"/>
      <arg name="enable_accel"  value="true"/>

      <!-- <arg name="filters" value="disparity,spatial,temporal,decimation"/> -->
    </include>

    <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
        <param name="use_mag" type="bool" value="false" />
        <param name="publish_tf" type="bool" value="false" />
        <param name="world_frame" type="string" value="enu" />
        <remap from="/imu/data_raw" to="/d400/imu"/>
    </node>

    <include file="$(find robot_localization)/launch/ukf_template.launch"/>
    <param name="/ukf_se/frequency" value="300"/>
    <param name="/ukf_se/base_link_frame" value="base_footprint"/>
    <param name="/ukf_se/odom0" value="odom_md"/>
    <rosparam param="/ukf_se/odom0_config">[true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true]
    </rosparam>
    <param name="/ukf_se/odom0_relative" value="true"/>
    <param name="/ukf_se/odom0_pose_rejection_threshold" value="10000000"/>
    <param name="/ukf_se/odom0_twist_rejection_threshold" value="10000000"/>

    <param name="/ukf_se/imu0" value="/imu/data"/>
    <rosparam param="/ukf_se/imu0_config">[false, false, false,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true]
    </rosparam>
    <param name="/ukf_se/imu0_differential" value="true"/>
    <param name="/ukf_se/imu0_relative" value="false"/>
    <param name="/ukf_se/use_control" value="false"/>
    <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> -->    

    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="d400_link_to_base_link" args="0.008 -0.0115 0.059 0 0 0 base_link d400_link"/> -->

</launch>

Plus my rtabmap param launcher below

 <launch>
    <arg name="rviz" default="false"/>
    <arg name="rtabmapviz" default="false"/>

    <include file="$(find fw_rev_04_navigation)/launch/fw_move_base_teb.launch">
    </include>

    <include file="$(find fw_rev_04_rtabmap)/launch/rtabmap.launch">
        <arg name="rtabmap_args" value="--delete_db_on_start"/>
        <arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
        <arg name="rviz" value="$(arg rviz)"/>
        <arg name="localization" value="false"/>

        <arg name="odom_topic" value="/odometry/filtered"/>
        <arg name="frame_id" value="base_footprint"/>
        <arg name="rgbd_sync" value="true"/>
        <arg name="depth_topic" value="/d400/aligned_depth_to_color/image_raw"/>
        <arg name="rgb_topic"   value="/d400/color/image_raw"/>
        <arg name="camera_info_topic" value="/d400/color/camera_info"/>
        <arg name="approx_rgbd_sync" value="false"/> 
        <arg name="visual_odometry" value="false"/>
        <arg name="scan_topic"      value="/scan_yd"/>
        <arg name="subscribe_scan"  value="true"/>
    </include>

</launch>
.
.
.
      <param name="Mem/UseOdometryGravity" value="true"/>
      <param name="Optimizer/GravitySigma"    value="0.3"/>
      <param name="Grid/FromDepth"           value="true"/>
      <!-- <param name="RGBD/CreateOccupancyGrid"  value="true"/>       -->
      <!-- all points below 50 cm are ground -->
      <param name="Grid/MaxGroundHeight"  value="$(arg max_ground_height)"/>       
      <!-- all points above 20 cm and below 2 m are obstacles -->
      <param name="Grid/MaxObstacleHeight"    value="$(arg max_obstacle_height)"/>
      <param name="Grid/RangeMax"             type="string" value="3.5"/>       
      <!-- Use simple passthrough to filter the ground instead of normals segmentation -->
      <!-- <param name="Grid/NormalsSegmentation" type="string" value="false"/> -->
      <!-- <param name="RGBD/OptimizeFromGraphEnd" value="true"/> -->
      <param name="Reg/Force3DoF"                type="string" value="true"/>
      <param name="Reg/Strategy"                 type="string" value="0"/>  
      <!-- <param name="Rtabmap/DetectionRate"        type="string" value="1"/> -->
      <param name="RGBD/AngularUpdate"           type="string" value="0.03"/>    <!-- Update map only if the robot is moving -->
      <param name="RGBD/LinearUpdate"            type="string" value="0.03"/>    <!-- Update map only if the robot is moving -->
      <!-- <param name="Rtabmap/TimeThr"              type="string" value="0"/> -->
      <!-- <param name="RGBD/NeighborLinkRefining" type="string" value="false"/> -->
      <param name="Vis/MaxDepth"  type="string" value="3.5"/>
      <param name="Vis/MinInliers"  type="string" value="25"/>
      <!-- <param name="Mem/CovOffDiagIgnored" type="string" value="true"/> -->
      <!-- output -->
      <remap from="grid_map" to="/map"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>

Relating to this problem, I've checked my covariance data.

covariance: [0.0006229568214647916, -8.083730846463338e-23, -2.3526478256779684e-27, 7.603975234514498e-32, -1.9686793224112045e-31, 1.0084986900882368e-26, -8.083730846463338e-23, 0.0006229568214647914, 6.593697615547681e-27, 9.816306764103932e-32, -4.042227063101472e-32, -2.9704252787807974e-27, -2.352672574167219e-27, 6.593697567362882e-27, 1.1034054753282324e-06, 3.649317321204718e-33, -4.804595389038422e-33, 2.460199139855491e-28, 2.083755590102779e-33, 4.88594046384911e-32, -4.6055065061564126e-32, 1.0814084149036437e-06, 3.242877461853746e-19, -1.8387080226128324e-21, -1.9686793224112045e-31, -4.042219882283864e-32, 6.731164903353323e-32, 3.242877461854316e-19, 1.0814084149036494e-06, 5.288115321342584e-21, 9.277193333936052e-27, -2.9720030005912395e-27, 2.9662673689168533e-28, -1.838708013874778e-21, 5.2881153300805475e-21, 1.9999397964668092e-09]

It does not seem to poor, localization works quite well.

Plz give me some tip to solve this error..

thank you

jaykorea commented 2 years ago

Solved this problem by downgrade rtabmap version to 0.20.16

Thank you

matlabbe commented 2 years ago

As explained in the error message, this error could be also caused by compilation flags: OptimizerG2O.cpp:958::optimize() Condition (optimizer.verifyInformationMatrices(true)) not met! [This error can be caused by (1) bad covariance matrix set in odometry messages (see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) or that (2) PCL and g2o hadn't been built both with or without "-march=native" compilation flag (if one library is built with this flag and not the other, this is causing Eigen to not work properly, resulting in segmentation faults).] That fatal error check is also there in 0.20.16, maybe it was a compilation issue? closing the issue.

huma0605 commented 1 year ago

I am seeing the same issue. Everything was working fine when localization = false. But when we set it to be true, we saw the same g2o error. Below is my package versions

Package: ros-noetic-rtabmap-ros
Version: 0.21.1-4focal.20230416.011814
Package: ros-noetic-pcl-ros
Version: 1.7.4-1focal.20230216.065759
Package: ros-noetic-libg2o
Version: 2020.5.3-1focal.20210424.015120

All the installations were done through simply

sudo apt-install rod-noetic-rtabmap-ros