rtabmap_ros icon indicating copy to clipboard operation
rtabmap_ros copied to clipboard

The detection effect of the external odometer loop is worse than that of using a visual odometer,why?

Open tandongxu0604 opened this issue 2 years ago • 1 comments

Hello, I am trying to transfer an external odometer to the rtabmap node. This external odometer is transmitted through a robot_ Localization obtained. But when the rtabmap node is building the map, the loop detection effect is much worse than using a visual odometer.The former will have many ghosts. May I ask what the problem may be? My rtabmap detection frequency is 1HZ, is it because the detection frequency is too low? this is my launch:

     <param name="wait_imu_to_init"  type="bool" value="false"/>
     <remap if="$(arg use_ins_imu)"   from="imu"             to="/sensor_ins/ins_imu"/>
     <!--remap unless="$(arg use_ins_imu)" from="imu"             to="/sensor/power/imu"/-->

     <param name="subscribe_rgbd"  type="bool" value="$(arg stereo_sync)"/>
     <param name="frame_id"        type="string" value="base_link"/>
     <param name="odom_frame_id"   type="string" value="odom"/>

     <param if="$(arg use_ekf)"     name="publish_tf"      type="bool"   value="false"/>
     <param unless="$(arg use_ekf)" name="publish_tf"      type="bool"   value="true"/>
     <param name="queue_size"      type="int"    value="1000"/>

     <!--0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D-->

     <param name="Odom/Strategy"      type="string" value="$(arg use_stereo_odometry_type)"/>  
     <param name="OdomORBSLAM/VocPath"  type="string" value="/home/ubuntu/orb_slam3.1.0_ws/src/ORB_SLAM3/Vocabulary/ORBvoc.txt"/>
     <param name="OdomORBSLAM/Bf"  type="double" value="47.90639384423901"/>
     <param name="OdomORBSLAM/Fps"  type="double" value="17.0"/>

     <param name="Vis/EstimationType" type="string" value="1"/> 
     <param name="Vis/MaxDepth"       type="string" value="0"/>
     <param name="Kp/MaxDepth"        type="string" value="0"/>
     <param name="Odom/GuessMotion" type="string" value="false"/>
     <param name="Odom/GuessSmoothingDelay" type="int" value="1"/>
     <param name="Vis/MinInliers"     type="string" value="10"/> 
     <param name="Vis/FeatureType"                  type="int"    value="$(arg vis_feature_type)"/> <!--    0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=  10=ORB‑OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector  -->
   </node>

  <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" output="screen"-->
  <node if="$(arg rtabmap)" name="rtabmap" pkg="rtabmap_ros" type="rtabmap"   args="--delete_db_on_start" output="screen">
     <param name="frame_id"        type="string" value="base_link"/>
     <param name="odom_frame_id"   type="string" value="odom"/>
     <param unless="$(arg stereo_sync)" name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_rgbd"      type="bool" value="$(arg stereo_sync)"/>
     <param name="subscribe_depth" type="bool" value="false"/>
     <remap from="depth/image" to="/elas/depth"/>
     <remap from="left/image_rect"   to="/sensor_stereo_1/left/image_rect_color"/>
     <remap from="right/image_rect"  to="/sensor_stereo_1/right/image_rect"/>
     <remap from="left/camera_info"  to="/sensor_stereo_1/left/camera_info"/>
     <remap from="right/camera_info" to="/sensor_stereo_1/right/camera_info"/>
     <remap from="rgbd_image"        to="/sensor_stereo_1/rgbd_image"/>

     <remap unless="$(arg use_ekf)" from="odom" to="/stereo_odometry"/>
     <remap if="$(arg use_ekf)" from="odom" to="/odometry/filtered"/>

     <param name="queue_size" type="int" value="1000"/>
     <param name="map_negative_poses_ignored" type="bool" value="true"/>

     <!-- RTAB-Map's parameters -->
     <param name="Rtabmap/TimeThr"             type="string" value="0"/>
     <param name="Grid/DepthDecimation"        type="string" value="4"/>
     <param name="Rtabmap/DetectionRate"        type="string" value="5"/> 

     <param name="Grid/FlatObstacleDetected"   type="string" value="true"/>

    <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB‑OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector -->
    <param name="Kp/DetectorStrategy"               type="string" value="$(arg vis_feature_type)"/>
    <param name="Vis/FeatureType"                  type="int"     value="$(arg vis_feature_type)"/>

    <!-- use same Vis/FeatureType as odom’s -->
    <param name="Vis/EstimationType"                type="string" value="1"/>   <!-- 0=3D->3D, 1=3D->2D (PnP) -->
    <param name="Vis/MaxDepth"                      type="string" value="0"/>
    <param name="Kp/MaxDepth"                       type="string" value="0"/>
    <param name="RGBD/CreateOccupancyGrid"          type="string" value="true"/>

    <param name="g2o/Optimizer"                  type="int"    value="1"/>
    <param name="Optimizer/Strategy"                  type="int"    value="3"/>
    <param name="g2o/Solver"                  type="int"    value="3"/>

    </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 unless="$(arg stereo_sync)" name="subscribe_stereo"    type="bool" value="true"/>
     <param name="subscribe_odom_info" type="bool" value="false"/>
     <param name="subscribe_rgbd"      type="bool" value="$(arg stereo_sync)"/>
     <param name="queue_size"          type="int" value="1000"/>
     <param name="frame_id"        type="string" value="base_link"/>

     <remap from="left/image_rect"  to="/sensor_stereo_1/left/image_rect_color"/>
     <remap from="right/image_rect"  to="/sensor_stereo_1/right/image_rect"/>
     <remap from="left/camera_info"  to="/sensor_stereo_1/left/camera_info"/>
     <remap from="right/camera_info" to="/sensor_stereo_1/right/camera_info"/>
     <remap from="rgbd_image"          to="/sensor_stereo_1/rgbd_image"/>
     <remap from="odom_info"         to="odom_info"/>
     <remap unless="$(arg use_ekf)" from="odom" to="/stereo_odometry"/>
     <remap if="$(arg use_ekf)" from="odom" to="/odometry/filtered"/>
     <remap from="mapData"           to="mapData"/>

     <!--param name="subscribe_depth" type="bool" value="true"/>
     <remap from="depth/image" to="/elas/depth"/-->

     
  </node>

tandongxu0604 avatar Aug 08 '23 15:08 tandongxu0604

It could depend if the odometry is drifting more, or the odometry covariance is poorly balanced in comparison to ones added during loop closure detection.

matlabbe avatar Aug 13 '23 21:08 matlabbe