search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1.01k stars 558 forks source link

Slow ICP odometry with high CPU usage (100%) #1167

Closed matlabbe closed 6 months ago

matlabbe commented 6 months ago

On some computers (tested on fresh Ubuntu 22.04/Humble), just launching:

ros2 run rtabmap_odom icp_odometry

results in 100% CPU usage: Screenshot from 2024-06-02 10-21-42

This is for a small scan topic (~300 points):

[1717348896.008828259]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.089698s
[1717348896.026145271]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.016670s
[1717348896.072244073]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.045455s
[1717348896.107692264]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.034808s
[1717348896.128743754]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.020406s
[1717348896.157508123]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.007192s
[1717348896.739073287]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.550094s
[1717348896.791934761]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.046886s
[1717348897.565722656]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.768880s
[1717348898.011795343]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.445456s
[1717348898.041099381]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.028485s
[1717348898.066546062]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.024490s

It seems happening if rtabmap is built with libpointmatcher. As if we launch with PCL's ICP instead:

ros2 run rtabmap_odom icp_odometry --ros-args -p Icp/Strategy:='"0"' -p Icp/PointToPlane:='"false"'

[1717349261.355527224]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.002155s
[1717349261.369945024]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.001967s
[1717349261.426555224]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.001318s
[1717349261.455269735]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.001604s
[1717349261.519712844]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.001315s
[1717349261.548337305]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.001334s
[1717349261.594777437]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.001736s
[1717349261.618543310]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.001344s
matlabbe commented 6 months ago

The issue is related to default OpenMP waiting policy, with libpointmatcher built with OpenMP support. If we do:

OMP_WAIT_POLICY=passive ros2 run rtabmap_odom icp_odometry

Everything is fine:

[1717349566.822038882]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.003475s
[1717349566.841366103]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.003860s
[1717349566.855502721]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.003687s
[1717349566.878782602]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.003916s
[1717349566.889291191]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.003879s
[1717349566.909944525]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.004162s
[1717349566.919669341]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.004131s
[1717349566.944726090]: Odom: ratio=0.686275, std dev=0.000100m|0.000173rad, update time=0.004026s

Cpu before launching icp_odometry: Screenshot from 2024-06-02 10-33-49

Cpu usage after launching icp_odometry (there is no significant difference): Screenshot from 2024-06-02 10-34-21

I also saw that when using introlab3it/rtabmap_ros:humble or introlab3it/rtabmap_ros:humble-latest docker containers. We should add "-e OMP_WAIT_POLICY=passive" when launching the container.