How to have nice cloudpoint of depth sensor?

[sympetrumz]

I need to create 3D mapping for outdoor navigation. However, the cloud point produce is not nice and accurate. I am quite new using RTABmap. Hopefully you can help.

[matlabbe]

Is it in a simulator? Can you share a database? The input data look strange.

[sympetrumz]

Yes it is. how can I sent the database input data? because Im quite new with ROS.

here is the file for my rtabmap database that i transfer to gdrive :

https://drive.google.com/drive/folders/1qBdc1P-57H8a6Dcitw0NrwkSjr5Uxukb?usp=sharing 

this is the new one since i run it yesterday.

I'm using Husky robot, VLP-16, Realsense camera and UST10. Just launching demo_husky.launch file for the testing

[matlabbe]

I think the main problem is that the lidar is giving max range data instead of NaN for invalid values, which makes ICP not working correctly:


By adding Icp/RangeMax, the resulting point cloud is a lot better:


cheers,
Mathieu

[sympetrumz]

May I know what changes I need to do and what is the command you run to get this result.

<?xml version="1.0"?>

<launch>

 

  <arg name="navigation"   default="true"/>
  <arg name="localization" default="false"/>
  <arg name="icp_odometry" default="false"/>
  <arg name="rtabmap_viz"  default="false"/>
  <arg name="camera"       default="false"/>
  <arg name="lidar2d"      default="false"/>
  <arg name="lidar3d"      default="false"/>
  <arg name="lidar3d_ray_tracing" default="true"/>
  <arg name="slam2d"       default="true"/>
  <arg name="depth_from_lidar" default="false"/>


  <arg     if="$(arg lidar3d)" name="cell_size" default="0.3"/>
  <arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>

  <arg if="$(eval not lidar2d and not lidar3d)" name="lidar_args" default=""/>

  <arg if="$(arg lidar2d)" name="lidar_args" default="
      --Reg/Strategy 1
      --RGBD/NeighborLinkRefining true
      --Grid/CellSize $(arg cell_size)
      --Icp/PointToPlaneRadius 0
      --Odom/ScanKeyFrameThr 0.5
      --Icp/MaxTranslation 1"/>

  <arg if="$(arg lidar3d)" name="lidar_args" default="
      --Reg/Strategy 1
      --RGBD/NeighborLinkRefining true
      --ICP/PM true
      --Icp/PMOutlierRatio 0.7
      --Icp/VoxelSize $(arg cell_size)
      --Icp/MaxCorrespondenceDistance 1
      --Icp/PointToPlaneGroundNormalsUp 0.9
      --Icp/Iterations 10
      --Icp/Epsilon 0.001
      --OdomF2M/ScanSubtractRadius $(arg cell_size)
      --OdomF2M/ScanMaxSize 15000
      --Odom/ScanKeyFrameThr 0.5
      --Grid/ClusterRadius 1
      --Grid/RangeMax 20
      --Grid/RayTracing $(arg lidar3d_ray_tracing)
      --Grid/CellSize $(arg cell_size)
      --Icp/PointToPlaneRadius 0
      --Icp/PointToPlaneNormalK 10
      --Icp/MaxTranslation 1"/>

 
  <remap from="/rtabmap/grid_map" to="/map"/>
  <include file="$(find rtabmap_launch)/launch/rtabmap.launch">
    <arg     if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
    <arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> 
    <arg name="localization"    value="$(arg localization)" />
    <arg name="visual_odometry" value="true" />
    <arg name="approx_sync"     value="$(eval camera or not icp_odometry)" />
    <arg name="imu_topic"       value="/imu/data" />
    <arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered" />
    <arg name="frame_id"        value="base_link" />
    <arg name="rtabmap_viz"      value="$(arg rtabmap_viz)" />
    <arg name="gps_topic"       value="/navsat/fix"/>

   
    <arg name="subscribe_scan"  value="$(arg lidar2d)" />
    <arg     if="$(arg lidar2d)" name="scan_topic"  value="/front/scan" />
    <arg unless="$(arg lidar2d)" name="scan_topic"  value="/scan_not_used" />

   
    <arg name="subscribe_scan_cloud"  value="$(arg lidar3d)" />
    <arg     if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
    <arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />

   
    <arg name="depth"             value="$(eval camera and not depth_from_lidar)" />
    <arg name="subscribe_rgb"     value="$(eval camera)" />
    <arg name="rgbd_sync"         value="$(eval camera and not depth_from_lidar)" />
    <arg name="rgb_topic"         value="/realsense/color/image_raw" />
    <arg name="camera_info_topic" value="/realsense/color/camera_info" />
    <arg name="depth_topic"       value="/realsense/depth/image_rect_raw" />
    <arg name="approx_rgbd_sync"  value="false" />

   
    <arg name="gen_depth"                  value="$(arg depth_from_lidar)" />
    <arg name="gen_depth_decimation"       value="4" />
    <arg name="gen_depth_fill_holes_size"  value="3" />
    <arg name="gen_depth_fill_iterations"  value="1" />
    <arg name="gen_depth_fill_holes_error" value="0.3" />

   
    <arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
    <arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
    <arg if="$(arg icp_odometry)" name="vo_frame_id" value="icp_odom" />
    <arg unless="$(arg slam2d)"   name="wait_imu_to_init" value="true" />
    <arg if="$(arg lidar3d)"      name="odom_args" value="--Icp/CorrespondenceRatio 0.01"/>
  </include>

 
  <include if="$(arg navigation)" file="$(find husky_navigation)/launch/move_base.launch" />

</launch>

[matlabbe]

In your lidar_args argument, you could add "--Icp/RangeMax 40"

[sympetrumz]

I got it!!! the data seems nice to me. Thank you so much. But I need one more help. I can't download the map into RVIZ. The purpose to run in RVIZ because I want to set Nav Goal. With this question also i want to ask how to implement A* SLAM into it.

[matlabbe]

You will need to use a navigation package that is consuming the map topic from rtabmap. As you are using an husky robot, you may try this demo: https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_demos/launch/demo_husky.launch