Gridmap is wrong with HDL32E Lidar and D435 camera fusion

[AhmedTM]

Hello guys, First I would like to thank you on this awesome work your package is really helpful and easy to use and debug. I am trying to build a gridmap for an outdoor environment simulation on gazebo I am using HDL32E LiDAR, D435 camera and wheel odometry this is how my launch file.

<?xml version="1.0"?>
<!-- -->
<launch>

    <!--
    Hand-held 3D lidar mapping example using a Velodyne PUCK, an external IMU and color camera (using D435i as example).
    Prerequisities: rtabmap should be built with libpointmatcher
    We use D435i imu only for lidar deskewing and icp_odometry guess in this example.
    Example:
     $ roslaunch rtabmap_examples test_velodyne_d435i_deskewing.launch
     $ rosrun rviz rviz -f map
     $ Show TF and /rtabmap/cloud_map topics
    -->

    <arg name="rtabmap_viz"   default="true"/>
    <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
    <arg name="deskewing"     default="false"/>
    <arg name="slerp"         default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
    <arg name="scan_topic"    default="points"/>
    <arg name="odom_topic"    default="ackermann_steering_controller/odom"/>
    <arg name="odom_frame_id"    default="odom"/>
    <arg name="use_sim_time"  default="false"/>
    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>

    <arg name="frame_id"        default="velodyne2"/>       <!-- base frame of the robot: for this example, we use velodyne as base frame -->
    <arg name="queue_size"      default="50"/>
    <arg name="queue_size_odom" default="10"/>
    <arg name="loop_ratio"      default="0.2"/>

    <arg name="resolution"         default="0.4"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
    <arg name="iterations"         default="10"/>

    <!-- Grid parameters -->
    <arg name="ground_is_obstacle" default="true"/>
    <arg name="grid_max_range"     default="20"/>

    <!-- For F2M Odometry -->
    <arg name="ground_normals_up" default="true"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
    <arg name="local_map_size"    default="15000"/>
    <arg name="key_frame_thr"     default="0.4"/>

    <!-- For FLOAM Odometry -->
    <arg name="floam"    default="false"/> <!-- 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="1"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->

    <!-- Static transform between velodyne and D435i: TODO: Adjust with real position/orientation!!! -->
    <node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="velodyne2_to_camera_tf" args="0 0.018 -0.103 0 0 0 velodyne2 camera_link 100"/>

    <group ns="steer_bot">
      <node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="$(arg scan_topic)"/>
        <remap from="odom"            to="icp_odom"/>
        <param name="guess_frame_id"  type="string" value=""/>
        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
        <param name="wait_for_transform_duration" type="double" value="0.2"/>
        <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"/>

        <!-- 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/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      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="$(arg key_frame_thr)"/>
        <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="$(arg local_map_size)"/>
        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
      </node>

       <!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

        <param name="frequency" value="50"/>
        <param name="sensor_timeout" value="0.1"/>
        <param name="two_d_mode" value="false"/>

        <param name="odom_frame" value="$(arg odom_frame_id)"/>
        <param name="base_link_frame" value="$(arg frame_id)"/>
        <param name="world_frame" value="$(arg odom_frame_id)"/>

        <param name="transform_time_offset" value="0.0"/>

        <param name="odom0" value="$(arg odom_topic)"/>
        <param name="odom1" value="icp_odom"/>

        <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
        <rosparam param="odom0_config">[true, true, true,
                                        true, true, true,
                                        true, true, true,
                                        true, true, true,
                                        true, true, true]</rosparam>

        <rosparam param="odom1_config">[
                                    true, true, true,
                                    true,  true,  true,
                                    true, true, true,
                                    true,  true,  true,
                                    true,  true,  true] </rosparam>

        <param name="odom0_differential" value="false"/>
        <param name="odom1_differential" value="false"/>

        <param name="odom0_relative" value="true"/>
        <param name="odom1_relative" value="true"/>

        <param name="print_diagnostics" value="true"/>

        <!-- ======== ADVANCED PARAMETERS ======== -->
        <param name="odom0_queue_size" value="50"/>
        <param name="odom1_queue_size" value="5"/>

        <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
            vroll, vpitch, vyaw, ax, ay, az. -->
        <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                    0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                    0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                    0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                    0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                    0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                    0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>

        <!-- The values are ordered as x, y,
            z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
            <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                            0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                            0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                            0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                            0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                            0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                            0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                            0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                            0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                            0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

      </node>

      <node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="true"/>
        <param name="wait_for_transform_duration" type="double" value="0.2"/>

        <remap from="odom" to="odometry/filtered"/>
        <remap from="scan_cloud" to="assembled_cloud"/>
        <remap from="rgb/image" to="camera/color/image_raw"/>
        <remap from="rgb/camera_info" to="camera/color/camera_info"/>

        <!-- 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="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="$(arg ground_is_obstacle)"/>
        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>

        <!-- ICP parameters -->
        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
        <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/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
      </node>

      <node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync" type="bool" value="true"/>
        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
        <remap from="odom_info" to="odom_info"/>
      </node>

      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_util/point_cloud_assembler" output="screen">
        <remap from="cloud"           to="$(arg scan_topic)"/>
        <remap from="odom"            to="icp_odom"/>
        <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
        <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
        <param name="fixed_frame_id"  type="string" value="" />
        <param name="queue_size"      type="int"    value="$(arg queue_size)" />
      </node>
  </group>

</launch>

The point cloud map is correct and the localization path is correct but the grid map is not correct. This is a screenshot of the output gridmap on rviz:

I know for fact I am setting some parameters wrong can you help me with this.

[matlabbe]

If ground_is_obstacle is true, everything will be black (i.e., obstacles).

[AhmedTM]

Yes thanks I noticed that and fixed it and I am still getting a very bad grid map I have added map_always_update to true it got a little better but still bad when I use teleop at rotation the odometry fails and messes up the cloud map and the grid map. The below images are my best trials the first one is with lidar odomtery only I did a lot of rotations and the odometry was not bad the second one was with robot_localization ekf and I didn't make any turns. Do you have any advice to improve the result.

[matlabbe]

For issues with robot_localization, you may ask on their project page. If odometry produced by icp_odometry is not akay, then maybe I can help. If you have a recording of the lidar messages points, I could take a look to see at least if icp_odometry is working as expected.

For the grid, is there an issue with position error or that the empty space is not filled enough with empty cells?

[AhmedTM]

Sorry for not being clear I have stopped robot_localization package and I was using icp_odometry only at this tests I also added Grid/MaxGroundHeight and Grid/MaxObstacleHeight parameters for rtabmap_slam to get a better grid and the result is better but I still have an issue with the position. Sometimes it feels like the map is not fixed although I am using map as the fixed frame in rviz. Here is a bag file recorded from the lidar data I am using a gazebo simulation environment the lidar is HDL32E and I limited its range to 180 degree because the back of the cloud have some problems. So, you will only find the 180 degree range of the cloud in the bag file. If you want me to record a full bag with the camera and odometry and the lidar please tell me. Appreciate your help. lidar bag

[matlabbe]

After 27-28 sec into the bag, there is no geometry complexity, all points are on the same plane:

icp_odometry will think the robot is not moving during that time or may do very bad drift.

[AhmedTM]

Yes, I can see it there is a part in the road where there is not geometric complexity in the FOV of the LiDAR sensor since it's an outdoor environment. That's why I was trying to fuse wheel odometry with LiDAR to somehow fix that but now I can see a new node in my rtabmap build that is called rgbdicp_odom but I can't find its documentation could this kind of fusion fix this issue ?

[matlabbe]

Can you give a screenshot of the simulated environment?

Here are some common challenges that simulated environments can have:

1. Low geometry complexity: making difficult for ICP-based odometry estimation to work properly (issue discussed above)
2. Low visual texture: making difficult to track visual features for visual odometry approach
3. Repeating exactly the same texture in the environment: making difficult for visual loop closure detection / re-localization (mainly for wrong localization)

If your simulated environment checks two or more of them at the same time, testing visual and/or lidar SLAM approaches may be even more difficult in the simulator than in reality. For example, if you have 1 and 2 at the same time, visual odometry won't help, lidar odometry won't help, thus you would need to rely only on wheel odometry. In your case I wouldn't use rgbdicp_odometry but try rgbd_odometry and icp_odometry in parallel to your wheel odometry. As you are targeting outdoor environments, you may try something like this (assuming wheel odometry is publishing on odom frame):

• rgbd_odometry:
  • guess_frame_id=odom
  • odom_frame_id=vo
  • Odom/ResetCountdown=1
• icp_odometry:
  • guess_frame_id=vo
  • odom_frame_id=icp_odom
  • Odom/ResetCountdown=1

You may need to adjust some ICP parameters to avoid icp_odometry to jump to out of the input guess. Also remap output odometry topics. Using guess_frame_id like this, when one of the odometry is lost, it will still republish the input guess on TF so that the following TF tree is never broken:

icp_odom -> vo -> odom -> base_link