Challenges on odometry when the street has high slopes.

[Mikor]

Hi Mathieu,

It's me again. This time I would like your help with something. I have recorded pointcloud, odometry and imu messages from a vlp-16 and a T265. The sensors were mounted on a car and the recording lasts 10 minutes. It seems that the topics coming from the T265 drift too much so I keep trying to do the mapping only with the vlp. So far so good, the problem is that the area is not flat, and the icp odometry goes wild and I'd like to get some tips from you if possible for what to check or not. Here is the bag file. I thought you might be interested to take a look at the data, it's a fairly big dataset.

(This is a questionable tf for the imu and the

velodyne rosrun tf2_ros static_transform_publisher 0 0 15 -0.450711 -0.3966168 -0.4002457 0.6923569 imu_link velodyne

 in case you use the T265 sensor )

[matlabbe]

Hi Anthony,

This was a nice rosbag to test, thx!

I started from the parameters from this post, then I moved to point to plane registration and decreased voxel size (also added option to assemble ALL velodyne point clouds):

roslaunch rtabmap_ros rtabmap.launch    \
   use_sim_time:=true \
   depth:=false \
   subscribe_scan_cloud:=true \
   frame_id:=base_link \
   scan_cloud_topic:=/velodyne_points_half \
   scan_cloud_max_points:=15000  \
   icp_odometry:=true \
   approx_sync:=false \
   scan_cloud_assembling:=true \
   scan_cloud_assembling_voxel_size:=0 \
   args:="-d  \
      --RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 0 \
      --Odom/ScanKeyFrameThr 0.5 \
      --OdomF2M/ScanMaxSize 30000  \
      --OdomF2M/ScanSubtractRadius 0.3   \
      --Icp/PM true \
      --Icp/VoxelSize 0.3   \
      --Icp/MaxTranslation 2   \
      --Icp/MaxCorrespondenceDistance 1 \
      --Icp/PMOutlierRatio 0.7 \
      --Icp/Iterations 10 \
      --Icp/PointToPlane true \
      --Icp/PMMatcherKnn 3 \
      --Icp/PMMatcherEpsilon 1 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 10 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01 \
      --Icp/PointToPlaneGroundNormalsUp 0.3"

I noticed that the velodyne clouds were doubled (easier to see when the car moved faster), reducing ICP accuracy. I think the velodyne was turning at 20 Hz (1200 RPM) but the velodyne_point_cloud node was started at 600 rpm, thus accumulating two rotations in single point cloud. I used this script to remove one rotation:

 #!/usr/bin/env python
import rospy
import math
from sensor_msgs.msg import PointCloud2

def callback(cloud):
    global cloud_pub;
    
    cloud.width = math.ceil(cloud.width/2)
    cloud.row_step = cloud.width * cloud.point_step
    cloud.data = cloud.data[0:cloud.row_step]
                   
    cloud_pub.publish(cloud)

if __name__ == '__main__':
    global cloud_pub
    
    rospy.init_node('scan_cut_half', anonymous=True)
    
    cloud_pub = rospy.Publisher('/velodyne_points_half', PointCloud2, queue_size=1)
    cloud_sub = rospy.Subscriber('/velodyne_points', PointCloud2, callback)

    rospy.spin()

I guessed the velodyne orientation on the car (I think your 15 meters transform between imu and velodyne was wrong):

rosrun tf2_ros static_transform_publisher 0 0 1.7 -0.6 0.35 -0.15 base_link velodyne

Then I started the bag:

rosbag play --clock vlp_t265_capture_1.bag

You may get something like this (there is a large gap between the beginning and end position, cause by some orientation errors):


The red links are the loop closures I added (here after optimization):


I added manually the loop closures like this, from two places we know they are the same, traversed in different directions:

Click add, then add manually a transform with 180 deg yaw:


Manually adjust the transform ([...] button) and hit refine:


Here are some pictures after exporting at high resolution:









Note that I added a footprint option in this commit to remove the car. In the same commit, I added a new implementation of voxel filter to use small voxel values on large maps, automatically split the map to avoid overflow in pcl::VoxelGrid filter.

The footprint length is large enough to remove also the car that was following.

Final thoughts: Having an IMU that we know exactly its orientation against the velodyne would help. I wasn't sure of the imu orientation with the velodyne to use it. The lidar orientation is nice to get more data in vertical, but I think icp_odometry would work better if it was horizontal (to see the road both in front and back of the vehicle), this will help with orientation estimation. You could play with the voxel size (and subtract radius) and ScanKeyFrameThr to see if it can somewhat less drift. Actually, with IMU and gravity optimization, it would have helped with the pitch/roll error.

cheers,
Mathieu

[Mikor]

Thank you so much for taking the time, always there is something new to learn from your posts. Have a nice weekend.

[Mikor]

Hi Mathieu, I just noticed this and it seems very promising. Do you think I could use it in ros melodic? Also I have one more question and this is about scan_cloud_assembling and the scan_cloud_assembling_voxel, what do they do?

As always, thank you in advance.

[matlabbe]

Hi Anthony,

cmr_lidarloop can be used to detect loop closures using only lidar. However, with how your lidar is tilted, not sure if it will be able to detect loop closures in different directions. The last loop closure when coming back to beginning may be detected.

Th code was tested on kinetic (python2), it could then work on melodic. For Noetic, it is more if scikit-learn python3 version is compatible with the code.

When scan_cloud_assembling is true, a node rtabmap_ros/point_cloud_assembler is created to accumulate scans before sending the assembled point cloud to rtabmap node (to avoid setting Rtabmap/DetectionRate low to process every scans, and thus would use more CPU for loop closure and graph optimization). scan_cloud_assembling_voxel_size tells that we don't want point_cloud_assembler to voxelize the assembled point cloud (the launch sets it to default 5 cm). There is also an argument scan_cloud_assembling_time that is set to default 1 second, so that the node is accumulating 10 point clouds (10Hz) during 1 second. As the assembled point cloud is published at 1 Hz, we also disabled Rtabmap/DetectionRate=0 to process all them (at 1Hz). For example, scan_cloud_assembling_time could be set to 0.5 if we want new nodes added to rtabmap at 2Hz.

cheers,
Mathieu