Increased update time odom on the last rtabmap version

[csvicbot-7]

Hi @matlabbe

I recently updated the version of rtabmap and rtabmap_ros to the latest (master branch), I tested the new version with two LIDARs Ouster (OS0-64-U) and Robosense (RS Helios-5515), in both cases I noticed that the update time of the odometry increases, and the frequency of publication of the topic rtabmap/odom decreases, as the robot moves. I explain the details in each case:

Ouster-OS64: The update time starts at 0.04s and grows to 0.2s, the frequency of the topic rtabmap/odom starts at 10Hz and drops to 8.5Hz.

Robosense: The update time starts at 0.03s and grows to 0.26s, the frequency of the rtabmap/odom topic starts at 10Hz and drops to 6.8Hz.

Additional details:

Ouster-OS64: When I use this sensor with version 0.20.21-noetic of rtabmap and 0.20.20.20-noetic of rtabmap_ros, the rtabmap/odom topic is published at 10hz and is stable.

Robosense: This sensor has no integrated IMU, for this case I am using an external IMU (inertial sense).

In the following link I attach the rosbag and log files for each case:

https://drive.google.com/drive/folders/18Qfrum7ZP_KWAjOKjKyRJQxLtCXmX2gk?usp=sharing

Thank you in advance for your help.

[matlabbe]

The detection rate of rtabmap node is 4 Hz and its processing time is around 600-700 ms, which means that node is using a lot of CPU that could have been used by odometry to keep its FPS. Without the rtabmap's debug log or database with included statistics on your computer, it is difficult to know where most of the processing is used. With 3D lidar sensors, one thing that could increase significantly CPU usage by rtabmap node is the occupancy grid config. I see in your case that RGBD/CreateOccupancyGrid is true. If you don't need an occupancy grid, I'll suggest to turn it off. I'll also suggest not setting 4Hz detection rate, but stick with the 1 Hz. If you need to record all scans, look the assemble option in test_ouster_gen2.launch example.

On the why it "seems" using less CPU on older version, some default parameters may have changed between the versions. We could compare parameters between the resulting databases:

rtabmap-info --diff map_old_version.db map_new_version.db

If you can share the databases with old and new versions, I could take a look.

I tested your two bags with test_ouster_gen2.launch like this:

# ouster bag:
roslaunch rtabmap_examples test_ouster_gen2.launch use_sim_time:=true imu_topic:=/os_cloud_node/imu/data frame_id:=base_link wait_for_transform:=0.01
# have to republish the static transform to avoid most extrapolation errors discussed below
rosrun tf static_transform_publisher 0.35 -0.2 1.15 0 0 0 base_link os_sensor 100
rosrun tf static_transform_publisher 0.006253 -0.011775 0.007645 0 0 0 os_sensor os_imu 100
rosrun tf static_transform_publisher 0 0 0.036180000000000004 1.57 0 0 os_sensor os_lidar 100
rosbag play --clock ouster.bag /os_cloud_node/imu/data:=/os_cloud_node/imu/data/filtered tf:=tf_old

# robosense bag:
roslaunch rtabmap_examples test_ouster_gen2.launch use_sim_time:=true imu_topic:=/inertial_sense/imu/data frame_id:=base_link scan_topic:=/rslidar_points wait_for_transform:=0.01
rosnode kill /imu_nodelet_manager
rosbag play --clock robosense.bag /inertial_sense/imu/data:=/inertial_sense/imu/data/filtered

The processing time on odometry would increase till the maximum local scan map size is reached (15000 in that example). I get around 55 ms processing time for ouster scans and 34 ms for the robosense scans. rtabmap node is around 50-60 ms.

I don't know if it is a recording issue, but sometimes the following would happen with the ouster bag:

[ERROR] [1702168865.577801095, 1700843213.959358158]: Could not get transform from base_link 
to os_imu after 0.100000 seconds (for stamp=1700843213.958758)! Error="Lookup would require 
extrapolation 0.009224629s into the future.  Requested time 1700843213.958757639 but the latest 
data is at time 1700843213.949532986, when looking up transform from frame [os_imu] to frame
[base_link]. canTransform returned after 0.0100525 timeout was 0.01."

This will make the cloud skewed:

When deskewing works correctly:

For robotsense bag, I think the imu is poorly synchronized with the lidar as there is no really difference with and without deskewing:

• deskewing:=false:
• deskewing:=true:

[csvicbot-7]

Hello @matlabbe, thank you very much for your reply.

I will try to share the databases of the new and old version as soon as I can.

For the moment, I share a new robosense bag with the database (rtabmap version 0.21.1). https://drive.google.com/drive/folders/18Qfrum7ZP_KWAjOKjKyRJQxLtCXmX2gk?usp=sharing

I hope it can help you to know where most of the processing is used.

I have seen that although the robot displacement is only in X and Y axis, the base_link and base_link_stabilized frame are displaced in the Z axis. Also, the odom frame is displaced 25cm with respect to the map frame. Could you tell me why this is happening?

I attach some images:

On the other hand, can you explain me what the deskewing is about?

[matlabbe]

The imu looks indeed not synchronized with the lidar timestamps. Could the IMU clock / GPS connected to lidar so it can use it as timestamp? Here showing the raw lidar in base_link_stabilized, note that the walls should not tilt (roll/pitch) if they were perfectly synchronized:

Discarding the IMU, here some tests with only the lidar. For convenience, I just ignored the /tf included in the bag and work with lidar frame directly. I used test_velodyne.launch for testing. The only change I did is to add the following under point_cloud_assembler node:

<param name="range_min"  type="double" value="1.5" />

Usage:

roslaunch rtabmap_examples test_velodyne.launch scan_topic:=/rslidar_points frame_id:=rslidar use_sim_time:=true resolution:=0.1
rosbag play --clock --pause robosense_2.bag tf:=tf_ignored /rtabmap/odom:=/rtabmap/odom_ignored

I also test with and without forcing 3DoF constraints, in case you want to do only 2D slam. To do so add the following under icp_odometry and rtabmap nodes:

<param name="Reg/Force3DoF" type="string" value="true"/>

The big difference is that as the lidar is not perfectly moving in 2D, the ground (in blue) is not as straight when forcing 3DoF: In 6DoF:

Here are some timings on my computer (I am using latest version of rtabmap):

From your database, you are using those parameters:

Reg/Force3DoF=false
Grid/ClusterRadius=2
Rtabmap/DetectionRate=4
RGBD/LocalRadius=1000
RGBD/NeighborLinkRefining=true
Icp/PointToPlaneK=50
Icp/RangeMin=1.5

I'll suggest to use the following instead to save computation time:

Reg/Force3DoF=true
Grid/NormalsSegmentation=false
Grid/MinGroundHeight=0.05
Grid/RangeMax=5 (default)
Grid/RayTracing=true
Grid/3D=false
Rtabmap/DetectionRate=1 (default)
RGBD/LocalRadius=10 (default)
RGBD/NeighborLinkRefining=false (default)
Icp/PointToPlaneK=10
Icp/RangeMin=0.5

For theory about lidar deskewing, I'll suggest this paper showing extreme cases: Lidar Scan Registration Robust to Extreme Motions