search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.83k stars 786 forks source link

GTSAM indeterminant linear system on ROS Humble. #1212

Closed brayanpa closed 6 months ago

brayanpa commented 9 months ago

Hi Mathieu,

I'm mapping a large environment using multiple sessions. While I have successfully achieved a good map, I am encountering an issue in localization mode: the robot gets sometimes localized very far from the map. After this happens no new location is accepted even with the RGBD/OptimizeMaxError parameter set to 0.0.

This is the error message I'm encountering:

[rtabmap-1] [ WARN] (2024-01-30 14:43:44.468) OptimizerGTSAM.cpp:595::optimize() GTSAM exception caught:
[rtabmap-1] Indeterminant linear system detected while working near variable
[rtabmap-1] 22978 (Symbol: 22978).
[rtabmap-1]
[rtabmap-1] Thrown when a linear system is ill-posed. The most common cause for this
[rtabmap-1] error is having underconstrained variables. Mathematically, the system is
[rtabmap-1] underdetermined. See the GTSAM Doxygen documentation at
[rtabmap-1] http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
[rtabmap-1] more information.
[rtabmap-1] Graph has 16 edges and 14 vertices

Unfortunately, replicating this issue is challenging as it occurs approximately after 4 hours, and once it happens, rtabmap needs to be restarted. I'm uncertain if it's related, but our odometry (obtained from TF) is good, so we set the covariance as:

"odom_tf_linear_variance": "0.0001"
"odom_tf_angular_variance": "0.0001"

I also don't know if it is related but we are modifying the RGBD/OptimizeMaxError parameter, it is 0.0 to obtain the initial location faster and after obtaining it we change it to 3.0. We are also using apriltags, but the times it failed the tags were not visible.

The commits we are currently using are:

rtabmap commit e619c577595ad221890d550ea3dbe6cbdebe9502
rtabmap_ros commit 2f365ab7e10025c4d13b7855dfb4b7c8bb01f2d3
gtsam 4.2.0  commit 4f66a491ffc83cf092d0d818b11dc35135521612

Let me know what other information might be useful or if you have any ideas how we could replicate it, thanks.

matlabbe commented 9 months ago

If RGBD/OptimizeMaxError needs to be 0 to accept the first localization, there is already something wrong. There should be an error/warning about localization rejected because error is too high.

You could play with RGBD/LocalizationPriorError, RGBD/MaxOdomCacheSize and RGBD/LocalizationSmoothing parameters instead. It is strange though that rtabmap would need to be restarted. I've seen this problem when a different robot than the one used for mapping had slightly camera calibration scale issue, then in some spots in the map that error would happen, but the robot would eventually get a localization accepted when moving to another zone.

To disable localization optimization based on multiple constraints, set RGBD/MaxOdomCacheSize to 0. The robot may jump more though, as it won't be constrained by past odometry and loop closure observations.

Parameter RGBD/LocalizationPriorError was introduced for a similar error. See comment https://github.com/introlab/rtabmap_ros/issues/1057#issuecomment-1793878306

To debug more, I would need the debug log leading to that error (when rtabmap node is launched with --udebug argument).

brayanpa commented 6 months ago

After adding a Lidar and changing Reg/Strategy' to 1 it has not happened again.