Lidar based 3d SLAM in sparse feature environment

Posted by Gi_T on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Lidar-based-3d-SLAM-in-sparse-feature-environment-tp10174.html

Hello, I’m currently working on a drone project with the goal of the drone being able to autonomously explore and build a map of a building that doesn’t have many visual features e.g. blank walls and sparse furniture.

I recently came across the rtabmap_drone_example repository which is build on PX4 and RTABMAP and wanted to adapt this to my usecase. The drone also contains a Velodyne VLP16 Lidar which I added to the available Gazebo model and configuration giving promising initial results on maps with lots of visual features like the aws-robomaker-small-house-world.

Working map:


Result:


The current configuration however doesn’t work well for maps without many visual features as the odometry starts to drift right after taking of.  Can you provide some guidance on possible approaches for getting stable odometry in such an environment. The real-world test map will be build out of MDF and contain sparse furniture.


Failure map:


Current slam.launch:

<?xml version="1.0"?>
<launch>
   
    <arg name="localization" default="false"/>
    <arg name="rtabmap_viz"   default="true"/>
    <arg name="ground_truth" default="false"/>
    
    <arg     if="$(arg localization)" name="pre_args" default=""/>
    <arg unless="$(arg localization)" name="pre_args" default="-d"/>
   
    <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="standalone rtabmap_util/imu_to_tf">
      <remap from="imu/data" to="/mavros/imu/data"/>
      <param name="fixed_frame_id" value="base_link_stabilized"/>
      <param name="base_frame_id" value="base_link"/>
    </node>

    <!-- To connect rtabmap planning stuff with move_base below -->
    <param name="/rtabmap/rtabmap/use_action_for_goal" value="true"/>
    <remap from="/rtabmap/move_base" to="/move_base"/>

    <!-- VSLAM -->
    <param name="/rtabmap/rtabmap/latch" value="false"/> <!-- For some reason, if we latch grid_map, the global costmap inflation layer will create holes on robot path. To avoid holes, republish grid_map on each update (latching disabled). -->
    <include file="$(find rtabmap_launch)/launch/rtabmap.launch">
      <arg name="localization"      value="$(arg localization)"/>
      <arg name="args"              value="$(arg pre_args) --Optimizer/GravitySigma 0.1 --Vis/FeatureType 10 --Kp/DetectorStrategy 10 --Grid/MapFrameProjection true --NormalsSegmentation false --Grid/MaxGroundHeight 1 --Grid/MaxObstacleHeight 1.6 --RGBD/StartAtOrigin true" />
      <arg name="rtabmap_viz"        value="$(arg rtabmap_viz)" />
      <arg name="frame_id"          value="base_link" />
      <arg name="odom_guess_frame_id" value="base_link_stabilized" />
      <arg name="rgb_topic"         value="/r200/camera/color/image_raw" />
      <arg name="depth_topic"       value="/r200/camera/depth/image_raw" />
      <arg name="camera_info_topic" value="/r200/camera/color/camera_info" />
      <arg name="imu_topic"         value="/mavros/imu/data"/>
      <arg name="wait_imu_to_init"  value="true"/>
      <arg name="approx_sync"       value="true"/>
      <arg name="subscribe_scan_cloud" value="true"/>
      <param name="subscribe_depth" value="false"/>
      <param name="subscribe_rgbd" value="false"/>
      <param name="subscribe_rgb" value="false"/>
      <arg name="scan_cloud_topic" value="/velodyne_points"/>
      
      <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/>
      <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
    </include>
    
    <!-- Costmap -->
    <node pkg="nodelet" type="nodelet" name="camera_points_xyz" args="standalone rtabmap_util/point_cloud_xyz">
      <remap from="depth/image"       to="/r200/camera/depth/image_raw"/>
      <remap from="depth/camera_info" to="/r200/camera/depth/camera_info"/>
      <remap from="cloud"             to="camera_cloud" />

      <param name="decimation"  type="double" value="4"/>
      <param name="voxel_size"  type="double" value="0.0"/>
      <param name="approx_sync" type="bool"   value="true"/>
    </node> 

    <!-- navigation -->  
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
       <remap from="map" to="/rtabmap/grid_map"/>
       <remap from="odom" to="/rtabmap/odom"/>
       <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />  
       <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
       <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
       <rosparam file="$(find rtabmap_drone_example)/param/global_costmap_params.yaml" command="load" />
       <rosparam file="$(find rtabmap_drone_example)/param/local_costmap_params.yaml" command="load" />
       <rosparam file="$(find rtabmap_drone_example)/param/base_local_planner_params.yaml" command="load" />
    </node>
   <node name="empty_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_empty">
       <param name="cell_type" value="0"/>
   </node>
   <node name="marked_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_marked" />

    <!-- joystick -->
    <rosparam file="$(find rtabmap_drone_example)/config/joy_config.yaml" />
    <node pkg="joy" type="joy_node" name="joy_node">
        <param name="autorepeat_rate" value="5"/>
    </node>
    <node pkg="teleop_twist_joy" type="teleop_node" name="teleop_node" output="screen">
        <param name="autorepeat_rate" value="5"/>
    </node>

    <!-- Ground truth -->
  <node if="$(arg ground_truth)" pkg="tf" type="static_transform_publisher" name="worldtomap_broadcaster" args="0 0 0 0 0 0 1 world map 100" />
  <node if="$(arg ground_truth)" pkg="rtabmap_util" type="gazebo_ground_truth.py" name="gazebo_ground_truth" output="screen">
    <param name="frame_id"        value="world"/>
    <param name="child_frame_id"  value="base_link_gt"/>
    <param name="gazebo_frame_id" value="drone::base_link"/>
  </node>
</launch>

Any help is greatly appreciated.

Update:
I now tried out some different combinations of sensors and maps. Results can be seen below:

AWS RoboMaker Small House World

VSLAM (default settings of repository)

<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
      <arg name="localization"      value="$(arg localization)"/>
      <arg name="args"              value="$(arg pre_args) --Optimizer/GravitySigma 0.1 --Vis/FeatureType 10 --Kp/DetectorStrategy 10 --Grid/MapFrameProjection true --NormalsSegmentation false --Grid/MaxGroundHeight 1 --Grid/MaxObstacleHeight 1.6 --RGBD/StartAtOrigin true" />
      <arg name="rtabmap_viz"        value="$(arg rtabmap_viz)" />
      <arg name="frame_id"          value="base_link" />
      <arg name="odom_guess_frame_id" value="base_link_stabilized" />
      <arg name="rgb_topic"         value="/r200/camera/color/image_raw" />
      <arg name="depth_topic"       value="/r200/camera/depth/image_raw" />
      <arg name="camera_info_topic" value="/r200/camera/color/camera_info" />
      <arg name="imu_topic"         value="/mavros/imu/data"/>
      <arg name="wait_imu_to_init"  value="true"/>
      <arg name="approx_sync"       value="true"/>
      
      <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/>
      <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
</include>

Result: https://youtu.be/X2k-8kg89g8

Camera + Lidar with Visual Odometry

<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
      <arg name="localization"      value="$(arg localization)"/>
      <arg name="args"              value="$(arg pre_args) --Optimizer/GravitySigma 0.1 --Vis/FeatureType 10 --Kp/DetectorStrategy 10 --Grid/MapFrameProjection true --NormalsSegmentation false --Grid/MaxGroundHeight 1 --Grid/MaxObstacleHeight 1.6 --RGBD/StartAtOrigin true" />
      <arg name="rtabmap_viz"        value="$(arg rtabmap_viz)" />
      <arg name="frame_id"          value="base_link" />
      <arg name="odom_guess_frame_id" value="base_link_stabilized" />
      <arg name="rgb_topic"         value="/r200/camera/color/image_raw" />
      <arg name="depth_topic"       value="/r200/camera/depth/image_raw" />
      <arg name="camera_info_topic" value="/r200/camera/color/camera_info" />
      <arg name="imu_topic"         value="/mavros/imu/data"/>
      <arg name="wait_imu_to_init"  value="true"/>
      <arg name="approx_sync"       value="true"/>
      <arg name="subscribe_scan_cloud" value="true"/>
      <arg name="scan_cloud_topic" value="/velodyne_points"/>
      
      <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/>
      <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
</include>

Result: https://youtu.be/V5ZD5773K80

Camera + Lidar with ICP Odometry

    <include file="$(find rtabmap_launch)/launch/rtabmap.launch">
      <arg name="localization"      value="$(arg localization)"/>
      <arg name="args"              value="$(arg pre_args) --Optimizer/GravitySigma 0.1 --Vis/FeatureType 10 --Kp/DetectorStrategy 10 --Grid/MapFrameProjection true --NormalsSegmentation false --Grid/MaxGroundHeight 1 --Grid/MaxObstacleHeight 1.6 --RGBD/StartAtOrigin true" />
      <arg name="rtabmap_viz"        value="$(arg rtabmap_viz)" />
      <arg name="frame_id"          value="base_link" />
      <arg name="odom_guess_frame_id" value="base_link_stabilized" />
      <arg name="rgb_topic"         value="/r200/camera/color/image_raw" />
      <arg name="depth_topic"       value="/r200/camera/depth/image_raw" />
      <arg name="camera_info_topic" value="/r200/camera/color/camera_info" />
      <arg name="imu_topic"         value="/mavros/imu/data"/>
      <arg name="wait_imu_to_init"  value="true"/>
      <arg name="approx_sync"       value="true"/>
      <arg name="subscribe_scan_cloud" value="true"/>
      <arg name="scan_cloud_topic" value="/velodyne_points"/>
      <arg name="icp_odometry" value="true"/>
      
      <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/>
      <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
 </include>

Result: https://youtu.be/KtJFd-DVsB4

Custom map

Same settings as above:
Custom map camera: https://youtu.be/WW58vcVW380
Custom map camera + lidar: https://youtu.be/hG3MEwAEl9s
Custom map ICP: https://youtu.be/4z_wYMdYGU0