获取g2o优化后顶点协方差

    ifstream fin(input_str.c_str());

    if (!fin)
    {
        cout << "File " << input_str.c_str() << " does not exist!" << endl;
        return;
    }

    std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver(new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>());
    std::unique_ptr<g2o::BlockSolverX> blockSolver(new g2o::BlockSolverX(std::move(linearSolver)));     // 矩阵块求解器

    g2o::OptimizationAlgorithmLevenberg* solver_algorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver_algorithm);

    int vertexCnt = 0, edgeCnt = 0;
    while (!fin.eof())
    {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT")
        {
            g2o::VertexSE3* v = new g2o::VertexSE3();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            if (index == 0) v->setFixed(true);
        }
        else if (name == "EDGE_SE3:QUAT")
        {
            g2o::EdgeSE3* e = new g2o::EdgeSE3();
            int idx1 = 0, idx2 = 0;
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
        }
        if (!fin.good()) break;
    }

    cout << "Read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "Prepare optimization ..." << endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    cout << "Start optimation ..." << endl;
    optimizer.optimize(200);
    cout << "Saving optimization results ..." << endl;

    std::vector<std::pair<int, int> > indices;
    for (size_t i = 0; i < optimizer.vertices().size(); ++i)
    {
        const g2o::VertexSE3* v = static_cast<g2o::VertexSE3*> (optimizer.vertices()[i]);

        if(v == nullptr)
        {
            std::cout << v->hessianIndex() << "顶点是null" << std::endl;
            continue;
        }
        if(v->fixed())
        {
            std::cout << v->hessianIndex() << "顶点固定" << std::endl;
            continue;
        }

        std::cout << v->hessianIndex() << std::endl;

        indices.push_back(std::pair<int, int>(v->hessianIndex(), v->hessianIndex()));
    }

    g2o::SparseBlockMatrix<Eigen::MatrixXd> spinv;
    if (optimizer.computeMarginals(spinv, indices))
    {
        //for check
        std::cout << spinv << std::endl;
        const auto* b = spinv.blockCols()[0].begin()->second;
        std::cout << *b << std::endl;

        double cov_array[36]
        {
            (*b)(0,0),(*b)(0,1),(*b)(0,2),(*b)(0,3),(*b)(0,4),(*b)(0,5),
            (*b)(1,0),(*b)(1,1),(*b)(1,2),(*b)(1,3),(*b)(1,4),(*b)(1,5),
            (*b)(2,0),(*b)(2,1),(*b)(2,2),(*b)(2,3),(*b)(2,4),(*b)(2,5),
            (*b)(3,0),(*b)(3,1),(*b)(3,2),(*b)(3,3),(*b)(3,4),(*b)(3,5),
            (*b)(4,0),(*b)(4,1),(*b)(4,2),(*b)(4,3),(*b)(4,4),(*b)(4,5),
            (*b)(5,0),(*b)(5,1),(*b)(5,2),(*b)(5,3),(*b)(5,4),(*b)(5,5)
        };
    }