Wheel odometry correction by icp_odometry node

[Divelix]

Hi Mathieu,

I want to use my wheel odometry topic as guess for icp_odometry node, but existing examples seems kind of messed/outdated (e.g. numerous warnings like "use scan_normal_radius instead of Icp/PointToPlaneRadius" and others) for me, so could you please check if I use these nodes right.

My idea was to pass /wheel_odom to icp_odometry, which will output more precise odomery in /odom topic and /odom would be input odometry to rtabmap node.

It seems kind of work via my launch file listed below, but:
1) I get errors: [ERROR] Rtabmap.cpp:1258::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 137 is ignored!
2) My wheel odometry is 10 Hz, but output /odom is less than 5 Hz and unstable (can not publish at all for few seconds in a row).
3) Why it forbids me to use "Icp/PointToPlaneRadius" and "Icp/PointToPlaneK" at the same time, while your example use them both?
4) Does icp_odometry and rtabmap nodes share the same "Icp/..." parameters? Other parameters too? Because logs warnings complained that I have "scan_voxel_size" in icp_odometry and "Icp/VoxelSize" in rtabmap at the same time.

<launch>
    <!-- Node that controls hardware, including L515 -->
    <node>...</node>
    
    
    <!-- Node that puplishes /wheel_odom -->
    <node>...</node>

    <arg name="rtabmapviz"       value="false"/>
    <arg name="rviz"             value="false"/>
    <arg name="visual_odometry"  value="false"/>
    <arg name="icp_odometry"     value="true"/>
    <arg name="rtabmap_db_path"  value="$(env PATH_TO_STORAGE)/rtabmap/rtabmap.db"/>
    <arg name="scan_cloud_topic" value="/frontal_camera/depth/color/points"/>

    <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="log" >
        <remap from="scan_cloud"         to="/frontal_camera/depth/color/points"/>
        <param name="frame_id"           type="string" value="base_footprint"/>
        <param name="odom_frame_id"      type="string" value="/odom"/>
        <param name="guess_frame_id"     type="string" value="/wheel_odom"/>
        <param name="scan_voxel_size"    type="string" value="0.05"/>
        <param name="scan_normal_k"      type="string" value="5"/>
        
        <param name="Icp/RangeMax"                  type="string" value="0"/>
        <param name="Icp/Epsilon"                   type="string" value="0.001"/>
        <param name="Icp/PointToPlane"              type="string" value="true"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
        <param name="Icp/PointToPlaneRadius"        type="string" value="0"/>
        <param name="Icp/Strategy"                  type="string" value="1"/>
        <param name="Icp/OutlierRatio"              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>

    <!-- RTAB-Map -->
    <group ns="rtabmap">
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
            <remap from="rgb/image"        to="/frontal_camera/color/image_raw"/>
            <remap from="depth/image"      to="/frontal_camera/aligned_depth_to_color/image_raw"/>
            <remap from="rgb/camera_info"  to="/frontal_camera/color/camera_info"/>
            <param name="approx_sync"      value="true"/> 
            <param name="queue_size"       value="10"/>
        </node>

        <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="delete_db_on_start">
            <param name="database_path"        type="string" value="$(arg rtabmap_db_path)"/>
            <param name="frame_id"             type="string" value="base_footprint"/>

            <param name="subscribe_rgbd"       type="bool"   value="true"/>
            <param name="subscribe_rgb"        type="bool"   value="false"/>
            <param name="subscribe_depth"      type="bool"   value="false"/>
            <param name="subscribe_scan_cloud" type="bool"   value="true"/>

            <param name="approx_sync"          type="bool"   value="true"/>

            <!-- inputs -->
            <remap from="scan_cloud"           to="/frontal_camera/depth/color/points"/>
            <remap from="odom"                 to="/odom"/>
            
            <!-- RTAB-Map's parameters -->
            <param name="Reg/Strategy"                   type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
            <param name="Reg/Force3DoF"                  type="string" value="true"/>
            <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
            <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>
            <param name="Icp/CorrespondenceRatio"        type="string" value="0.2"/>
            <param name="Icp/RangeMax"                   type="string" value="0"/>
            <param name="Grid/RangeMax"                  type="string" value="0"/>
            <param name="Grid/Sensor"                    type="string" value="0"/>
        </node>
    </group>
</launch>

[matlabbe]

Hi,

For your questions:

1) Looks like the received odometry is null
2) Which kind of camera do you use (is it L515 as in the top comment)? icp_odometry won't work well with camera point clouds unless the depth camera is a TOF camera (very accurate point clouds) and the field of view is high (icp will diverge quite fast if you are looking only at a flat wall). If /odom is not published for a while, check warnings or errors telling if the odometry got lost. 5 Hz is indeed slow, what is the framerate of "/frontal_camera/depth/color/points"? You may increase scan_voxel_size to 0.2 or 0.3. To use icp_odometry, make also sure that you have rtabmap built with libpointmatcher (rtabmap --version).
3) PCL doesn't support both parameters at the same time to compute normals for 3D point clouds. In the example you referred, it used a LaserScan (for which they can be used both at the same time in 2D), not PointCloud2.
4) Icp/* parameters can be used for both icp_odometry and rtabmap nodes. For odometry, you may get warnings telling you it converted to ros parameters for convenience.

I have other examples here using icp_odometry with wheel odometry as guess for 3D Lidar: https://github.com/introlab/rtabmap_ros/blob/fa53c7c0009fd61c6c7bea7ef1489b11d471304e/launch/demo/demo_husky.launch#L27
However, Icp parameters are tuned for a 3D lidar, not a TOF camera. For ICP parameters for TOF camera, you may check this post.

cheers,
Mathieu

[Divelix]

Yes, I use pointcloud from L515 (stable 30 fps). Rtabmap built with libpointmatcher, yes.

I didn't quite understand your point about TOF cameras:
1) As far as I know, TOF cameras have not really high field of view
2) L515 is not a TOF camera but solid-state lidar, do you consider it not accurate enough to use TOF configuration from your example?
3) What parameters are better to use for icp_odometry+rtabmap via L515?

And also, what does scan_voxel_size do? Is it some kind of decimation to sparse point cloud?
Thanks for your answers

UPD:
In your reference for L515 parameters I noticed that you enable IMU and launch Madgwick filter. Does RTAB-Map itself (without third party nodes/nodelets) able to use IMU data (because in one of your old answers you adviced to use robot-localization package for IMU)?
And also, why do you use "point_cloud_xyz" nodelet instead of L515 built-in pointcloud filter?

[matlabbe]

Hi,

I consider LiDAR camera and TOF camera the same things (they are both Time-Of-flight range detection). When I say TOF/LiDAR, it is to differentiate from RGB-D camera based on Structured-Light (Kinect for xbox360) or Stereo IR (realsense D400 series), which are not very good for ICP (particularly those based on stereo).

Kinect for Azure have slightly larger field of view than L515, so it is more robust in general.

For L515 parameters based on ICP, I would start with the referred post in my previous answer. The post you referred is from 2016, rtabmap is an ongoing project, so yes it can use IMU now (in a loosely coupled manner). The IMU should have the quaternion already computed though, so it is why we rely on Madgwick filter to compute it. scan_voxel_size is an uniform downsampling filter, it applies the pcl::VoxelGrid filter before doing the estimation. IMU will help in general to get better ICP, as the IMU orientation is used to have a better prior guess for scan matching. The map will also be automatically aligned with gravity.

point_cloud_xyz is used to create the point cloud faster, as we decimate the image x4 when creating it.

cheers,
Mathieu

[Divelix]

How does rtabmap use IMU exactly? Is there any docs about it?
When I try to use code from your example, I get these logs:
[ WARN] [1645786758.165136718]: odometry: Could not get transform from base_footprint to frontal_camera_imu_optical_frame (stamp=1645786757.766360) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="Could not find a connection between 'base_footprint' and 'frontal_camera_imu_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.200665 timeout was 0.2."
[ERROR] [1645786758.165176262]: Could not transform IMU msg from frame "frontal_camera_imu_optical_frame" to frame "base_footprint", TF not available at time 1645786757.766360

I tried to find cause of the problem and found out that tf loses transform between gyro frame and imu optical frame when I turn Madgwick filter node on. Do you have any ideas why this could happen? Here are tf trees for IMU without and with Madgwick filter: Just IMU

IMU with madgwick filter.

[Divelix]

Also, when I run your example, I get these warnings/errors:
[ WARN] (2022-02-28 08:26:41.030) OdometryF2M.cpp:529::computeTransform() Registration failed: "Cannot compute transform (cor=1312 corrRatio=0.168616/0.200000 maxLaserScans=7781)" (guess=xyz=0.000000,0.000000,0.000000 rpy=-0.000500,-0.000189,-0.000246)
[ WARN] [1646036801.094962627]: We are receiving imu data (buffer=8), but cannot interpolate imu transform at time 1646036800.691414. IMU won't be added to graph.
[ERROR] (2022-02-28 08:26:41.095) Rtabmap.cpp:1258::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored!
[ERROR] [1646036802.420825463]: Overwriting previous data! Make sure IMU is published faster than data rate. (last image stamp buffered=1646036801.951970 and new one is 1646036801.985143, last imu stamp received=1646036801.950938)
[ERROR] (2022-02-28 08:26:42.514) Rtabmap.cpp:1258::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 41 is ignored!

[Divelix]

I found solution: there was a mistake in my static transform between base_link and base_footprint. I didn't know tf/tf2 can't do transforms up the tree, so it is crucial to make base_footprint parent of base_link.

[matlabbe]

Hi,

For the odom->frontal_camera_imu_optical_frame published by imu_filter, make sure you start the imu filter node with " _publish_tf:=false" like in my example.

For "Registration failed: "Cannot compute transform (cor=1312 corrRatio=0.168616/0.200000 maxLaserScans=7781)" (guess=xyz=0.000000,0.000000,0.000000 rpy=-0.000500,-0.000189,-0.000246)", It is because I set "--Icp/CorrespondenceRatio 0.2" relatively high to stop odometry when there is not so much overlap between consecutive frames.