Seeking advice for globally consistent map updates

[Jeonghwan Park]

Dear Mathieu,
First of all, thanks so much for your devotion to RTABMap both in terms of code maintenance/updates and forum management!
I am constantly astounded by your commitment on the task of keeping RTABMap such a capable framework.

I am currently seeking some advice on the subject of map updates.
My setup includes a RGBD camera and a 2d laser scanner, and I'm using rtabmap-ros version 0.21.5.
On a high level, the task i wish to achieve is to update the map while keeping it globally consistent, that is, making sure that the overall shape of the map follows the original map, so that the fixed coordinates that i have(for navigation tasks, etc) stays valid after the update.
I know that this can be done by enabling mapping mode and moving around with the original database loaded, but this makes the map database file grow in size after each update.
So far, I have thought of three approaches on how to achieve this:
1. Place markers on the map while creating the original database, store their position, and start the whole mapping process over when a map update is needed and set the priors on the marker with their stored positions.
2. Move around in the original map in localization mode while recording sensor data with their global pose, and run the mapping process offline with the recorded data with the pose priors set as the recorded global pose.
3. Enable mapping mode and move around with the original database while covering the entire mapped area, and remove the "old" data (sensor data and nodes acquired during the original mapping process) so that the contents of the database is replaced with the new data.
Can you share your thoughts on which one of these approaches that you assume is the most feasible, or suggest a more graceful or efficient way to perform the update? I'd also really appreciate some advice on how to perform these approaches using rtabmap gui or rtabmap-databaseviewer gui.

Thank you for your commitment and time for RTABMap users!

Best regards,
Jeonghwan Park

[Jeonghwan Park]

I've just realized that option 3 is possible with rtabmap-reprocess.
However it alters the origin of the map to be at the first node of the "new" data, since there are no more anchors or priors that relates the nodes to the original coordinate system.
What would be a good way to assign priors to the new data so that consistency with the old map is preserved?
Somehow there must be priors that are uniformly distributed across the new map in order to preserve the overall shape of the new map, otherwise graph optimization might skew the global shape a bit...

[matlabbe]

Hi,

Good question. We can do something similar to option 3. I don't know if your remapping process should be autonomous or teleoperated. In either case, after you created a new database of the environment, you want to align it to the old map to keep the same world frame, then discard all old data in the final database. Here what could be done:

1. Based on the optimized graph of the original database, add Prior links (prior links have from_id=to_id and type kPosePrior) to every nodes in the original map with low covariance and using the corresponding optimized poses. This could be done by a python script (sqlite3 directly) or cpp cli (using DBDriver) to add those links.
2. Offline, append the new map to the original one, using rtabmap-reprocess:

rtabmap-reprocess -a --Optimizer/PriorsIgnored false "original.db;new.db" output_combined.db

3. At this stage, output_combined.db contains both maps aligned with original world frame. Re-run again the script of step 1 to fix the remaining nodes from the second session.
4. Extract second map: re-run rtabmap-reprocess to extract the second session in a new database. Assuming the new added session has map_id=1 (where nodes of original maps have map_id=0):

rtabmap-reprocess -start_s 1 -pub_loops -Mem/GenerateIds false output_combined.db new_aligned.db

The new "new_aligned.db" database would contain only data of the new map, but poses aligned with the previous map, thus keeping the same origin. One downside of fixing the whole original map is that if there are errors / map deformations in the original map, all subsequent updates will keep these errors. However, all coordinates will stay the same. In theory, there could be a "photocopy of a photocopy" issue overtime, which would make the map slowly drift after each copy. I however yet to see it in practice, we would need to answer this question: how many cycles (tens, hundreds or thousands?) are required to deform the map more than X cm?

The option with apriltags can be one that should be robust to the "photocopy of a photocopy" problem, if the tag positions are always the same, the new map will be always in same coordinates. A variant of above, instead of setting Priors on poses, you may set Marker/Priors parameter with the poses of the markers estimated during the original mapping session. Only steps 2 and 4 above would be required. The robot would have to observe at least one tag again in the new session to keep the constraints aligned in the original tag's origin.

cheers,
Mathieu

[Jeonghwan Park]

matlabbe wrote

1. Based on the optimized graph of the original database, add Prior links (prior links have from_id=to_id and type kPosePrior) to every nodes in the original map with low covariance and using the corresponding optimized poses. This could be done by a python script (sqlite3 directly) or cpp cli (using DBDriver) to add those links.
2. Offline, append the new map to the original one, using rtabmap-reprocess:
3. At this stage, output_combined.db contains both maps aligned with original world frame. Re-run again the script of step 1 to fix the remaining nodes from the second session.
4. Extract second map: re-run rtabmap-reprocess to extract the second session in a new database. Assuming the new added session has map_id=1 (where nodes of original maps have map_id=0):

Wow this makes sense! Didn't occur to me... Indeed this would preserve the original map's shape including its errors

matlabbe wrote

One downside of fixing the whole original map is that if there are errors / map deformations in the original map, all subsequent updates will keep these errors. However, all coordinates will stay the same. In theory, there could be a "photocopy of a photocopy" issue overtime, which would make the map slowly drift after each copy. I however yet to see it in practice, we would need to answer this question: how many cycles (tens, hundreds or thousands?) are required to deform the map more than X cm?

I haven't mentioned in my original question but I've had the exact same worries about incremental updates. Obviously it would depend on the environment but it would be difficult to set a criteria... Apriltags would provide a good global anchor but there may be some cases when installing markers is not allowed at all so your suggested process seems to be the best thing for now

With that said I have one more related question: What would be a good way to change the origin of the map's coordinate system? For example I want the coordinate x=3.0, y=5.0, yaw=1.0 to be the new origin so that all node coordinates are updated acoordingly. One complex approach would be to calculate the relative pose of the "previous" origin wrt the "new" origin and set the prior of the first node accordingly, but I'm wondering if there might be a better systematic approach...

Always thanks for your devotion!

Best regards,
Jeonghwan Park

[matlabbe]

Hi Jeonghwan,

With that said I have one more related question: What would be a good way to change the origin of the map's coordinate system? For example I want the coordinate x=3.0, y=5.0, yaw=1.0 to be the new origin so that all node coordinates are updated acoordingly. One complex approach would be to calculate the relative pose of the "previous" origin wrt the "new" origin and set the prior of the first node accordingly, but I'm wondering if there might be a better systematic approach...

There are a couple of options:

1. In rtabmap-databaseViewer, we can rotate the map with the corresponding slider (position cannot be changed by this approach though). This doesn't change any constraints in the map, it only updates the optimized graph, meaning as soon the map is re-optimized it will lose the orientation. Without memory management enabled, the optimized graph should not be re-optimized in localization mode.


2. Add a prior on the first node like you mentioned (don't forget to launch rtabmap with Optimizer/PriorsIgnored=false afterwards):


3. Launch rtabmap node with "map_frame_id" parameter set to "rtabmap" (or anything you want), then create a parent frame in the TF tree with the position/orientation offset of the origin:

ros2 run rtabmap_slam rtabmap --ros-args -p map_frame_id:=rtabmap ...

ros2 run tf2_ros static_transform_publisher --x 1 --y 2 --yaw 0.1 --frame-id map --child-frame-id rtabmap

cheers,
Mathieu

[Jeonghwan Park]

matlabbe wrote

3. Launch rtabmap node with "map_frame_id" parameter set to "rtabmap" (or anything you want), then create a parent frame in the TF tree with the position/orientation offset of the origin:

This is actually a very practical workaround/approach! I'll try to incorporate it into my setup.

I would very much like to make a donation for RTABMap but paypal says it isn't supported in my country(S.korea)...

Thank you again for your devotion and support!

Best regards,
Jeonghwan Park