Closed csvicbot-7 closed 1 year ago
What are the parameters used for icp_odometry (rosparam dump params.yaml
)? I cannot reproduce your issue on my side using:
roslaunch rtabmap_ros test_velodyne.launch
On rtabmap node side, I see also that your odometry is jumping around (over 5 cm) so your parameter RGBD/LinearUpdate
(at 0.05
) doesn't filter nodes added to map. The objects are not cleared in /rtabmap/mapData
topic, to get a 3D occupancy grid with ray tracing, you should subscribe to /rtabmap/octomap*
topics (and make sure Grid/RayTracing
is true.).
/rtabmap/MemoryThr
doesn't mean what you think (it limits the size of the local optimized graph of the map), and is not related to icp odometry.
If you can record 30 seconds of the lidar point cloud you are using in a rosbag, it would help to debug on my side. It seems to have a lot more rings than the lidar I can test on (VLP16), maybe the default parameters are too strict.
I started icp_odometry in valgrind to monitor the RAM usage, it seems not leaking (after 1 hour):
export ROS_NAMESPACE=rtabmap && \
valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=1000 \
~/catkin_ws/devel/lib/rtabmap_ros/icp_odometry scan_cloud:=/velodyne_points
On a previous test, I saw 0.7% RAM increase after 20 minutes in TOP (this computer has 32GB of RAM). However, I am not sure if it is just the system not reporting that memory has been released by the executable. For example, on another test I saw rtabmapviz increasing up to 1.6GB, but even after clearing the cache in the UI, TOP will still show the same RAM usage even if the memory has been actually freed.
Hi @matlabbe
We are using a Ouster Lidar sensor (OS-0-64-U13), it has 64 rings. I have recorded two bag files and they are available on the same link: https://drive.google.com/drive/folders/1PiFYCBn96PehjWbGftjm1exEAdY1TtVg?usp=share_link
In the folder you will also find the database corresponding to these bagfiles.
You mentioned something important about the odometry, why is it jumping around (over 5cm)? I think this problem can be better seen in the "2023-02-02-17-35-24.bag" file.
About the icp_odometry parameters, you will be able to see all the rtabmap parameters that we are using in the launch file ("ouster_neo.launch").
I really appreciate your help, thanks.
Hi,
this parameter is too high:
<param name="Odom/ScanKeyFrameThr" type="string" value="0.95"/>
It seems it is that value in my ouster examples, I would need to revise that. In the velodyne example, it is set to 0.6
. This will avoid the local odom map to be updated too often, in particular when someone passes in front of the sensor, and thus limit the drift of the sensor when not moving (meaning odom will be more stable now and remove the 5 cm jumps).
It also seems to limit the growth in RAM usage over time, as the local odom map is not updated as often. But normally updating the local odom map should bounded in memory, I think it is because the sensor is not moving, the old keyframes keep growing in numbers and are not flushed.
I checked in the code and looked at this log, and indeed the buffered scans just kept growing! Here if I set Odom/ScanKeyFrameThr=0.99
and after 48 sec of one of your bag:
[DEBUG] (2023-02-20 21:44:29.111) OdometryF2M.cpp:1011::computeTransform() scansBuffer=439, mapSize=10419 newPoints=1 maxPoints=15000
It kept 439 scans in buffer, just for 1 point added by the latest scans. Normally the buffer would clean up if the sensor is moving, or not grow if it is not moving, but Odom/ScanKeyFrameThr=0.99
is making the map updating while not moving. I'll try to fix that in coming days.
cheers, Mathieu
Updated default scan threshold in ouster examples: https://github.com/introlab/rtabmap_ros/commit/8cddf6d6ff9cc12c00c425bb855f6056c818b47c
Hi @matlabbe
I really appreciate your help, I tested setting Odom/ScanKeyFrameThr=0.8
and it works fine!, the RAM increase problem is solved.
I have a question, why is it necessary to create the odom frame?, why not use only the frame map?
I will be very grateful for your reply.
It is to comply to REP105: https://www.ros.org/reps/rep-0105.html
The coordinate frame called odom is a world-fixed frame. The pose of a mobile platform in the odom frame can drift over time, without any bounds. This drift makes the odom frame useless as a long-term global reference. However, the pose of a robot in the odom frame is guaranteed to be continuous, meaning that the pose of a mobile platform in the odom frame always evolves in a smooth way, without discrete jumps.
In a typical setup the odom frame is computed based on an odometry source, such as wheel odometry, visual odometry or an inertial measurement unit.
The odom frame is useful as an accurate, short-term local reference, but drift makes it a poor frame for long-term reference.
The coordinate frame called map is a world fixed frame, with its Z-axis pointing upwards. The pose of a mobile platform, relative to the map frame, should not significantly drift over time. The map frame is not continuous, meaning the pose of a mobile platform in the map frame can change in discrete jumps at any time.
In a typical setup, a localization component constantly re-computes the robot pose in the map frame based on sensor observations, therefore eliminating drift, but causing discrete jumps when new sensor information arrives.
The map frame is useful as a long-term global reference, but discrete jumps in position estimators make it a poor reference frame for local sensing and acting.
In summary, robot control would be done in odom frame because it is smooth (no jumps), while global planning would be done in map frame, which can have large discrete jumps over time but is more globally accurate.
Thank you @matlabbe , I will close this issue.
Hi @matlabbe
I see that when rtabmap is running for hours without the sensor moving, the ram memory consumption increases, I guess this should not happen because if the sensor does not move the map will not grow.
In this image you can see the memory consumption at the beginning:
In this other image you can see the memory consumption after 7 hours:
I have also seen that the
rtabmap/mapData
topic saves points that were previously captured. In the image you can see that the points of a human shape are still being published, even though the person is no longer there.I have read that you can limit memory usage by using the
/rtabmap/MemoryThr
parameter. Can you tell me what value I should set it at? Or what parameter should I change?I attach the database, it is available in the following link:
https://drive.google.com/drive/folders/1PiFYCBn96PehjWbGftjm1exEAdY1TtVg?usp=share_link
Thanks.