search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.62k stars 763 forks source link

Question about running rosbag file with RTABMAP #796

Open sunglyoungKim opened 2 years ago

sunglyoungKim commented 2 years ago

Hello,

I have recorded data stream from 2D lidar + realsense D455 camera setup with turtle bot by rosbag record. The rtabmap.db is correct and I retrive 2D and 3D maps. However, when I run the simulation with the following launch file I got this Warning message.

[ WARN] (2021-12-16 13:14:44.473) RegistrationIcp.cpp:900::computeTransformationImpl() Transform is found (xyz=0.014076,-0.001439,0.000000 rpy=0.000000,-0.000000,-0.000090) but no correspondences has been found!? Variance is unknown! [ WARN] (2021-12-16 13:14:44.473) OdometryF2M.cpp:529::computeTransform() Registration failed: "Cannot compute transform (cor=0 corrRatio=0.000000/0.100000 maxLaserScans=583)" (guess=xyz=0.370777,-0.022517,-0.000000 rpy=0.000000,0.000000,-0.254357)

The initial map is updated but after that, it doesn't update it anymore because the registration failed.

I wonder what parameters should I relax. Could you point me out?

<!-- Choose visualization -->
  <arg name="rviz" default="True" />
  <arg name="rtabmapviz" default="false" />

  <!-- Choose hector_slam or icp_odometry for odometry -->
  <arg name="hector" default="false" />

  <!-- If "hector" above is false, this option feeds wheel odometry to
       icp_odometry as guess ( to be more robust to corridor-like environments).
       If so, use original demo_mapping.bag containing wheel odometry! -->
  <arg name="odom_guess" default="true" />

  <!-- Example with camera or not -->
  <arg name="camera" default="true" />

  <!-- Limit lidar range if > 0 (has effect only when hector:=false) -->
  <arg name="max_range" default="25" />

  <!-- Point to Plane ICP? (has effect only when hector:=false) -->
  <arg name="p2n" default="true" />

  <!-- Use libpointmatcher for ICP? (has effect only when hector:=false) -->
  <arg name="pm" default="true" />

  <param name="use_sim_time" type="bool" value="true"/>

  <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
     <remap from="scan"      to="/scan"/>
     <remap from="odom"      to="/scanmatch_odom"/>
     <remap from="odom_info"      to="/rtabmap/odom_info"/>

     <param name="frame_id"        type="string" value="base_link"/>

     <param if="$(arg odom_guess)" name="odom_frame_id"   type="string" value="icp_odom"/>
     <param if="$(arg odom_guess)" name="guess_frame_id"  type="string" value="odom"/>

     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/RangeMax"      type="string" value="$(arg max_range)"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
     <param unless="$(arg odom_guess)" name="Icp/MaxTranslation" type="string" value="0.3"/> <!-- can be set to reject large ICP jumps -->
     <param if="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="true"/>
     <param if="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="5"/>
     <param if="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0"/>
     <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
     <param name="Icp/Strategy"             type="string" value="$(arg pm)"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
     <param name="Icp/OutlierRatio" type="string" value="0.85"/>
     <param name="Odom/Strategy"        type="string" value="0"/>
     <param name="Odom/GuessMotion"     type="string" value="true"/>
     <param name="Odom/ResetCountdown"  type="string" value="0"/>
     <param name="Odom/ScanKeyFrameThr"  type="string" value="0.9"/>
  </node>

  <group ns="rtabmap">
    <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      <param name="approx_sync"     value="false"/>

    </node>

  <arg name="rgbd_odometry"     default="true"/>
  <arg name="localization"      default="false"/>

  <arg if="$(arg localization)" name="database_path"     default="simul_db.db"/>
  <arg unless="$(arg localization)" name="database_path"    default="simul_test.db"/>

  <arg     if="$(arg localization)" name="args"  default=""/>
  <arg unless="$(arg localization)" name="args"  default="--delete_db_on_start"/>
  <arg name="rgb_topic"   default="/camera/color/image_raw"/>
  <arg name="depth_topic" default="/camera/aligned_depth_to_color/image_raw"/>
  <arg name="camera_info_topic" default="/camera/color/camera_info"/>
  <arg name="depth_camera_info_topic" value="/camera/depth/camera_info"/>

  <arg name="wait_for_transform"  default="120.0"/> 

    <!-- SLAM -->

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">

      <param name="database_path"       type="string" value="$(arg database_path)"/>

      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>

      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd"  type="bool" value="$(arg camera)"/>
      <param name="subscribe_scan"  type="bool" value="true"/>

      <remap from="scan" to="/scan"/>

      <!-- use actionlib to send goals to move_base --> 
      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base"            to="/move_base"/>

      <param name="odom_sensor_sync" type="bool" value="true"/>
      <remap from="grid_map" to="/map"/>

      <!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
      <param if="$(arg hector)" name="odom_frame_id"            type="string" value="hector_map"/>
      <param if="$(arg hector)" name="odom_tf_linear_variance"  type="double" value="0.0005"/>
      <param if="$(arg hector)" name="odom_tf_angular_variance" type="double" value="0.0005"/>

      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>

      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"       type="string" value="0"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Reg/Force3DoF"      type="string" value="true"/>

      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value = "10"/>
      <param name="Icp/CorrespondenceRatio"  type="string" value="0.3"/>
      <param name="Icp/VoxelSize"            type="string" value="0.03"/>
      <param name="Icp/RangeMax"             type="string" value="$(arg max_range)"/>
      <param name="Grid/RangeMax"            type="string" value="$(arg max_range)"/>
      <param name="Grid/Sensor" type="string" value="0"/>
      <param name="Grid/MaxObstacleHeight" type="string" value="1.50" />
      <param name="Grid/MaxGroundHeight"       type="string" value="0.1"/>

      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  <!-- Set to false to generate map correction between /map and /odom -->

      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
      <param name="Vis/MinInliers"               type="string" value="15"/>      <!-- 3D visual words minimum inliers to accept loop closure -->
      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving --> 
      <param name="Rtabmap/TimeThr"              type="string" value="0"/>
      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
      <param name="GridGlobal/MinSize"           type="string" value="30"/>
      <param name="Odom/Holonomic"               type="string" value="false"/>

      <!-- 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)"/> 

      <!-- turn on gpu -->
      <param name="FAST/Gpu"                     type="string" value="true"/>
      <param name="ORB/Gpu"                      type="string" value="true"/>
      <param name="SURF/GpuVersion"              type="string" value="true"/>
      <param name="Vis/CorNNType"                type="string" value="4"/> <!-- GPU brutalForce --> 

    </node>

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_rgbd"      type="bool" value="$(arg camera)"/>
      <param name="subscribe_laserScan" type="bool" value="true"/>
      <param name="frame_id"            type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>

      <remap from="scan"            to="/scan"/> 

      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
    </node>

  </group>

  <!--  <include file="$(find robot_localization)/launch/ukf_template.launch"/> -->

  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/simulation_rviz.rviz" output="screen"/>
<arg name="wait_for_transform"  default="0.2"/>
   <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgbd_image"      to="/rtabmap/rgbd_image"/>
    <remap from="cloud"           to="voxel_cloud" />

    <param name="voxel_size" type="double" value="0.01"/>
    <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
  </node>
matlabbe commented 2 years ago

icp_odometry got lost for some reasons: ICP couldn't not find correspondences. Are you using rtabmap built with libpointmatcher? Can you share your rosbag? or the first 10 seconds of the bag?