using RTAB-Map with a 2D lidar with a limited Line Of Sight and a D435i

[yonim]

Hi Mathieu,

I lead the Autonomous Drones lab at Tel Aviv University.

We would like to use RTAB-Map with a 2D lidar with a limited line of sight (los) and a D435i.
The los of the 2D lidar is limited to 90 degrees forward and 90 degrees backward (see attachment),

How would you recommend using RTAB-Map for this configuration?

Thank you,  Yoni

[matlabbe]

Hi,

You may filter the laser scan topic and put "inf" for range values on left and right depending on their angle.

If the drone moves always at the same height (and their is no high roll or pitch variations), you may simulate it like a 2D mobile robot and use the appropriate rtabmap config (in 2D).

For 6 DoF trajectories, you could feed the 2D lidar in rtabmap but I would disable ICP refinements using it as the scans may not be always in the same plane (making difficult to do ICP). You may also not feed the lidar to rtabmap, letting it rely only on D435i data for the map, while 2D lidar could be used by the low level controller for obstacle avoidance.

If you have odometry independent of the lidar, it could be possible to assemble the laser scans for 1 second (for example) and feed that assembled 3D cloud to rtabmap (using PointCloud2 interface instead of LaserScan).

cheers,
Mathieu

[yonim]

Thank you for the prompt answer!

We're going to operate the drone as a ground robot in a constant height (with roll and pitch near 0), thus we're only interested in 3 DOF navigation.
How would you output the 2d occupancy grid ?  RVIZ?
Can you suggest a ROS package for the path planning and  the autonomous navigation?

Thank you for all this effort !!

[matlabbe]

For 2D navigation, look for move_base package: http://wiki.ros.org/navigation

If rtabmap is subscribed to a 2D laser scan, it will output a 2D occupancy grid topic, which can be visualized in rviz. This map can be fed to move_base above for global planning. IF your drone can respond to Twist command, then it would be compatible with move_base.

You may check this turtlebot example with rtabmap+move_base integration: http://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot

cheers,
Mathieu

[yonim]

When filtering the laser scan topic, and putting "inf" for range values on left and right depending on their angle, is this done through the laser_filter ROS package or using the parameters of RTAB-Map node?

Thank you

[matlabbe]

I think it is doable with laser_filters using a 2 consecutive LaserScanAngularBoundsFilterInPlace in the same scan filter chain. I verified and with invalid ranges like max_range+1, rtabmap uses transformLaserScanToPointCloud to convert the scan, and it seems to check if values are under the max_range, so it would work.

scan_filter_chain:
- name: angleLeft
  type: laser_filters/LaserScanAngularBoundsFilterInPlace
  params:
    lower_angle: 0.785398163
    upper_angle: 2.35619449
- name: angleRight
  type: laser_filters/LaserScanAngularBoundsFilterInPlace
  params:
    lower_angle: -2.35619449
    upper_angle: -0.785398163

cheers,
Mathieu

[yonim]

Thanks.
We're trying to follow your suggestion and rely only on the D435i data for the map (using rtabmap.launch).
The problem is that we need the 2D laser scan for the 2D occupancy grid (for the navigation).
When we enable the scan in rtabmap.launch, <arg name="subscribe_scan"   default="true"/> , we receive the following warning, and the rtabmapviz map is empty. Please advise.

[ WARN] [1614864783.081739984]: /rtabmap/rtabmapviz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmapviz subscribed to (exact sync):
   /rtabmap/odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /scan \
   /rtabmap/odom_info


Thank you  !!

[matlabbe]

You may have to set approx_sync to true when you are synchronizing multiple sensors. I would also suggest to use rgbd_sync node to pre-sync D400 data.

Also when receiving this warning, like it is said, check if all topics are published:

rostopic hz /rtabmap/odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /scan \
   /rtabmap/odom_info 

When some of them are not published, then we have to check why the nodes publishing them is not publishing them.

EDIT: you may look on this page if you can see a setup similar of what you want to do: http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot

[yonim]

Thanks again !!

I was wondering if in the visual odometry, while using the D435i , you fuse the IMU with the depth and RGB to improve the odometry?

Warm regards, Yoni

[yonim]

I was wondering if in the visual odometry, while using the D435i , you fuse the IMU with the depth and RGB to improve the odometry (tightly or loosely coupled optimization)?

roslaunch rtabmap_ros rtabmap.launch \
    rtabmap_args:="--delete_db_on_start --Optimizer/GravitySigma 0.3" \
    depth_topic:=/camera/aligned_depth_to_color/image_raw \
    rgb_topic:=/camera/color/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    approx_sync:=false \
    wait_imu_to_init:=true \
    imu_topic:=/rtabmap/imu

Thank you!!

[matlabbe]

Hi,

The approach is loosely coupled. I add gravity factors (based on estimated IMU quaternion computed by madgwick or complementary filters) in the local bundle adjustment (g2o or gtsam) of F2M odometry (position integration from imu is not used, only orientation). Setting Optimizer/GravitySigma lower will make bundle adjustment to follow more the IMU. If the IMU is noisy, you don't want it to be too low either, and follow more 3D visual features instead.

Note that it is also possible to build RTAB-Map with some supported tightly coupled VIO approaches to choose them while launching rtabmap.launch by just setting "Odom/Strategy":

rtabmap --params | grep Odom/Strategy
Param: Odom/Strategy = "0"    [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]

For built-in odometry approaches (F2M or F2F), only F2M uses IMU in its optimization, F2F will use it only to improve the motion guess with the next frame.

The IMU adds also gravity constraints in the SLAM back-end, for each node in the graph. During graph optimization, it will then make the map aligned with gravity.

cheers,
Mathieu

[yonim]

Thanks Mathieu,

While working only with the D435i (using the rtabmap.launch version below) in 3 DOF, we lose localization near walls.
How would you recommend to add a lidar to this configuration to solve the issue?

Best, Yoni

<launch>
     
  <!-- Choose between depth and stereo, set both to false to do only scan -->      
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>

  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>         
   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>  <!--Yoni orig is false-->
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>
  
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
  
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>

  <!-- Nodes -->
  <group ns="$(arg namespace)">
     
    <!-- relays -->
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
      </group>      
    </group>
    
    
    <!-- Visual odometry -->
    <group unless="$(arg icp_odometry)">   <!--Yoni orig is unless-->
      <group if="$(arg visual_odometry)">
      
        <!-- RGB-D Odometry -->
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>
      
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
          <param name="config_path"                 type="string" value="$(arg cfg)"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
  
          <param name="Reg/Force3DoF"               type="string" value="true"/>

        </node>       
      </group>
    </group>
    
   
    
    
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node unless="$(arg stereo)" name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>      
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <!-- 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)"/>

      <param name="Reg/Force3DoF"             type="string" value="true"/>

    </node>

</launch>

[matlabbe]

See minimal examples on this page if one can fit your needs:
http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot

[idanaviv10]

Hi Mathieu,

In my project I also using a lidar with 90 degree on each side (180 degree in total). During the mapping session, the map is drifting, I think is might due to the low fov of the lidar, which cause the icp odometry to drift. Another problem that might affect the drift is the low number of loop closures, many times I wont get any loop closure even though I am repeating the same route  (I am using sift as feature point).

What do you think I could do to reduce the map drifting? Do you have any suggestion to improve the odometry and the loop closure?

Warm regards,
Idan

 <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
    <remap from="rgb/image"       to="/d435/color/image_raw"/>
    <remap from="depth/image"     to="/d435/aligned_depth_to_color/image_raw"/>
    <remap from="rgb/camera_info" to="/d435/color/camera_info"/>
    <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
    <param name="approx_sync"       value="false"/> 
  </node>

  <!-- ICP Odometry -->
    
    <node  pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry">
     <remap from="scan"                   to="/scan_filtered"/>
     <param name="frame_id"                    type="string" value="base_footprint"/>
     <param name="odom_frame_id"               type="string" value="icp_odom"/>
     <param name="guess_frame_id"               type="string" value="odom"/>
     <param name="publish_tf" type="bool"   value="true"/>
     <param name="Odom/ResetCountdown"  type="string" value="1"/>
     <param name="Force4DoF"  type="string" value="true"/>
     <param name="Icp/PointToPlane"  type="string" value="true"/>
     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
     <param name="Icp/PointToPlaneK"  type="string" value="5"/>
     <param name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
     <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
     <param name="Icp/PM"             type="string" value="true"/>
     <param name="Icp/PMOutlierRatio" 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>


  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)"> <!--delete_db_on_start -->
    <param name="frame_id"        type="string" value="base_footprint"/>
    <param if="$(arg localization)" name="publish_tf" type="bool"   value="true"/>
    <param unless="$(arg localization)" name="publish_tf" type="bool"   value="true"/>
    <remap from="scan" to="/scan_filtered"/>    
    <param name="approx_sync" value = "true"/>
    <param name="subscribe_scan" type="bool" value="true"/>
    <param name="subscribe_depth" type="bool" value="false"/>
    <param name="subscribe_rgbd" type="bool" value="true"/>
    <param name="subscribe_rgb" type="bool" value="false"/>
    <param name="queue_size" type="int" value="10"/>
    <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>

    <!-- RTAB-Map's parameters -->
    <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
    <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
    <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
    <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
    <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
    <param name="Grid/FromDepth"            type="string" value="false"/> <!-- occupancy grid from lidar -->
    <param name="Reg/Force3DoF"             type="string" value="true"/>
    <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->

    <param name="Kp/DetectorStrategy"       type="string" value="1"/>
    <param name="Kp/RoiRatios"              type="string" value="0 0 0 0.25"/>
    <param name="Rtabmap/DetectionRate"              type="string" value="1"/>

    <!-- ICP parameters -->
    <param name="Icp/VoxelSize"                 type="string" value="0.05"/>
    <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

    <!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base --> 
    <param name="use_action_for_goal" type="bool" value="true"/>
    <remap from="move_base"            to="/move_base"/>
    <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>

[matlabbe]

Hi,

do you have a database to share? If the FOV of the lidar is too narrow, short range and environment not enough complex geometrically, ICP won't work very well. For example, if the lidar can only see left and right, and you are moving in a corridor, it is likely that it will drift a lot. How about not using ICP odometry? just wheel odometry?