Retrieve posterior uncertainty after optimisation?

[miquelmassot]

Hi!

I'd like to know if there's a way to retrieve the posterior uncertainty (or information matrix) after optimisation has been done. I can't seem to find information about this, although some old documentation seemed to state the method .covariance() on BaseVertex class, which no longer exists. Does anyone have information about this?

[RainerKuemmerle]

After optimization has been run, one can retrieve the estimate of the uncertainty (as approximated by the Hessian) via computeMarginals on the SparseOptimizer class.
I admit that documentation is outdated. In the past, the covariance was stored on each vertex which caused a high memory footprint.
Now instead a sparse matrix has to be passed for storing this information on demand. You have to consider that the covariance is expressed in the manifold space.

The class below shows research code for computing a condensed graph given a set of nodes/edges

https://github.com/RainerKuemmerle/g2o/blob/master/g2o/examples/g2o_hierarchical/edge_labeler.cpp
Paper: http://ais.informatik.uni-freiburg.de/publications/papers/grisetti10icra.pdf

Furthermore, note that computing the inverse takes significantly more time as just solving the system.

[miquelmassot]

Thanks! I'll have a look and see if I can reach that through the python wrapper.

[tudoroancea]

Hi @miquelmassot ! Did you end up finding a solution to you problem with the python wrapper? I am interested as well

[RainerKuemmerle]

In #685 I fixed the slam2d example for the master branch. It shows how to pass the sparse matrix for storage of the covariance and retrieving it again afterwards.

[miquelmassot]

Thanks @RainerKuemmerle, I'll see if I can do the same through the python wrapper.

[miquelmassot]

This is what I came up with:

from g2o import SparseBlockMatrixX

vertices = graph_slam.optimizer.vertices()

cov_vertices = []
covariances = SparseBlockMatrixX()
for vertex_idx in vertices:
    v = vertices[vertex_idx]
    if not v.fixed():
        cov_vertices.append((v.hessian_index(), v.hessian_index()))
print("Sparse pattern contains", len(cov_vertices), "blocks")

print("Computing covariance matrix...")
m = graph_slam.optimizer.compute_marginals(covariances, cov_vertices)
print("Done,", m)

print("Covariance matrix:")
for vertex_idx in vertices:
    print("Working on vertex idx:", vertex_idx)
    v = vertices[vertex_idx]
    if not v.fixed():
        print("Hessian idx is:", v.hessian_index())
        covariance = covariances.block(v.hessian_index(), v.hessian_index());
        print("Covariance is:", covariance)

and the results I see are:

1. The bool return of compute_marginals is False - is this correct?
2. I can retrieve the hessian indexes, but any access to the covariances matrix segfaults the program.

In the example I have, there are 100 2D vertices and 15 PointXY observations.

Sparse pattern contains 100 blocks
Computing covariance matrix...
Done, False
Covariance matrix:
Working on vertex idx: 114
Working on vertex idx: 113
Working on vertex idx: 112
Working on vertex idx: 111
Working on vertex idx: 110
Working on vertex idx: 109
Working on vertex idx: 108
Working on vertex idx: 107
Working on vertex idx: 106
Working on vertex idx: 105
Working on vertex idx: 104
Working on vertex idx: 103
Working on vertex idx: 102
Working on vertex idx: 101
Working on vertex idx: 100
Working on vertex idx: 99
Hessian idx is: 99
Segmentation fault (core dumped)

[miquelmassot]

Full working example (that is not capable to retrieve the covariance) here: https://github.com/miquelmassot/g2o-python-examples/blob/main/g2o%20covariance%20retrieval.ipynb

[RainerKuemmerle]

I believe my drawing function is broken but extracting the values seems to work.
I wonder what function was called beforehand. The compute_marginals was not wrapped on my pymem branch.
I pushed the branch I have been working on so far.

[RainerKuemmerle]

Ok, this looks now better after rotating the covariance in a more proper way.

[RainerKuemmerle]

@miquelmassot I updated https://github.com/RainerKuemmerle/g2o/blob/pymem/python/examples/notebook_slam2d.ipynb accordingly to plot the covariance of a PoseGraph.

[miquelmassot]

Thanks, @RainerKuemmerle that's brilliant!
I have tried running your notebook with my g2o-python and am running into issues related to how the library is packaged in Python (PyPI) - so I need to solve that first and then I'll generate your same example on my side too.

With the current setup, I get the following error when loading the manhattanOlson3500.g2o file

virtual bool g2o::OptimizableGraph::load(std::istream&) unknown type: VERTEX_SE2
virtual bool g2o::OptimizableGraph::load(std::istream&) unknown type: EDGE_SE2

[RainerKuemmerle]

For me the loading by the factory works because I link all types to the g2opy SO. Is this different in the g2o-python build?
I noticed that the types factory itself is also not wrapped to python which means we cannot query the known types in advance to better understand the error.

[miquelmassot]

I am calling the same CMakeLists to wrap g2opy as g2o-python, so there shouldn't be any difference, but there is. I am still trying to find out why.

[RainerKuemmerle]

Ha, I referenced a wrong issue in the above commit :(

@miquelmassot I added also some wrapper for the factory itself. Does this report empty lists for you?
https://github.com/RainerKuemmerle/g2o/blob/pymem/python/core/py_factory.cpp#L23

[github-actions]

This issue is stale because it has been open 60 days with no activity. Remove stale label or comment or this will be closed in 14 days.

[github-actions]

This issue was closed because it has been stalled for 14 days with no activity.