Hello. I have a question about rtabmap behavior that I am not sure about.
This is a Gazebo Ignition simulation of TurtleBot 4, and the OAK-D-Pro is modeled by an Ignition rgbd_camera sensor. The robot starts moving and then follows a path that contains a rotation right at the beginning. I am using rtabmap and rtabmap_ros v0.22.1 with all parameters at default. The relevant ones are RGBD/LinearUpdate=0.1, RGBD/AngularUpdate=0.1, Mem/STMSize=10, and NeighborLinkRefining=false.
In this scenario, the localization pose drifts about 0.4 m compared to ground truth, and the internal state shows Rehearsal_merged = 0, Reactivation/ms ≈ 0, and Immunized_globally = 0. When I run slam_toolbox on the same input, the same drift does not happen. See the attached trajectory comparison image:
I tried to follow the code to understand what is going on.
In the rotation segment, the isMoving check inside Memory::rehearsalMerge becomes true because of the yaw change:
|
bool isMoving = fabs(x) > _rehearsalMaxDistance || |
|
fabs(y) > _rehearsalMaxDistance || |
|
fabs(z) > _rehearsalMaxDistance || |
|
fabs(roll) > _rehearsalMaxAngle || |
|
fabs(pitch) > _rehearsalMaxAngle || |
|
fabs(yaw) > _rehearsalMaxAngle; |
When yaw change is larger than _rehearsalMaxAngle (RGBD/AngularUpdate, default 0.1), isMoving becomes true, and then both fullMerge and intermediateMerge become false. In the same segment, loop closure candidates are limited to WM, and a new node moves from STM to WM only after STM exceeds Mem/STMSize=10.
So in the beginning of the run, there are not enough WM candidates, and no loop closure hypothesis is formed.
I checked the rtabmap Info messages for this case. For about 53 seconds, 28 nodes were processed, and in all of them Rehearsal_merged = 0, Immunized_globally = 0, and loop_closure_id = 0.
During the first 24 seconds (13 nodes), Working_memory_size stayed at 1, and it only started to grow after Short_time_memory_size reached STMSize=10. rtabmap_info.txt
I tried NeighborLinkRefining=true. In some runs the drift was reduced, but the result was not stable between runs because of randomness, so I could not use it as a real fix.
My current understanding is that the two mechanisms, rehearsal merge and loop closure, are both inactive at the same time in the starting-rotation segment, and that this is why the drift accumulates without correction. But I am not sure if this is the intended behavior, or if I am misunderstanding something, or if there is some other parameter that can fix this.
Could you let me know if this different behavior between rtabmap and slam_toolbox on the same input is within the intended behavior of rtabmap, or if it is something worth looking into?
Thank you
Hello. I have a question about rtabmap behavior that I am not sure about.
This is a Gazebo Ignition simulation of TurtleBot 4, and the OAK-D-Pro is modeled by an Ignition rgbd_camera sensor. The robot starts moving and then follows a path that contains a rotation right at the beginning. I am using rtabmap and rtabmap_ros v0.22.1 with all parameters at default. The relevant ones are RGBD/LinearUpdate=0.1, RGBD/AngularUpdate=0.1, Mem/STMSize=10, and NeighborLinkRefining=false.
In this scenario, the localization pose drifts about 0.4 m compared to ground truth, and the internal state shows Rehearsal_merged = 0, Reactivation/ms ≈ 0, and Immunized_globally = 0. When I run slam_toolbox on the same input, the same drift does not happen. See the attached trajectory comparison image:
I tried to follow the code to understand what is going on.
In the rotation segment, the isMoving check inside Memory::rehearsalMerge becomes true because of the yaw change:
rtabmap/corelib/src/Memory.cpp
Lines 4246 to 4251 in 27ca7ec
When yaw change is larger than _rehearsalMaxAngle (RGBD/AngularUpdate, default 0.1), isMoving becomes true, and then both fullMerge and intermediateMerge become false. In the same segment, loop closure candidates are limited to WM, and a new node moves from STM to WM only after STM exceeds Mem/STMSize=10.
So in the beginning of the run, there are not enough WM candidates, and no loop closure hypothesis is formed.
I checked the rtabmap Info messages for this case. For about 53 seconds, 28 nodes were processed, and in all of them Rehearsal_merged = 0, Immunized_globally = 0, and loop_closure_id = 0.
During the first 24 seconds (13 nodes), Working_memory_size stayed at 1, and it only started to grow after Short_time_memory_size reached STMSize=10. rtabmap_info.txt
I tried NeighborLinkRefining=true. In some runs the drift was reduced, but the result was not stable between runs because of randomness, so I could not use it as a real fix.
My current understanding is that the two mechanisms, rehearsal merge and loop closure, are both inactive at the same time in the starting-rotation segment, and that this is why the drift accumulates without correction. But I am not sure if this is the intended behavior, or if I am misunderstanding something, or if there is some other parameter that can fix this.
Could you let me know if this different behavior between rtabmap and slam_toolbox on the same input is within the intended behavior of rtabmap, or if it is something worth looking into?
Thank you