Merging Maps "Up" instead of "Down"

[mathislm]

Hello,

I'm not sure this is the correct way to phrase this but here is a more detailed explanation of what I'm trying to achieve: Currently when two maps overlap and a merge is detected, the current map is merged into the older map (so "down"). This "teleports" our robot without any warning, since it's suddenly being located with regards to the previous map.

I would like to keep locating it in the current map and merge the previous map in the current one (so "up"). This is preferable I think because it is also in this current map that any navigation task (or other tasks) are in progress and there is no reason to drop it: We haven't lost the tracking or failed anything, we actually just improved our localization !

Has anyone attempted to do that already or would have some insight on how to do that ?

Thank you in advance for your time, Mathis

[leahcornelius]

This is an interesting point, I’ve been thinking about something similar myself. An idea would be to lock the coordinate frame when merging maps so the previous map is superinposed onto the current map (ie “up” as you put it). I am not sure if this is a simple modification as it does require translating the position of every key frame and feature point from the old map’s origin to the current maps origin.

I will have a look at implementing this when I am home. I have a local fork of mORB-SLAM3 which I have added ROS 2 integration to so I will look at adding it there.

[leahcornelius]

The other solution that might be preferable would be to instead add an additional layer to abstract the pose of the camera/robot. Rather than directly using the output from OS3 we could instead have a system that uses a fixed global frame. When the system is initialised, the origin point of the map is aligned with this fixed global point. If the position is lost and a new map started we repeat this process (essentially returning the outputted position to 0,0,0). If a map merge is performed by orb slam, the system accounts for this and introduces a static transform to the outputted pose to maintain the same location as before the merge occoured. This static transform persists until we are delocalised (ie new map) or the map is closed. This would be a much much simpler solution than modifying OS3 directly.

[mathislm]

(I am very sorry for the lack of answer, I wrongly though my mail notifications were on...) I not exactly sure how I would setup the first solution you proposed, I took a look at the loop closure and I think you are right, we would need to translate the position of every keyframe/map points. If I were to do it, I would probably do that after their algorithme, which is very hard to hack.

However, I tested something similar to what you proposed in your second answer. The output of orbslam3 goes into an EKF so I basically introduced a static transform by using the filter's result when we get the first transform from a new map (different map ID). Of course each time we do that we introduce an error so this is really not ideal, but it's a good simple solution. Also I need to differentiate the creation of a new map (where I would prefere to reset the EKF) and a merge, where I just need to have this static transform. But this repo lacks a lot of getters so I haven't done it yet.

I'll keep you updated if I try other things!