Inverse fail (numeric decomposition)

[hridaybavle]

Hi all,

I need to compute the covariance matrices of the landmarks and I am trying to use the computeMarginals() function in order to compute them. I when I pass any kind of vertex pair to the function I get the error Inverse fail (numeric decomposition)

This is the code I took from the g2o core:

std::vector<std::pair<int, int> > blockIndices; for (size_t i=0; i<graph.activeVertices().size(); i++){ g2o::OptimizableGraph::Vertex* v=graph.activeVertices()[i]; if (v->hessianIndex()>=0){ blockIndices.push_back(std::make_pair(v->hessianIndex(), v->hessianIndex())); } if (v->hessianIndex()>0){ blockIndices.push_back(std::make_pair(v->hessianIndex()-1, v->hessianIndex())); } }

g2o::SparseBlockMatrix<Eigen::MatrixXd> spinv;
if (graph.computeMarginals(spinv, blockIndices)) {
    for (size_t i=0; i<graph.activeVertices().size(); i++){
        g2o::OptimizableGraph::Vertex* v=graph.activeVertices()[i];
        std::cerr << "Vertex id:" << v->id() << std::endl;
        if (v->hessianIndex()>=0){
            std::cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< std::endl;
            std::cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex()));
            std::cerr << std::endl;
        }
        if (v->hessianIndex()>0){
            std::cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< std::endl;
            std::cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
            std::cerr << std::endl;
        }
    }
}

Any idea what could be reason of this error?

Here is my simplified .g2o file

VERTEX_TRACKXYZ 0 0 0 0 VERTEX_TRACKXYZ 1 2 0 0 VERTEX_TRACKXYZ 2 4 0 0 VERTEX_TRACKXYZ 3 6 0 0 VERTEX_TRACKXYZ 4 8 0 0 VERTEX_TRACKXYZ 5 10 0 0 VERTEX_TRACKXYZ 6 12 0 0 VERTEX_TRACKXYZ 7 14 0 0 VERTEX_TRACKXYZ 8 16 0 0 VERTEX_TRACKXYZ 9 18 0 0 EDGE_POINTXYZ 0 1 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 1 2 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 2 3 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 3 4 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 4 5 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 5 6 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 6 7 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 7 8 2 0 0 1 0 0 1 0 1 EDGE_POINTXYZ 8 9 2 0 0 1 0 0 1 0 1