Integrate external markers from apriltag_ros with Priors

[fboris]

I would like to use marker message(https://docs.ros.org/en/melodic/api/apriltag_ros/html/msg/AprilTagDetectionArray.html) from  external apriltag_ros node and make these marker has priors(like this https://github.com/introlab/rtabmap/blob/master/corelib/include/rtabmap/core/Parameters.h#L877).

But I could not find any example or relative discussion on this. Is it possbile to make markers from external node to have prior definition?

[matlabbe]

You can set Marker/Priors even if you subscribe to apriltag_ros output. Make sure you are using the same IDs.

I don't think AprilTagDetectionArray supports metadata describing the real pose of the tag. Note that the poses output from apriltag_ros are relative to camera, not in global fix frame, so they cannot be used as priors.

[fboris]

Yes, apriltag_ros send relative pose from tag to camera.  I saw there is coordinate tranform from tag to odom frame when it is going to insert new landmarks. (code) This conversion function looks like it is converting poses under camera frame to global-fix frame. If I modify apriltag_ros to send poses under a global fixed frame, will it disrupt the coordinate conversion in the landmarksFromROS function?

[fboris]

Hi Mathieu,
above digram is what sensors layout on my robot.
I would like to use front rgdb cam (oak-d) as rgbd-slam for rtabmap.
Due to the limitation  of installation of tags. I need to use camera on the side(normal rgb camera not rgdbd) to detect tags and contraint the map of rtabmap.  
Also, making camera point to the tag get better marker detection result. It is reason I need to use apriltag_ros to get detections and define prior of tags.
Or do you have any better suggestions? But again, thanks for your reply.
Best,
fboris

[matlabbe]

The fixed frame in landmarksFromROS (odom) is used to adjust tag pose accordingly to current odometry timstamp used to create the new node in the graph. Nodes may be created at a different time than when the tag has been detected. The returned landmarks contain relative poses from base_link frame to tag frame, so you cannot feed a not relative tag to that function.

It is fine to use apriltag_ros on the two other RGB cameras, the tag detections will be transformed in base_link frame and used as is.

I still think using Marker/Priors is the best solution in your case. Note that the tricky part with tags, is to set appropriate rotation. You can do a run without Marker/Priors, open database in rtabmap-databaseViewer and mouse-over the landmarks in GraphView to know their position/rotation. Then based on your real-world coordinates, you can adjust the values to create the priors.

cheers,
Mathieu

[fboris]

Mathieu,

Thanks for you suggestion. I can setup prior correctly from database.

But I still have one issue. I make robot move around 20 meters. when I make robot go to origin. the /rtabmap/localization_pose shows very low x/y/z translation error(less than 0.1m). But the heading(yaw) has 10 degrees error(reading is 10 degrees, but it is supposed to be 0 degrees) even I make side camera see the apriltag. Any suggestion of this issue? Thanks

Best,

[fboris]

Mathieu,
After weeks trying, i make heading error less(cause by bad prior)! But I has another question. Base on discussion on https://github.com/introlab/rtabmap_ros/issues/960  , rtabmap follows REP 105. Is it possible:
1. correct localization output (/rtabmap/localization_pose) back to odometry? Like feeding back the drift odometry to make odometry " corrected" by map.
2. Is `/rtabmap/localization_pose` timestamp same as odometry? following code looks like it is. But im not sure
- https://github.com/introlab/rtabmap_ros/blob/a97efff760720132ced2a607f384dc5d28a0d296/rtabmap_slam/src/CoreWrapper.cpp#L1061
- https://github.com/introlab/rtabmap_ros/blob/a97efff760720132ced2a607f384dc5d28a0d296/rtabmap_slam/src/CoreWrapper.cpp#L2115C30-L2115C35
I'm thinking when localization has new output(pubLocPoseOnlyWhenLocalizing_=true), it correct corresponding odometry

[matlabbe]

You cannot feed back the corrected odometry pose to odometry node. If you need an odometry pose corrected by vslam at odometry frame rate, you could make a node subscribing to odometry topic, then convert the pose in map frame using latest TF map->odom.

"/rtabmap/localization_pose" uses same stamp than odometry and equivalent to converting odometry pose at same stamp with map->odom TF at that time.

[fboris]

Mathieu,
Thanks for your explanation. I make external apritltag_ros work with rtabmap now. I turn on memory management of rtabmap(--Rtabmap/TimeThr 200 ). To prevent "position jump", I also enable -RGBD/OptimizeFromGraphEnd true(from discussion). But I have problem if WM do include landmark. the localization output will have a lot of offset. It looks like lost constraint from landmark.
it is test what I've done.
1. there is tag at x=0 y= 0 and x=7.4 y=0, i make robot to visible both arpiltag. after a while, i make rotbot to a knownpoit x y z yaw = (4.24, 0.08, 0.01 , 355.6)

2. I try to make robot move between two arpiltag. So robot cannot see apriltag anymore. After a while, the map is looks like twist and  position of known point has a lot of drift. x y z yaw = (4.16,0.24,0.74,353.42)

So It looks like of WM has no landmark inside. the map is going to twist. From the paper and previous discussion, graph optimization is doing in working memory. Is it possible to enforce rtabmap to keep node which saw landmark always inside the working memory to keep landmark constraint? https://github.com/introlab/rtabmap/blob/master/corelib/src/Rtabmap.cpp#L4324 looks like can do the job. But I'm not sure it is correct.

[matlabbe]

I'll be honest, I don't remember ever tested landmarks with memory management enabled. If you can share the database of that mapping session it would be useful to reproduce your setting.

[fboris]

Hi Mathieu,
Here is database(https://drive.google.com/file/d/1jEsJ7k8DuQ6Jn3V4Ci0F58XFEu-P51Bo/view?usp=sharing). This is hand held testing. I use rgbd camera(oakd) for odometry and rtabmap(forward). Another single camera for tag detection(downward). I use apriltag_ros for tag detection. "  -Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3 --Kp/DetectorStrategy 11 --Vis/FeatureType 11  --RGBD/MarkerDetection false --Optimizer/PriorsIgnored false --VhEp/Enabled false --Mem/UseOdometryFeatures=True --Rtabmap/DetectionRate 1  --RGBD/OptimizeMaxError 0 --Optimizer/Strategy 2  --RGBD/OptimizeRobust true  --Rtabmap/TimeThr 200 --RGBD/OptimizeFromGraphEnd true"  these parameters  has been used in RTAB. You will see some apriltag on the wall in database. But I actually do not use this. I use apriltag on the floor.

[fboris]

Hi Mathieu,

Later, I update another test to check "Mem/ReduceGraph true". Complete args : "  -Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3 --Kp/DetectorStrategy 11 --Vis/FeatureType 11  --RGBD/MarkerDetection false --Optimizer/PriorsIgnored false --VhEp/Enabled false --Mem/UseOdometryFeatures=True --Rtabmap/DetectionRate 1  --RGBD/OptimizeMaxError 0 --Optimizer/Strategy 2  --RGBD/OptimizeRobust true  --Rtabmap/TimeThr 200 --RGBD/OptimizeFromGraphEnd true --Mem/ReduceGraph true".

The result of localization is more accurate. And You can see the graph preserve node around landmark. I think because I was keeping walking  same space. rtabmap didn't add more node in WM(this part im no sure, may need you to clarify). My assumption is rtabmap pose graph in second test might  be:
1) better geometric distribution, so optimization of WM will be more precise( compare with  Sep 03, 2024 post)
2) landmarks are in the pose graph(with Prior). So those prior marker constraint the error.
But again, This part may need your clarification. It is why I have idea: Is it possible to keep all the landmarks in WM? So rtabmap localization output will be always precise even we do memory management.

Here is second test db file(https://drive.google.com/file/d/1zWSPW8aoGDNBsY78gysNuixARCR5cfZL/view?usp=sharing).

[matlabbe]

Hi fboris,

I think I see what you are meaning about the landmarks (when dialing down the memory threshold Rtabmap/MemoryThr to 60 to forget as most nodes as possible). The use of Mem/ReduceGraph=true makes it even more interesting with memory management enabled. Not related your problem, I've found an issue with WM increasing out of bound with Mem/ReduceGraph=true by doing the trajectory your are doing (revisiting always the same areas).

In your case, the issue is that as soon as a landmark is not linked to local graph anymore, the graph origin will change drastically to latest node in the graph (RGBD/OptimizeFromGraphEnd=true). Here is an example of a sequence happening when nodes are moved back and forth between WM and LTM.

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`


Another interesting effect is that because the landmarks are on the ground at z=0, if odometry is drifting vertically, as soon as the landmarks are not in local graph anymore, the whole map will jump in z (accordingly to current odometry):


I think the memory management and graph optimization are behaving as expected with the landmarks. The local map looks fine, but the the origin is jumping around. That behavior is observed in this paper (Figure 11 for example, we see the local graph changing origin over time), but as long as planning goals are "nodes" and not "poses", robot is still able to navigate even if the origin changes.

So for your question:

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. I created an issue to implement that feature. Let me know if I missed something!

cheers,
Mathieu

[fboris]

Mathieu,
Thanks for your explaination. Yes, I want a consistent map for both navigation and visualization. For example, I want the robot to go (3,0,0) all the time.
For the idea you proposed,
1) keep track of the landmarks and add their constraints in graph optimization
2) add a "Link::kVirtualClosure" link to oldest node in the local graph
what is proper way to implement 1?

Another things, what is the complete command you were using for testing my dataset? I'm also checking detail how  to implement as you said.

Best,
fboris

[matlabbe]

When a landmark is detected, a new pose corresponding to that landmark is added to local graph here. When the landmark gets disconnected from the local graph, it is removed from the local graph here. The idea could be to not remove it, but instead add a virtual link to oldest node in local graph with that landmark id. That virtual link would need to be removed as soon we recall a node having that landmark as constraint and that node is linked to local graph. There could be some implementation details missing though.

I opened the database in RTAB-Map standalone app, clicked "yes" to use same parameters from the database, but clicked "no" to download data. Then close the database. Open Preferences->Source, set Source type as "Database" and scroll down to select that database. Click yes to use database's odometry and to set detection rate to 0 to process all frames. You can change the frame rate at the top of the window to reprocess slower or faster than real-time. On the main window, click on Play button. The GIF I created was by taking screenshots from the Graph View, opening them in Gimp as layers and aligning them on the map frame (change opacity of layers to align layers), then exported as animated gif.

cheers,
Mathieu

[fboris]

Mathieu,
Thanks for clear explanation for re-process database again. I tried to follow your steps. But it actually has following error when it click "start"

[ERROR] (2024-09-13 01:16:09.244) VWDictionary.cpp:789::addNewWords() Descriptors (size=32) are not the same size as already added words in dictionary(size=256)
[ERROR] (2024-09-13 01:16:10.296) VWDictionary.cpp:789::addNewWords() Descriptors (size=32) are not the same size as already added words in dictionary(size=256)


And I actually to uncheck Ignore landmark/  Ignore features to make it not has error at startup. But there are still some warning as following.

[ WARN] (2024-09-13 01:12:52.526) OptimizerGTSAM.cpp:872::optimize() GTSAM exception caught:
Indeterminant linear system detected while working near variable
299 (Symbol: 299).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
 Graph has 451 edges and 144 vertices
[ WARN] (2024-09-13 01:12:52.526) Rtabmap.cpp:5266::optimizeGraph() Optimization has failed (poses=144, guess=1, links=451)...
[ WARN] (2024-09-13 01:12:52.526) Rtabmap.cpp:5157::optimizeCurrentMap() Failed to optimize the graph! returning empty optimized poses...
[ WARN] (2024-09-13 01:12:52.526) Rtabmap.cpp:3767::proce

After a while, It will have following message and rtabmap cannot do detection anymore.

[ERROR] (2024-09-13 01:13:14.901) VWDictionary.cpp:789::addNewWords() Descriptors (size=32) are not the same size as already added words in dictionary(size=256)
[ERROR] (2024-09-13 01:13:14.921) VWDictionary.cpp:789::addNewWords() Descriptors (size=32) are not the same size as already added words in dictionary(size=256)
[FATAL] (2024-09-13 01:13:14.921) RegistrationVis.cpp:1401::computeTransformationImpl() Condition (toWordIds.size() == kptsTo.size()) not met!

I compiled rtabmap 0.21.6 with gtsam 4.2.0. Any steps i missed or i use wrong version?

[matlabbe]

I don't have torch installed on this computer, so I cannot say it is because of SupertPoint Torch that was used. However, You should be able to start and ignore the features from the database anyway. Here is a video of what I did: https://www.youtube.com/watch?v=fiZPpJd9wq4 (I manually change the framerate to speedup the process when the camera is not moving, then reduce to 2 Hz to be able to follow).

cheers,
Mathieu

[fboris]

Mathieu,
I think i miss one step "create new database". Everything process fine now. So those  step you shown, rtabmap only took rgbd image and odometry. Is it correct?

[matlabbe]

It reads odometry and landmark constraints and RGB-D data from the input database, and stream the data (like a camera driver) to rtabmap running in a new database. When unchecking "Ignore features" option in Source settings, you can make the DBReader publishing the visual features for each frame. For example, I could have unchecked it to use the superpoint features from the input database even if my rtabmap version is not built with superpoint support.