Can't localize from the map (g2o not handling correctly localization priors)

[matuzawa1]

Nice to meet you. I'm sorry for my poor English.

I’m trying to get a topic called “/rtabmap/localization_pose” in the rtabmap localization mode using the RGBD camera (Intel Realsense D455). I made the map as shown in the red line in the following figure, but when I moved along the blue line, I could not locate the position.

Can you tell me why I cannot localize in this case? Is there something wrong with the way the map is made in this case?

Thank you for your answer.

[matlabbe]

I don't know how look like your environment, but if there are only white walls, it may not be able to localize.

[matuzawa1]

Thanks for the reply.　 The wall is not only white. If the direction is the same as the direction in which the map was created, it can be matched in localization mode, but if I start from the opposite direction (as shown in the figure above), it cannot be matched.

In rtabmapviz, the map is shown in red, but it does not seem to be localized.

[matlabbe]

It should be able to localize in both directions, if you mapped in both directions. The loop closure hypothesis is rejected, but it is detected. There should be a warning on the terminal showing the full reason why it has been rejected.

[matuzawa1]

Thank you for your response even though it was a basic question. The following warning was displayed on the terminal.

[ WARN] (2023-09-18 10:22:05.919) Rtabmap.cpp:3193::process() Rejecting localization (768 <-> 343) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.609870 (edge 768->343, type=2, abs error=0.152969 m, stddev=0.042375). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2023-09-18 10:22:05.919) Rtabmap.cpp:3222::process() Rejecting localization (768 <-> 343) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 51.492512 (edge 768->343, type=2, abs error=162.554121 deg, stddev=0.055097). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ INFO] [1695000125.921045978, 1694937322.589728873]: Odom: quality=332, std dev=0.009442m|0.018649rad, update time=0.037620s [ WARN] (2023-09-18 10:22:05.922) Rtabmap.cpp:4334::process() Republishing data of requested node(s) 328 352 (Rtabmap/MaxRepublished=2)

Are these warning messages parameter dependent?

[matlabbe]

Parameter RGBD/OptimizeMaxError could be increased to be less strict. However the angle error is 160 deg, which is way over that threshold. Maybe this one should be really rejected, but if you see only single warning telling error ratio is just over 3, then increasing the parameter may help to accept it.

[matlabbe]

I tried similar experiment than this one, but could not reproduce the optimization warnings.

If you have databases/bags to share, I could try to reproduce it here.

[sun-rabbit]

英特尔

hello，May I ask what model of your robot is? I also want to perform 3D graphics on Xuri Pi, but I do not have relevant information. Have you written a tutorial for this experiment

[HarukiTAYAMA-tp]

Hello, nice to meet you.

I am working with matsuzawa1. Sorry for the late reply.

I also tried changing parameters, but could not solve this problem. I will share the data(Google Drive), so I would appreciate it if you could do if you could help us identify the cause.

The data shared is as follows

• "rtabmap.db": database (6m round trip)
• "loc_1.bag": A data which I want to localize

By the way, currently, Realsense is not installed in the robot, but the cart equipped with RealSense is moved manually.

Thank you for your help.

[HarukiTAYAMA-tp]

Earlier, I forgot to attach the file I was using. I am using localization_launch.txt(this is a text file), which is customized based on "opensource_tracking.launch" introduced here.

[matlabbe]

Thx for the data, it seems there is a bug with g2o optimizer for that example. When using gtsam instead --Optimizer/Strategy 2, it localizes (accepts) correctly.

Digging a little more, I think the way we set the priors for the map's nodes during localization doesn't work as expected with g2o. If we set --RGBD/LocalizationPriorError 0.1, localization with g2o will work.

[HarukiTAYAMA-tp]

Thank you for your reply.

By using GTSAM, localization worked correctly. I'm very grateful for your advice.

There's one more thing I'd like to ask you about. Now, I'm changing from g2o to GTSAM using the GUI (rtabmap-viz). Is there any way to include that change in the launch file?

[matlabbe]

Yes, you can add --Optimizer/Strategy 2 to args attribute of rtabmap node, or add <param name="Optimizer/Strategy" type="string" value="2"/> under rtabmap node.

[HarukiTAYAMA-tp]

I wish you a happy New Year.

Thanks for taking the time to explain that to me. I was able to change to GTSAM correctly in the launch file.

[masskro0]

I had the exact same problem and was able to fix it by replacing the optimizer. But what could be the reason for that? Using g2o during mapping works and loop hypothesis are accepted. As soon as I go into localization mode, the g2o optimizer won't work. All features are marked as red points, this doesn't happen with GTSAM

[matlabbe]

features are marked as red points

This bug should not affect feature matching, but would reject localizations (red background on loop closure detection view). As stated above, this is related to RGBD/LocalizationPriorError parameter (default 0.001), which may need to be set to 0.1 or larger with g2o. Other option is to disable localization cache with RGBD/MaxOdomCacheSize=0.