What is the localization priority between proximity detection and landmark detection?

[wizard-coder]

Hi we have a problem with localization with landmark detection.

we am using rtabmap to navigate through the tunnel.

For localization, we are using only 3d lidar(velodyne 16), and for landmark detection, we are using apriltag ros.

If the RGBD/ProximityPathMaxNeighbors parameter is set to 0(i.e. proximity detection is disabled), localization is performed by landmark detection.

But, When RGBD/ProximityPathMaxNeighbors parameter is set to 1, localization is not triggered by landmark detection, even though april tag is detected (Loop/Landmark_detected value is changed in rtabmap/info topic).

Is there a priority between proximity detection and landmark detection?

Below is the rtabmap setting.

Rtabmap/DetectionRate: 1

RGBD/NeighborLinkRefining: false
RGBD/ProximityBySpace: true
RGBD/ProximityMaxGraphDepth: 50
RGBD/ProximityPathMaxNeighbors: 1
RGBD/AngularUpdate: 0.05
RGBD/LinearUpdate: 0.05
RGBD/MaxLoopClosureDistance: 0
RGBD/MarkerDetection: false
RGBD/ProximityPathFilteringRadius: 3

Marker/Dictionary: 20
Marker/Length: 0.1
Marker/CornerRefinementMethod: 3
Marker/VarianceLinear: 0.1
Marker/VarianceAngular: 9999

Mem/NotLinkedNodesKept: false
Mem/STMSize: 30
Mem/LaserScanNormalK: 20

Reg/Strategy: 1
Reg/Force3DoF: true

Kp/MaxFeatures: -1

Optimizer/GravitySigma: 0.3
Optimizer/Iterations: 20

Grid/Sensor: 0 
Grid/3D: false
GridGlobal/ProbHit: 0.9 
GridGlobal/ProbMiss: 0.35 
Grid/RayTracing: true
Grid/RangeMax: 10.0
Grid/RangeMin: 1.0
Grid/MaxObstacleHeight: 0.0 
Grid/MaxGroundHeight: 0.2
Grid/NormalsSegmentation: true

Icp/VoxelSize: 0.05
Icp/PointToPlaneK: 20
Icp/PointToPlaneRadius: 0
Icp/PointToPlane: true
Icp/Iterations: 30
Icp/Epsilon: 0.1
Icp/MaxTranslation: 10.0 
Icp/MaxRotation: 3.14 
Icp/MaxCorrespondenceDistance: 0.5 
Icp/Strategy: 1
Icp/OutlierRatio: 0.85 
Icp/CorrespondenceRatio: 0.1
Icp/Force4DoF: true
Icp/DownsamplingStep: 1
Icp/RangeMin: 1
Icp/RangeMax: 10
Icp/PointToPlaneLowComplexityStrategy: 2

Mem/IncrementalMemory: false

[matlabbe]

Apriltags detection is independent of RGBD/ProximityPathMaxNeighbors parameter. Both would be done at the same time.

localization is not triggered by landmark detection, even though april tag is detected (Loop/Landmark_detected value is changed in rtabmap/info topic)

Are there warnings on terminal? Is there a proximity detection id in Loop/ at the same time?

[wizard-coder]

I'm sorry for my late reply because of my business trip.

There were no warnings in the terminal.

The video below is a rviz screen showing the rtabmap/info topic.

https://user-images.githubusercontent.com/51668314/230286216-8200636d-13aa-498c-b00c-d91c5cee9583.mp4

To verify that localization works by landmarks, I intentionally set the initial position 5 meters in front of the actual position.

At 24 seconds into the video, the Loop/Id is 17(This value is the same as the value by proximity detection) even though the Loop/Landmark_detected_node_ref is 13. And localization is not triggered by landmark detection.

I don't know why this doesn't work.

[matlabbe]

Is the localization wrong and should have jumped when detecting the tag? If localization was fine, it means the robot did a proximity detection at the same time than a landmark detection, using both constraints during localization. If the localization was wrong with previous proximity detections and then the landmark detection should have make the robot jump to another place, then there is a problem. During localization, if proximity detection doesn't rally with landmark detection, landmark detection has priority to make the robot jump at the right place. However, if proximity detection and landmark detection says the same thing, they will be both used. The rtabmap info may only show Proximity, but in reality both are used internally.

You can add /rtabmap/mapOdomCache in rviz topic to see better the localization links. You may see a proximity link at the same time than a landmark link, which is fine.

[wizard-coder]

I checked the debug message and saw that both proximity and landmark are used. And it seems to decide that proximity detection is more accurate. Under normal circumstances, this should work correctly. However, in a tunnel-like environment, especially in my environment , proximity detection almost seems to be more accurate than landmark detection because all places are similar, even if localization position by the landmark detection is the actual location. Even if the localization pose and the actual location are hundreds of meters apart, the environment is almost the same, so the landmark does not work.

Therefore, I implemented a separate program to estimate the localization location from the landmark detection and then publish the initial_pose topic to the rtabmap node for localization.

Thank you for your interest in my question.

[matlabbe]

Proximity detection works locally, so if the whole environment is a corridor, it may find always proximity detections (even if robot is not a the right location). However, it is wrong that detecting the landmark doesn't make the robot jumps at that location even if proximity detections were found (at another place). RTabmap always check if proximity detection constraints match with all detected constraints (visual global loop closures and landmark constraints). In case landmark constraints and proximity detections don't say the same thing, it will ignore proximity detection and directly make the robot jumps to landmark location. If it doesn't do that, it means either the covariance of the landmark is too large or the one of the proximity detection is too large. I'll keep this open to try to reproduce your problem in simulation.