Retrieving the uncertainty of a vertex in g2o

I am manipulating the tutorial_slam2d example in the g2o package. I would like to output the marginal uncertainty of each vertex. However, using the following code snippet produces results that don't look right:

SparseOptimizer optimizer;
...
ofstream g2oOptResults;
string g2oFileID = "g2oResults__" + fileID + ".txt";

g2oOptResults.open(g2oFileID.c_str());

for (int i=0; i<5; i++)
{
VertexSE2* testPose = dynamic_cast<VertexSE2*>(optimizer.vertex(i));
Matrix<double,3,3> uncertainty = testPose->uncertainty();

g2oOptResults << uncertainty << endl;
g2oOptResults << " " << endl;
}

g2oOptResults.close();


The problem is the returned matrix clearly cannot represent a covariance matrix; it is not symmetric. As a typical example I get:

0.325765, 0.919874, 0.919874
-0.00215401, 0.00202767, 0.00202767
0.0196631, 0.00424371, 0.00424371

The values look like they might belong to a symmetric matrix (i.e. I get three unique values and three pairs of identical values). They are just not arranged correctly.

Any thoughts?

Edit: on a related note, the vertices in optimizer (instance of SparseOptimizer) have an uncertainty associated with them _before_ optimization. It doesn't seem that should happen; I don't tell optimizer anything about uncertainty in the vertices, just the fixed edges.

(Sorry this is so lengthy)

edit retag close merge delete

Eric, could you specify what you mean by "this"? What part is it you mean has been removed? The whole tutorial_slam2d? A specific method of one of the classes?

( 2012-07-19 04:49:27 -0500 )edit

Ah sorry for being unclear. I meant the uncertainty associated with a given vertex (which is the entire focus of your question). I could find no mention of it when I looked through the current development version. No uncertainty() member and I didn't see the uncertainty marginalization functionality

( 2012-07-19 08:23:45 -0500 )edit

Alright, so I see that the uncertainty() method doesn't exist in the github version... The version available from https://code.ros.org/svn/ros-pkg/stacks/vslam/trunk/g2o points back to the OpenSLAM.org version, which they describe as stable. Anyone guess when is the next stable release?

( 2012-07-19 08:31:09 -0500 )edit

No idea. You may want to email one of people working on that Github for more info.

( 2012-07-19 09:31:54 -0500 )edit

Hi jcm76, Did you ever get an answer regarding this? Thanks!

( 2015-01-08 03:26:04 -0500 )edit

Sort by ยป oldest newest most voted

This seems to have been removed from the current development versions of g2o here: https://github.com/RainerKuemmerle/g2o so perhaps there are bugs with the version of g2o ROS is using?

more