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)
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?
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
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?
No idea. You may want to email one of people working on that Github for more info.
Hi jcm76, Did you ever get an answer regarding this? Thanks!