rtabmap_ros: "GTSAM exception caught: Indeterminant linear system detected while working near variable" then crash
Hi everyone, I’m working in a project with ROS1 in which we use Rtabmap for localization in a previously mapped environment. I’m using the localization_pose to feed my localization stack. Recently I have experienced problems with GTSAM which warns for “inderteminant linear systems”. Eventually, it also crashes with the following FATAL ERROR.
FATAL] (2023-10-27 09:25:31.539) Transform.cpp:184::inverse() Condition (invertible) not met! [This transform is not invertible! xyz=inf,inf,0.000000 rpy=0.000000,0.000000,-1.267575
[0.298596 0.954380 0.000000 inf;
-0.954380 0.298596 -0.000000 inf;
-0.000000 0.000000 1.000000 0.000000;
0 0 0 1]]
terminate called after throwing an instance of 'UException'
what(): [FATAL] (2023-10-27 09:25:31.539) Transform.cpp:184::inverse() Condition (invertible) not met! [This transform is not invertible! xyz=inf,inf,0.000000 rpy=0.000000,0.000000,-1.267575
[0.298596 0.954380 0.000000 inf;
-0.954380 0.298596 -0.000000 inf;
-0.000000 0.000000 1.000000 0.000000;
0 0 0 1]]
By setting the “–udebug --logconsole” arguments I was able to see some information about the problem. However something very strange took my attemption, which are the following debug messages:
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2946::process() Constraint -8->-8 (type=PosePrior)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2948::process() Constraint 1058->-8 (type=Landmark, var = 0.001000 0.010000)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2946::process() Constraint 875->875 (type=PosePrior)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2948::process() Constraint 1058->875 (type=LocalSpaceClosure, var = 0.000003 0.002736)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2946::process() Constraint 876->876 (type=PosePrior)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2948::process() Constraint 1058->876 (type=GlobalClosure, var = 0.000002 0.001697)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2946::process() Constraint 1033->1033 (type=PosePrior)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2948::process() Constraint 1058->1033 (type=LocalSpaceClosure, var = 0.000027 0.007702)
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2952::process() Pose -8 xyz=0.851523,-0.041897,0.404141 rpy=0.085773,0.006991,1.561444
[DEBUG] (2023-10-27 09:25:31.535) Rtabmap.cpp:2952::process() Pose 875 xyz=0.319759,-0.040769,0.000000 rpy=0.000000,-0.000000,0.013201
[DEBUG] (2023-10-27 09:25:31.536) Rtabmap.cpp:2952::process() Pose 876 xyz=0.297985,-0.013632,0.000000 rpy=0.000000,0.000000,-0.039805
[DEBUG] (2023-10-27 09:25:31.536) Rtabmap.cpp:2952::process() Pose 1033 xyz=0.240514,-0.004088,0.000000 rpy=0.000000,-0.000000,0.023084
[DEBUG] (2023-10-27 09:25:31.536) Rtabmap.cpp:2952::process() Pose 1058 xyz=-197.406799,29.594700,0.000000 rpy=0.000000,0.000000,-3.107898
The above logs, show the constraints used in the optmization. However the last line took my attemption. In the referred line we can see that the pose of the node 1058 is very distant (in terms of xyz) from the other nodes of the graph. Then, I was looking in the code and found these debug messages in the Rtabmap library. I noticed that the odom pose is not being transformed to the same frame of the graph nodes before optimization, as shown in the following lines:
Based in my knowledge of optimization, I thought it would be better for the optimization problem (in terms of speed and accuracy) if the initial guess was near to the minimum. I understand that the mapCorrection output is being used to generate the mapToOdom information. However, in my point of view, it would be better for the optimization problem, if the odom pose was transformed to the map frame with the last map->odom transform before passing it to the optimizer. Then the mapCorrection output could be used to correct the last map->odom TF from /tf.
I would appreciate any comments and clarifications on this. Thank you!
About this issue
- Original URL
- State: closed
- Created 8 months ago
- Comments: 16 (9 by maintainers)
Commits related to this issue
- tuning localization priors (for https://github.com/introlab/rtabmap_ros/issues/1057) — committed to introlab/rtabmap by matlabbe 8 months ago
- Tuning localization priors (added RGBD/LocalizationPriorError parameter) (#1156) * tuning localization priors (for https://github.com/introlab/rtabmap_ros/issues/1057) * Set prior error value to s... — committed to introlab/rtabmap by matlabbe 8 months ago
- Localization: Refactored getConnectedGraph() for 2d slam and tags (https://github.com/introlab/rtabmap_ros/issues/1057). Transform: Fixed is3DoF() and is 4DoF() — committed to introlab/rtabmap by matlabbe 7 months ago
I updated the code with a change for landmark constraints in localization mode that could help with your issue (see https://github.com/introlab/rtabmap/pull/1156). You may give a try. If the issue is still there, you may increase this new parameter called
RGBD/LocalizationPriorErrorto0.01or0.1.Hi, I have some updates again.
I made the tests with the commit above and finally the issue was solved. The system is working perfectly and everything is great.
Thanks for the help and explanations about this awesome package/library.
The odometry topic covariance looks fine, but if you are using
odom_frame_id, those parameters are used:which should not cause that issue with those default values.
In the commit above, I updated the code to remove the last argument of
getConnectedGraph()so this would fix the current issue. I won’t “fix” the problem of switchingReg/Force3DoFfrom true to false or vice-versa when the map contains landmarks, as the changes would be quite deep to do and I think the current code is more efficient in its current state.I removed the last argument of that function and simplified the code. I think I reproduced the issue of switching from a map created with
Reg/Force3DoF=true, then try to localize withReg/Force3DoF=false, this make the robot flips upside down. It is because the tags were transformed for 3DoF optimization, which is not the real pose for 6DoF. Here are two examples of the same tag when mapping withReg/Force3DoF=trueandReg/Force3DoF=falserespectively (the real orientation is the second one, with z axis perpendicular to the tag):To make 2D optimization working, as it optimizes only x,y, and yaw, the orientation of the tag should have z-axis up. The bug is that after optimization, we don’t flip back the correct orientation of the tag, so if we switch to 6DoF in localization, the robot will think the tag is flipped 90 deg. I need to dig more into this in the next days.
I will try to reproduce the issue that this commit was fixing, I don’t remember on top of my head what was the issue addressed there. In your case, if I understand, if you change this line by:
Would it fix your issue?
Just confirming that even if odometry pose is large, GTSAM should be able to converge correctly. Here I reset odometry to x=10000 and y=8000:
Maybe the tag detection is too wrong to correctly converge.