TIps on increasing the accuracy and robustness of icp odometry

[dinossht]

I am using Livox Mid360 lidar with icp odometry and slam approach.

Here is my current launch file. I experience using external odometry approach such as using fastlio leads to better mapping. But is there any potential parameter tuning I can do to improve the accuracy and robustness (meaning not easily losing tracking). If computation is not a factor here, what would you recommend tuning.

[dinossht]

Here is my config

[matlabbe]

Icp/MaxCorrespondenceDistance and OdomF2M/ScanSubtractRadius can be increased approximately 10-15 times the voxel size. I generally use voxel size of 20-30 cm indoor, and >=50 cm outdoor. Deskewing with IMU is a must if the lidar is not already deskewed. Make sure the IMU to lidar transform is accurate (and lidar clock is sync with computer/imu clock).

Icp/CorrespondenceRatio of 0.2 for odometry is high, if you don't want rejecting odom when switching from large environment to small environment, set it very low. On a drone, we set it to 0.01 because we preferred having a bad drift than no odometry at all. However, for rtabmap node, keeping it high like 0.2 can help to avoid closing loop closures on very small overlaps (which most of the times is wrong).

Icp/MaxTranslation can be increased too, if there are some lags for some reason, it may happen that the transform between two scans would be over 20 cm.

EDIT: we can also set Icp/PointToPlaneGroundNormalsUp=0.9 when the robot is moving mostly on flat ground (e.g. car), that helps to keep normals up for the ground when the lidar rays are far from each other.

With 3D lidar, I tend to increase OdomF2M/ScanMaxSize=15000 or higher by default to have a bigger local scan map.

[dinossht]

Thank you Mathieu. Seems like increasing ScanSubtractRadius helps a wrt. accuacy! When I enable deskew, the odometry does not work. Have you experienced this?


And what does this do exactly: PointToPlaneGroundNormalsUp? If I walk with the lidar sensor, does this apply as well?

[matlabbe]

dinossht wrote

When I enable deskew, the odometry does not work. Have you experienced this?

What kind of warnings/errors do you have? Note however that I am note sure if the timestamps set in livox point cloud are as expected (I tested mostly on velodyne and ouster lidars). If you can share a small rosbag of livox point cloud and imu, that would be useful to check if the format is as expected.

dinossht wrote

And what does this do exactly: PointToPlaneGroundNormalsUp? If I walk with the lidar sensor, does this apply as well?

If the lidar is always held upwards, that could be used. When computing normals of point on the ground, if the distance between two rays is too large, the estimated normal can be upside down. That option will flip all normals upward if they are close to 0.9*-z axis. This is all done in base frame, so it is why the lidar is rotated, it won't work properly. I mostly enable this on ground robots or cars with a lidar fixed upside up.

[dinossht]

Thank you for the explanation on PointToPlaneGroundNormalsUp. Here is the bad with lidar and imu:livox.bag. No warnings or anything is displayed, so it is a bit hard to debug unfortunately.

[matlabbe]

I am currently seeing that error:

terminate called after throwing an instance of 'std::runtime_error'
  what():  Time is out of dual 32-bit range

and icp_odometry dies. Here the gdb backtrace:

#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff6937859 in __GI_abort () at abort.c:79
#2  0x00007ffff6bc08d1 in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff6bcc37c in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff6bcc3e7 in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff6bcc699 in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007fffe9416cac in ros::TimeBase<ros::Time, ros::Duration>::fromSec(double) () at /opt/ros/noetic/lib/librostime.so
#7  0x00007fffaec78a9a in ros::Time::Time(double) () at /catkin_ws/devel/lib//librtabmap_odom_plugins.so
#8  0x00007fffd2357324 in rtabmap_conversions::deskew_impl(sensor_msgs::PointCloud2_<std::allocator<void> > const&, sensor_msgs::PointCloud2_<std::allocator<void> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, tf::TransformListener*, double, bool, rtabmap::Transform const&, double) () at /catkin_ws/devel/lib/librtabmap_conversions.so
#9  0x00007fffd2359d5b in rtabmap_conversions::deskew(sensor_msgs::PointCloud2_<std::allocator<void> > const&, sensor_msgs::PointCloud2_<std::allocator<void> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, tf::TransformListener&, double, bool) ()
    at /catkin_ws/devel/lib/librtabmap_conversions.so
#10 0x00007fffaee5e788 in rtabmap_odom::ICPOdometry::callbackCloud(boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&) ()
    at /catkin_ws/devel/lib//librtabmap_odom_plugins.so
#11 0x00007fffaee7e5db in boost::_mfi::mf1<void, rtabmap_odom::ICPOdometry, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>::operator()(rtabmap_odom::ICPOdometry*, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&) const ()
    at /catkin_ws/devel/lib//librtabmap_odom_plugins.so
#12 0x00007fffaee7c6df in void boost::_bi::list2<boost::_bi::value<rtabmap_odom::ICPOdometry*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, rtabmap_odom::ICPOdometry, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>, boost::_bi::rrlist1<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&> >(boost::_bi::type<void>, boost::_mfi::mf1<void, rtabmap_odom::ICPOdometry, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>&, boost::_bi::rrlist1<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>&, int) () at /catkin_ws/devel/lib//librtabmap_odom_plugins.so

The lidar data's timestamp field is data type 8 (double):

fields: 
  - 
    name: "timestamp"
    offset: 18
    datatype: 8
    count: 1

The timestamp values look like this:

First: 1732526855256824064.000000
Last: 1732526855357080320.000000

Topic header stamp is:
header: 
  seq: 5653
  stamp: 
    secs: 1732526855
    nsecs: 256824017
  frame_id: "livox_frame"

The stamps are in nano seconds (10^-9), and are causing issue with this conversion (as we assume the double value is in seconds). ros::Time will assert if the number of seconds is over max integer (32 bits) here.

I think when I added the original code to support FLOAT64 type, the point cloud I tested was coming from a Hesai lidar and the timestamps were in seconds, not nanoseconds. The bag is too short to see if it really worked, but I pushed a fix here: https://github.com/introlab/rtabmap_ros/commit/e5667b280a516e1b610438d7985db67678016b33 (EDIT: also need this commit)

cheers,
Mathieu

[dinossht]

I finally got to test your fixes, and deskewing seems to work with livox lidar!

[matlabbe]

Great, good to now!