GTSAM Optimization Error

[g.bartoli]

Hi Mathieu,
it's me again... Now that I'm trying all the RTAB-Map possibilities, I hope that my reports can help you in further refining your software, which I sincerely think is a really powerful and complete SLAM system, I'm just trying to be of any help! :)

I'm trying the Robust Graph Optimization with GTSAM like explained here, and as soon as RTAB-Map finds the first loop closure this is the error I receive:

[ERROR] (2015-10-06 17:21:29.709) Graph.cpp:1323::optimize() GTSAM exception catched: 
Indeterminant linear system detected while working near variable
16 (Symbol: 16).

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.

I can reproduce this behaviour every time, so I enabled DEBUG level and these are the messages just before the error appears:

[ INFO] (2015-10-06 17:21:29.707) Memory.cpp:2836::addLink() to=16, from=37 transform: xyz=0,595137,0,259167,-0,112459 rpy=-0,010555,-0,094801,0,571400
[DEBUG] (2015-10-06 17:21:29.707) Memory.cpp:2848::addLink() Add link between 16 and 37
[DEBUG] (2015-10-06 17:21:29.707) Signature.cpp:98::addLink() Add link 37 to 16 (type=1)
[DEBUG] (2015-10-06 17:21:29.707) Signature.cpp:98::addLink() Add link 16 to 37 (type=1)
[ INFO] (2015-10-06 17:21:29.707) Rtabmap.cpp:1824::process() timeAddLoopClosureLink=0,089908s
[ INFO] (2015-10-06 17:21:29.707) Rtabmap.cpp:2067::process() timeLocalSpaceDetection=0,000014s
[ INFO] (2015-10-06 17:21:29.707) Rtabmap.cpp:2109::process() Update map correction
[ INFO] (2015-10-06 17:21:29.707) Rtabmap.cpp:2820::optimizeCurrentMap() Optimize map: around location 37
[ INFO] (2015-10-06 17:21:29.707) Rtabmap.cpp:2829::optimizeCurrentMap() get 37 ids time 0,000032 s
[DEBUG] (2015-10-06 17:21:29.707) Rtabmap.cpp:2861::optimizeGraph() ids=37
[DEBUG] (2015-10-06 17:21:29.707) Memory.cpp:4599::getMetricConstraints() 
[ INFO] (2015-10-06 17:21:29.707) Rtabmap.cpp:2863::optimizeGraph() get constraints (37 poses, 37 edges) time 0,000143 s
[DEBUG] (2015-10-06 17:21:29.707) Graph.cpp:1164::optimize() Optimizing graph...
[DEBUG] (2015-10-06 17:21:29.707) Graph.cpp:1183::optimize() fill poses to gtsam...
[DEBUG] (2015-10-06 17:21:29.707) Graph.cpp:1198::optimize() fill edges to gtsam...
[DEBUG] (2015-10-06 17:21:29.707) Graph.cpp:1278::optimize() create optimizer
[ INFO] (2015-10-06 17:21:29.707) Graph.cpp:1292::optimize() GTSAM optimizing begin (max iterations=150, robust=1)

And these are just after the error:

[ INFO] (2015-10-06 17:21:29.709) Rtabmap.cpp:2880::optimizeGraph() Optimization time 0,002134 s
[ INFO] (2015-10-06 17:21:29.709) Rtabmap.cpp:2832::optimizeCurrentMap() optimize time 0,002353 s
[ERROR] (2015-10-06 17:21:29.709) Rtabmap.cpp:2846::optimizeCurrentMap() Failed to optimize the graph! Keeping the graph without optimization...
[ INFO] (2015-10-06 17:21:29.709) Rtabmap.cpp:2163::process() Updated local map (old size=37, new size=37)
[ INFO] (2015-10-06 17:21:29.709) Rtabmap.cpp:2179::process() timeMapOptimization=0,002699s
[ INFO] (2015-10-06 17:21:29.710) Rtabmap.cpp:2206::process() sending stats...
[ INFO] (2015-10-06 17:21:29.710) Rtabmap.cpp:2211::process() Loop closure detected! With id=16
[ INFO] (2015-10-06 17:21:29.710) Rtabmap.cpp:2215::process() send all stats...
[ INFO] (2015-10-06 17:21:29.710) Rtabmap.cpp:2238::process() Set loop closure transform = xyz=-0,627234,0,102032,0,173694 rpy=-0,042479,0,085430,-0,572715
[ INFO] (2015-10-06 17:21:29.710) Rtabmap.cpp:2242::process() Set map correction = xyz=-0,000000,0,000000,0,000000 rpy=-0,000000,0,000000,0,000000
[ INFO] (2015-10-06 17:21:29.710) Rtabmap.cpp:2295::process() Time creating stats = 0,000332...
[DEBUG] (2015-10-06 17:21:29.710) Memory.cpp:1515::cleanup() 

If it can be of any help, using g2o optimization does not produce this error. If you need some more informations and tests, just tell me.

[matlabbe]

Hi,

Is this happened with the standalone or ROS? I tried again today (following the example with the standalone) with the latest GTSAM code and I have no problems. I have seen this error but on large multi-session databases with multiple maps. It is like there are not enough constraints for the unknowns in the graph ("underconstrained variables"). Make sure to use default parameters. You could export the parameters you are using here. I didn't test much GTSAM and g2o, so there are maybe some issues about using them. Just for this example, I've shown some issues here.

cheers

[g.bartoli]

It's happening with the standalone ROS and these are my settings where I just changed some odometry and loop-constraint parameters. Today I will make another test with default parameters and I will tell you about it.

However, after some test with g2o versus TORO, I must say that in the same environment TORO performs much better in optimizing the graph, the room walls results more perpendicular and the overall structure more stable. I used debug mode to inspect the iterations made by both algorithm, and TORO converge in about 20 iterations, while g2o needs about 50 iterations to meet the OptimizeEpsilon criterion, maybe these two methods needs a different value for that parameter.

[matlabbe]

I tried again on ROS too, and no problem. I tested also with your parameters and no error.

For the second part, there is effectively a bug with g2o+Vertigo (Robust graph optimization). I did a small mapping with a loop closure. If both are used, the map is like not optimized. If I uncheck Vertigo, the graph is correctly optimized. If I leave it checked but I check "Ignore covariance", the graph is correctly optimized. In the code, when Vertigo is activated, I add Vertigo constraints in the graph before optimization, with covariance of 1. Maybe g2o put all the graph error in these links without optimizing the others (if they have covariance < 1, or information matrix > 1). Well, there is a problem with setting covariance values and Vertigo, this would explain why I have the related covariance issues.

cheers

[matlabbe]

Well, just did another mapping and no problem this time with g2o+Vertigo. Maybe to solution is in why the graphes in the issues section are not all working.

[g.bartoli]

Ok, thanks for pointing that out, I will make some more experiment and let you know if I find some more info about this.