Add option to keep landmarks with priors linked to local graph so that origin doesn't jump when corresponding nodes are transferred to LTM.

[matlabbe]

Original post: http://official-rtab-map-forum.206.s1.nabble.com/Integrate-external-markers-from-apriltag-ros-with-Priors-tp10358p10426.html

Is it possible to enforce rtabmap to keep node which saw landmark always inside the working memory to keep landmark constraint?

Currently not. I think what you want is that the origin stays the same (in landmarks origin) even if nodes that observed the landmarks have been transfered in LTM. I don't think immunizing all nodes up to landmarks (to keep them in local graph) is a good solution (that won't scale well). I think the most efficient way would be to keep track of the landmarks and add their constraints in graph optimization (so that with their priors the origin will stay the same). One idea would be to add a "Link::kVirtualClosure" link to oldest node in the local graph (and updating that link when the oldest node attached to is transferred, then change it again to new oldest node in local graph). That could keep the graph from drastically jumping around. I say "keeping it linked to oldest node in local graph" instead of "newest node in local graph" in order to limit origin drifting overtime if the landmarks are not re-observed for a while.

Example of the issue:

1. Both landmarks are connected to current graph, origin of the graph is then based on Marker/Priors
2. One landmark remaining, origin of the graph is still based on Marker/Priors
3. Both landmarks are gone, origin is based on odometry of latest node added to graph
4. Retrieved a node linked to local graph with landmark, origin is back on Marker/Priors
5. Same than 4, now both landmarks are back
6. Like 3, both landmarks are gone from local graph, origin jumps back to latest node
7. A node with landmark reappear in the local graph, the origin is back on Marker/Priors