Problem with g2o and GTSAM in localization mode

[g.bartoli]

Hi Mathieu,
I'm doing some experiments with the other graph optimizers (g2o and GTSAM).

During the mapping phase, I have to say that in my setup g2o builds a more coherent graph than TORO, while GTSAM seems to perform about the same as TORO.
So, after a new map was built with g2o enabled, I reloaded it in localization mode and tried the navigation. Most of the time everything work as intended, but sometimes, as soon as the robot relocalizes itself using visual features, the optimizer gives this error:

CHOLMOD warning: not positive definite
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
[ WARN] [1447927845.454764623]: The origin for the sensor at (-2.78, 2.17) is out of map bounds. So, the costmap cannot raytrace for it.
[ERROR] (2015-11-19 10:53:12.406) util3d_mapping.cpp:255::create2DMapFromOccupancyLocalMaps() Large map size!!
map min=(-1519136538624.000000, -101776277504.000000) max=(5833464217600.000000,3109813485568.000000). There's maybe an error with the poses provided! The map will not be created!
[ WARN] [1447926792.406751896]: Grid map is empty! (local maps=95)

It seems that the graph has totally inconsistent coordinates and in RViz I see a sort of "exploded" projection of the map far away from the reference frame (in the following screenshot you can see the misaligned map and the green and blue lines vanishing into infinity):



If you want, I can also post the "debug.txt" created by CHOLMOD, however it is just a long sequence of ~4000 matrix coefficients.
On the other side, if it can be of any help for you, this is the correspondent error that I receive with GTSAM in localization mode:

[ERROR] (2015-11-19 15:23:44.420) Graph.cpp:1397::optimize() GTSAM exception catched: 
Indeterminant linear system detected while working near variable 462 (Symbol: 462).
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.
[ERROR] (2015-11-19 15:23:44.421) Rtabmap.cpp:2948::optimizeCurrentMap() Failed to optimize the graph! Keeping the graph without optimization...
[FATAL] (2015-11-19 15:23:44.422) Rtabmap.cpp:2489::process() Condition (uContains(_optimizedPoses, _lastLocalizationNodeId)) not met! [id=413]

Do you think this is a bug? Do you need some more informations about it?

Thanks,
Guido

[matlabbe]

Hi Guido,

g2o and GTSAM sometimes don't return a solution. I'll check how to handle these cases (I opened an issue).

In the mean time, in localization mode, optimization occurs only if there are nodes retrieved/transferred between LTM and WM. You could initialize the map with all nodes in WM and disable memory management (if it was enabled), so that optimization will be done only one time at the loading and there will be no optimization on localization:

In your localization launch file:
<param name="Mem/IncrementalMemory" type="string" value="false"/>  <!-- Localization mode -->
<param name="Mem/InitWMWithAllNodes" type="string" value="true"/>  <!-- Load all nodes in WM -->
<param name="Rtabmap/TimeThr" type="string" value="0"/>            <!-- Disable memory management -->

cheers

[g.bartoli]

Ok, thanks for taking a look at this.
If it can be of any help, the options you reported were already set like that in the localization launch file.

Have a nice day,
Guido

[matlabbe]

I commited a fix to handle the case when optimization fails. You can try that if it can help.

cheers

[g.bartoli]

Ok, thanks Mathieu, this week I will make some tests and let you know the outcomes!

Have a nice day,
Guido