3D map is tilted

[Nash]

Hi,

first i must say thank you for the great contribution that you are giving to the community.

I want to make outdoor 3D map with Velodyne VLP-16 and Zed stereo camera. Point cloud map should be constructed from lidar. I am using visual ICP odometry from lidar, but similar result (actually result is a little bit worse) I am getting with external odometry fused with IMU.

If I force 3DOF (Reg/Force3DoF = true) map looks good, but I need to scan terrain which isn't flat. When I scan office with Reg/Force3DoF = false, 3D map is tilted.
I have tried to improve mapping with different detection strategies (SURF, SIFT), and also by changing Vis/ parameters, but no success.
This is how 3D map looks:

Could you please help me with this issue?

I am using rtabmap 0.20.14 built from source with following setup:

<launch>
  
    <arg name="frame_id" default="base_link"/>
    <arg name="rtabmapviz"    default="false"/>
    <arg name="scan_20_hz"    default="false"/>
    <arg name="use_sim_time"  default="true"/>    

    <arg name="left_image_topic"          default="/front_zed/fz/left/image_rect_color"/>
    <arg name="right_image_topic"         default="/front_zed/fz/right/image_rect_color"/>
    <arg name="left_camera_info_topic"    default="/front_zed/fz/left/camera_info"/>
    <arg name="right_camera_info_topic"   default="/front_zed/fz/right/camera_info"/>

    <group ns="rtabmap">

        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="screen">
          <remap from="left/image_rect"   to="$(arg left_image_topic)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
          <param name="approx_sync"     type="bool"   value="false"/>
        </node>

      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="/velodyne_points"/>
        <param name="frame_id"   type="string" value="$(arg frame_id)"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>

        <param name="guess_frame_id"   type="string" value="$(arg frame_id)"/>
        <param name="wait_imu_to_init" type="bool" value="false"/>
        <param name="Reg/Strategy"     type="string" value="1"/>

        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="false"/>
        <param name="Icp/Iterations"          type="string" value="10"/>
        <param name="Icp/VoxelSize"           type="string" value="0.2"/>
        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
        <param name="Icp/Epsilon"             type="string" value="0.001"/>
        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>

        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
        <param name="Odom/Strategy"              type="string" value="1"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
        <param name="Odom/AlignWithGround"       type="string" value="true"/>
      </node>

      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
        <param name="subscribe_rgbd"       type="bool" value="true"/>
        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="true"/>
        
        <remap from="scan_cloud" to="/velodyne_points"/>
        <remap from="odom" to="odom"/>

        <!-- RTAB-Map's parameters -->
        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
        
        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
        <param name="Mem/STMSize"                    type="string" value="30"/>
        
        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Reg/Force3DoF"                  type="string" value="false"/>  <!--Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.Default = false-->

        <param name="Grid/CellSize"                  type="string" value="0.1"/>
        <param name="Grid/RangeMax"                  type="string" value="20"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
        <param name="Grid/FromDepth"                 type="string" value="false"/>
        <param name="Grid/3D"                        type="bool" value="true"/>  <!--A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map. Default = true.-->

        <!-- ICP parameters -->
        <param name="Icp/VoxelSize"                  type="string" value="0.2"/>
        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
        <param name="Icp/PointToPlane"               type="string" value="false"/>
        <param name="Icp/Iterations"                 type="string" value="10"/>
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
        <param name="Icp/Strategy"                   type="string" value="true"/>
        <param name="Icp/OutlierRatio"               type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="0.4"/>
        <param name="Icp/PointToPlaneGroundNormalsUp" type="string" value="1.0"/>

      </node>

      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
        <param name="subscribe_rgbd" type="bool" value="true"/>
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="approx_sync" type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <remap from="scan_cloud" to="/velodyne_points"/>
        <param name="subscribe_depth" value="false"/>
      </node>
  </group>

  <include file="$(find scout_bringup)/launch/sim_rviz.launch"/>

</launch>

If needed generated database can be found at: https://drive.google.com/file/d/1V8cyo1Xr3UPIAr3H2qZypacgLg-VjAA-/view?usp=sharing

Thank you in advance!
Cheers,
Nash

[matlabbe]

Hi,

To debug icp_odometry, I would require a rosbag with at least the velodyne_points topic. The lidar has a lot of constraints in x and y axes, but not much along z axis, making it drift more. There could be also some lidar skewing going on, making ICP drifting more in rotation. You may try with Icp/PointToPlane=true for icp_odometry. It is also possible to use FLOAM with icp_odometry if you want to compare (there is an example here, you must follow the link in comments to know how to install floam).

cheers,
Mathieu

[Nash]

Hi,

thank you for your feedback.

I have tried with Icp/PointToPlane set to true and the result is the same.
With floam feature, result is much better - 2D map is perfect, but 3D is still little bit tilted.
Map pictures are:



I have used following setup:

<launch>
  
    <arg name="frame_id" default="base_link"/>
    <arg name="rtabmapviz"    default="true"/>
    <arg name="scan_20_hz"    default="false"/>      

    <arg name="left_image_topic"          default="/front_zed/fz/left/image_rect_color"/>
    <arg name="right_image_topic"         default="/front_zed/fz/right/image_rect_color"/>
    <arg name="left_camera_info_topic"    default="/front_zed/fz/left/camera_info"/>
    <arg name="right_camera_info_topic"   default="/front_zed/fz/right/camera_info"/>

    <arg name="loop_ratio" default="0.4"/> <!-- Set to 0.2 for kitti -->
    <arg name="floam"    default="true"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
    <arg name="resolution" default="0.2"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
    <arg name="iterations" default="10"/>
    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (car, kitti) -->
    <arg name="queue_size" default="10"/>

    <group ns="rtabmap">
       
        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="screen">
          <remap from="left/image_rect"   to="$(arg left_image_topic)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="rgbd_image"/> 
          <param name="approx_sync"     type="bool"   value="false"/>
        </node>

      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="/velodyne_points"/>
        <param name="frame_id"   type="string" value="$(arg frame_id)"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>

        <param name="guess_frame_id"   type="string" value="$(arg frame_id)"/>
        <param name="wait_imu_to_init" type="bool" value="false"/>
        <param name="Reg/Strategy"     type="string" value="1"/>
        <param name="queue_size"      type="int"    value="$(arg queue_size)"/>

        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
        <param     if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
        <param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
        <param name="Icp/Epsilon"             type="string" value="0.001"/>
        <param     if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
        <param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
        <param name="Icp/Strategy"                  type="string" value="true"/> 
        <param name="Icp/OutlierRatio"      type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>

        <!-- Odom parameters -->       
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
        <param name="publish_tf"                 type="string" value="true"/>
      </node>

      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
        <param name="subscribe_rgbd"       type="bool" value="false"/>
        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="subscribe_scan"       type="bool" value="false"/>
        <param name="approx_sync"          type="bool" value="true"/>
        
        <remap from="scan_cloud" to="/velodyne_points"/>
        <remap from="odom" to="odom"/>

        <!-- RTAB-Map's parameters -->
        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
        <param name="Mem/STMSize"                    type="string" value="30"/>
        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
        
        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Reg/Force3DoF"                  type="string" value="false"/>  <!--Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.Default = false-->

        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
        <param name="Grid/RangeMax"                  type="string" value="20"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
        <param name="Grid/FromDepth"                 type="string" value="false"/>
        <param name="Grid/3D"                        type="bool" value="true"/>  <!--A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map. Default = true.-->
        <param name="Grid/RayTracing"                type="string" value="true"/>

        <!-- ICP parameters -->
        <param name="Icp/VoxelSize"                  type="string" value="0"/> <!-- already voxelized by point_cloud_assembler below -->
        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
        <param name="Icp/PointToPlane"               type="string" value="true"/>
        <param name="Icp/Iterations"                 type="string" value="$(arg iterations)"/>
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
        <param name="Icp/Strategy"                   type="string" value="true"/> 
        <param name="Icp/OutlierRatio"               type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>

      </node>

      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
        <param name="subscribe_rgbd" type="bool" value="false"/>
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="approx_sync" type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <remap from="scan_cloud" to="/velodyne_points"/>
        <param name="subscribe_depth" value="false"/>
        <remap from="odom_info" to="odom_info"/>
        <param name="odom_frame_id" type="string" value="odom"/>
      </node>
  </group>

  <include file="$(find scout_bringup)/launch/sim_rviz.launch"/>

</launch>

If you have time and if you are willing to inspect rosbag with only velodyne_points, here is the link:
https://drive.google.com/file/d/1_9_ikEAhqyX_qdGkDiSi4X274yrSSSx5/view?usp=sharing

Thanks in advance!

BR,
Nash

[matlabbe]

Hi Nash,

Thanks for the rosbag. I also tested floam and f2m odometry. I updated test_velodyne.launch with more options to work this dataset (I updated the key frame thr and added some grid parameters). Note that for best results with f2m, resolution should be 0.1m. Increasing local_map_size (default 15000) may also help to get lower drift. Setting ground_normals_up can be also set to true with ground robot like that.

With floam odometry:

roslaunch rtabmap_ros test_velodyne.launch use_sim_time:=true floam:=true ground_is_obstacle:=false grid_max_range:=5 

With f2m odometry:

roslaunch rtabmap_ros test_velodyne.launch use_sim_time:=true ground_normals_up:=true ground_is_obstacle:=false grid_max_range:=5 local_map_size:=30000

Results (FLOAM on left, F2M on right):

From each side (gray line is the grid, ortho mode):







When looking at the probability map, we can see that the small blacks cells in middle of the path (caused by the following person) have lower probability than static objects/walls but igh enough to be set as obstacles in the grid map.

Here the same maps with GridGlobal/OccupancyThr=0.9 (default 0.5):

[Nash]

Hi Mathieu,

thank you for your support!
I have tried your settings and I got the same results.

BR,
Nash