Trying to Understand ICP Registration During Localization

[seanxu112]

Hello Mathieu,

During localization, I thought the icp would just grab the closest n nodes' scan and perform icp with current scan data. However, this parameter changed my understanding: RGBD/ProximityMaxGraphDepth.

My current understanding is, icp is only performed when proximity links are created (is this true for localization?). And proximity links can only stretch to MaxGraphDepth defined above. If RGBD/ProximityMaxGraphDepth = 50 and my most recent loop closure is 60 nodes away, icp will not be correcting with the laser scan. Is this true for both localization and mapping?

My question comes from an observation that is similar to the example you wrote in the proximity link section of Long-Term Online Multi-Session Graph-Based SPLAM withMemory Managemen. Basically the robot only has seen one direction of the path; when I drove the robot facing the other direction during localization, icp is no longer correcting for robot pose, because I can easily see the scan-gridmap mismatch in rviz (do not know how to visualize past scans that the icp is performing on).

Is my understanding correct or I am not setting some parameters correctly. If my understanding is correct, is there any harm in just using the scan of the nodes that is spatially close and performing icp indefinitely? I do think the initial loop closure is important.

Sincerely,
Sean

[matlabbe]

Hi,

RGBD/ProximityMaxGraphDepth is not used in localization mode, all nodes close to the robot are tested for proximity detections: https://github.com/introlab/rtabmap/blob/862eb0a90a00d81fc0adf73cbc0c285fec3a7e17/corelib/src/Rtabmap.cpp#L2291-L2298. In mapping mode, that parameter is used as you said.

Proximity detection is done in both mapping and localization modes.

I tried to reproduce your experiment using the demo_mapping.bag. First, create a map up to 116 seconds and kill the bag:

$ roslaunch rtabmap_ros demo_robot_mapping.launch
$ rosbag play --clock -s 110 demo_mapping.bag

# stop the bag at 116 sec, then close rtabmap.

Start rtabmap in localization mode, and restart the bag at 110 seconds to localize at the beginning

roslaunch rtabmap_ros demo_robot_mapping.launch localization:=true
rosbag play --clock -s 110 demo_mapping.bag

After the first loop, you can disable loop closure detection in Preferences->Vocabulary, set "Maximum words per image" to "-1" instead of 500. Only proximity detection with icp will be working (I saw a wrong loop closure making jump the robot at another place, this could be avoided by tuning other parameters but it is not the point of this post).

The robot should be able to localize on the whole trajectory backward (see yellow loop closure):
 

[seanxu112]

Interesting. It is good to see that code snippet. This behavior is definitely desirable. However, when I tried to shoot a video to demonstrate my reverse hallway problem, the problem is not showing anymore.

The same issue still exists in general, for example when my t265 odometry shifts a bit like the image below, the robot cannot adjust itself back.



In this case, I just gave /rtabmap/initialpose a bad guess intentionally, since I cannot control the noise of t265. The robot is only delocalized translationally for 0.3 meters, but cannot recover localization. Do you think this is the limitation of ICP and this case it just converged to a local minima? I am using the default param for LocalRadius and MaxLocalRetrieved, they are 10 and 2 respectively. The database can be found here: https://drive.google.com/drive/folders/1mokP42sL8ejj6lMUySeDz2O3lAnpj-zh?usp=sharing

Sincerely,

Sean

[matlabbe]

Hi,

If Icp/MaxCorrespondenceDistance is 0.1 (default 0.1 or 0.05 if you built with or without libpointmacher), an offset of 0.3 won't be corrected as there would be almost no correspondences under 10 cm.

To recover larger errors, first I recommend to build rtabmap with libpointmatcher, then increase Icp/MaxCorrespondenceDistance to 50 cm or even 1 meter.

On a side note, I checked your database and the RGB-D camera orientation is not correct. You should update your TF so that the camera is looking higher. This can be debugged with RVIZ by setting your base link in global options, then show RGB-D cloud, the ground in the cloud should be aligned with RVIZ grid.


This may be also the cause why walls in the occupancy grid look puffy:


As if we use Grid/FromDepth=false, Grid/3D=false ad Grid/RayTracing=true with laser scans, there is the result:


cheers,
Mathieu

[seanxu112]

Oh nice, it is definitely can recover larger distance now, but it seems there are more oscillation for the robot pose. I am wondering whether this problem should be addressed by libpointmatcher or is there some rtabmap param for it? I am currently using a very naive yaml setup for libpointmatcher.

I also cannot figure out what is the difference between MaxCorrespondenceDistance and MaxTranslation. If the icp correspondence distance is 0.5m, wouldn't that send a correction for 0.5m for MaxTranslation? And is MaxLocalRetrieved used in the sense that it takes n nodes that is closest and combine the scans in these nodes. Then rtabmap uses the combined scan for Icp against the current scan?

And also I fixed the tf, next time I map the wall will hopefully be less puffy; thank you for pointing that out.

Sincerely,
Sean

[matlabbe]

Hi Sean,

$ rtabmap --params
Param: Icp/MaxCorrespondenceDistance = "0.1"  [Max distance for point correspondences.]
Param: Icp/MaxTranslation = "0.2"             [Maximum ICP translation correction accepted (m).]

RGBD/MaxLocalRetrieved = "2"            [Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).]

Param: RGBD/ProximityPathMaxNeighbors = "0"     [Maximum neighbor nodes compared on each path. Set to 0 to disable merging the laser scans.]

If you set Icp/MaxCorrespondenceDistance to 1 meter, it doesn't mean the ICP correction will be 1 meter, it is the maximum distance between corresponding points during ICP iterations.

Icp/MaxTranslation will reject any transforms where ICP correction is more than this value. If you expect that ICP correction could be over 20 cm (correcting larger drifts), then increase this parameter. This parameter exists in case ICP diverges on something worst (causing large wrong ICP correction).

RGBD/MaxLocalRetrieved is used with memory management, it is independent of proximity detection. It is not used if you didn't enable memory management.

If RGBD/ProximityPathMaxNeighbors is one, only the closest node scan is compared to current scan. If RGBD/ProximityPathMaxNeighbors > 1, we combine scans on the closest path and compare it to current scan.

You don't need to feed a libpointmatcher yaml file to rtabmap unless you do something very specific or use advance filtering from libpointmatcher, the default config used by rtabmap would work relatively good already. Without libpointmatcher config file, I cannot really comment on this. My comments so far assumed that you didn't use a yaml file for libpointmatcher.

cheers,
Mathieu

[seanxu112]

Thanks for the quick reply Mathieu. These information enough for me to play around with the params you mentioned and with libpointmatcher yaml file.

I will update this post when I have new questions or cannot solve the issue by myself.

Thank you for the help.

Sincerely,
Sean

[seanxu112]

Hi Mathieu,

Is there a way to check the reference scan data? It seems like sometimes the ICP is correcting itself to hard-to-understand locations, if I can get the reference data that it is trying to matching to; it might be easier to debug. I have also uploaded some videos to the same google drive link in the post above, mostly the robot position is just oscillating while remaining still.

Though after sometime of tuning my libpointmatcher yaml which can be found here: pm_1.yaml I have set MaxCorrespondenceDistance, MaxTranslation both to 0.5, and RGBD/ProximityPathMaxNeighbors to 1.
If I set KdtreeMatcher.maxDist = 0.05, the oscillation does not happen as drastically. However, I do not know if that explains the huge oscillation shown in the videos. It almost feels like a fallacy, since I am limiting the matcher to be within 5cm, I should not expect huge movement.

Either visualizing on rtabmapviz/rviz or having a ton of vtk file would both work for me. I tried to use the VTKFileInspector for libpointmatcher, but i don't know where the output files are.

Sincerely,
Sean

[seanxu112]

Just to answer my own problem literally 40 mins ago :D. It seems like if I just run PointToPlaneErrorMinimizer with SurfaceNormalDataPointsFilter: knn = 5, the behavior is much more desirable, I think the default libpointmatcher from rtabmap has too high of a outlierRatio, which I didn't spend too much time tuning.

A way to visualize the reference scan data would still be greatly appreciated. It might still help me debug any future ICP related problems.

Thanks for your help Mathieu.

Sincerely,
Sean

[matlabbe]

For ICP tuning, I usually use rtabmap-databaseViewer, open Constraints View and browse neighbor or loop closure links. There is a "Refine" button that can be used to refine the links. You can open Core Parameters view and tune ICP parameters before hitting "Refine".

It could also be possible to export the individual scans in PCD files with File-> Export clouds, uncheck Assemble.

cheers,
Mathieu

[seanxu112]

This information is immensely useful. I already have solved some issues that is hard to figure out last Friday.

Is there anything similar for rtabmapviz? My goal is to be able to visualize the proximity links during localization. From database-Viewer we can look at the constraint view, but it is not accessible during localization.

I have set Mem/LocalizationDataSaved to true, is there a way to put these recorded data to constraint view? I just want to see which nodes' data is used for icp.

My current hypothesis is that since my LocalRadius is set to 10 meters (will definitely reduce that), scans from the proximity links that should not be connected are now connected. For example scan returns of two parallel board that are close to each other but separated by 10-20cm, I can only see one board at a time, but icp will try to squeeze them at some proximity links but not the others.

Edit: I think the problem might be loop closure? When I disabled visual loop closure by setting Kp/MaxFeature = -1, that problem do not exist anymore. Is that something that I am missing?

Sincerely,
Sean

[matlabbe]

Do you have a database to share?

When setting Kp/MaxFeature = -1, you are disabling loop closure detection. If the map is worst after a loop closure, this can come from bad odometry's covariance, not correctly set ICP parameters or even wrong loop closure.

[seanxu112]

I think the reason is actually proximity link is switching between two different nodes. I checked with rostopic echo /rtabmap/info/proximityDetectionId, and I have ProximityMaxPath=3.

I do not know if the cause of this problem is due to the computation cost during memory management (data are separated), since when I just mapped a small area, the localization is nearly perfect. Thus, I decided to map again with TimeThr=700. However, now the odometry drift does not seems to recover at all, and there is no loop closure when I tried to detect more loop closure. I am in the process of add loop closures manually.

Both databases are included in this google drive here: https://drive.google.com/drive/folders/1PVZ_6PKK3lL44PX36eJuSW8GINx0WorD?usp=sharing I also included my pm.yaml

Thank you for the time, Mathieu.

Sincerely,
Sean

[seanxu112]

Hi Mathieu,

After playing around with some params: rtabmap-reprocess --RGBD/ProximityByTime false --RGBD/ProximityBySpace true --RGBD/ProximityOdomGuess true --Rtabmap/TimeThr 0 --Icp/MaxTranslation 1.0 Memory_management.db MM_huber05.db
and changed my outlier filter (huber) tuning param to 0.5, I got much better result on this map. It is in the google drive link above "MM_huber05.db".

However, I still see some problems with proximity link, especially with the picture below. mismatch.png
Is there a parameter to reject such result? Or it is simply ICP failing to converge to the right orientation?

Sincerely,
Sean

[matlabbe]

Hi Sean,

sorry for the delay, I was quite busy this winter. I've been playing with your without_memory_management.db:

rtabmap-reprocess |
   --Icp/OutlierRatio 0.85 \
   --Icp/VoxelSize 0.05 \
   --RGBD/NeighborLinkRefining false  -\
   -Kp/DetectorStrategy 8 \
   --Vis/FeatureType 8 \
   --RGBD/LocalBundleOnLoopClosure false  \
   --RGBD/OptimizeMaxError 3 \
   --Icp/MaxCorrespondenceDistance 0.3 \
   --Icp/CorrespondenceRatio 0.3 \
   --Mem/STMSize 30   \
   without_memory_management.db \
   output.db

This output.db was more cleaner to test why RGBD/OptimizeMaxError is not working properly on your map. I've found that your odometry poses have non-null Z values, while using 2D lidar and Reg/Force3DoF=true. It seems that the optimizer doesn't like having poses with Z values and 3DoF contraints in 2D mode, making RGBD/OptimizeMaxError check failing. For example, I tried to merge manually the two sessions but even if the images were taken at the same place (the end of session 1 and beggining of session 2), there was a Z offset of 40 cm! Here the two poses (taken from exported odometry poses_odom.txt):

1455216201.124591 8.288856 -2.205386 -0.903682 0.000000 0.000000 0.035750 0.999361
1455208280.178878 -0.100181 0.002711 -0.545289 0.000000 0.000000 -0.013912 0.999903

However, with Reg/Force3DoF turned off, loop and optimizations seem working with RGBD/OptimizeMaxError:

rtabmap-reprocess |
   --Icp/OutlierRatio 0.85 \
   --Icp/VoxelSize 0.05 \
   --RGBD/NeighborLinkRefining false  -\
   --Kp/DetectorStrategy 8 \
   --Vis/FeatureType 8 \
   --RGBD/LocalBundleOnLoopClosure false  \
   --RGBD/OptimizeMaxError 3 \
   --Icp/MaxCorrespondenceDistance 0.3 \
   --Icp/CorrespondenceRatio 0.3 \
   --Mem/STMSize 30   \
   --Reg/Force3DoF false \
   without_memory_management.db \
   output.db

On your memory_management.db, with RGBD/NeighborLinkRefining=false as above, proximity detections are not very accurate as assembled scans seem to contain odom drift:


If I set RGBD/NeighborLinkRefining=true to correct odometry drift, the blue points are more superposed, however the whole global map is deformed:


Here is without any loop closure, proximity detection or neighbor refining (pure odom):


My feeling is that the lidar attitude (roll/pitch) is not always null. Does your lidar rotate in pitch and roll when moving? Also, there are some Z values changing as observed in previous database. Here is if I remove any lidar in pose optimization and loop closures:

rtabmap-reprocess \
   --Icp/OutlierRatio 0.85 \
   --Icp/VoxelSize 0.05 \
   --RGBD/NeighborLinkRefining false  \
   --Kp/DetectorStrategy 8 \
   --Vis/FeatureType 8 \
   --RGBD/LocalBundleOnLoopClosure false  \
   --RGBD/OptimizeMaxError 3 \
   --Icp/MaxCorrespondenceDistance 0.3 \
   --Icp/CorrespondenceRatio 0.3 \
   --Mem/STMSize 30 \
   --Reg/Force3DoF false \
   --RGBD/ProximityByTime false \
   --Icp/PMConfig "" \
   --RGBD/ProximityPathMaxNeighbors 0 \
   --Reg/Strategy 0  \
   memory_management.db \
   output.db

Odom (without graph optimization):

With graph optimization:


Now the optimized map seems more accurate than the odometry one (e.g., removing the doublewall at the bottom left).

[seanxu112]

Hello Mathieu,

It is interesting that you mentioned a z offset, and that has not been on my mind at all.

My current setup is a VIO with the t265 camera, which publishes the 3D odom information. I feed that odom data to rtabmap, and subscribe to it myself, and manually remove the roll and pitch, and the z translation. Then I take this modified 2D odom information and send it though tf that links to the rest of the robot. So essentially, rtabmap is provided with both the 2D odom information with the tf, and the 3D odom information with the odometry message. I am wondering if this is why the odom pose have 3D information?

I have currently set the odom_topic to some arbitrary value. Do you think that would solve the problem?

Sincerely,
Sean Xu

[matlabbe]

Hi Sean,

The best would be that both odom poses and TF poses are 2D. Did you disable TF published from T265? What I would do is to make sure T265 doesn't publish TF, make a node subscribing to T265 odom topic, modify roll/pitch/z values then republish the odometry topic to rtabmap with corresponding TF. Otherwise, you can also modify realsense2_camera node to provide 3DoF odom topic and TF directly.

cheers,
Mathieu