covariance of the g2o optimzation

[DarrenWong]

We want to evaluate the g2o optimization result, try to add the below code to compute the covariance of the last keyframe refer to https://github.com/RainerKuemmerle/g2o/blob/4b9c2f5b68d14ad479457b18c5a2a0bce1541a90/g2o/examples/target/static_target.cpp#L107
The GTSAM have a similar interface will have a fine result.
But the covariance matrix computed by g2o seems not correct as it is smaller as time goes on.

Do you have any suggestions if we want to get the covariance of last keyframe after g2o optimization?

add code after

hdl_graph_slam/apps/hdl_graph_slam_nodelet.cpp

Line 582 in b1d9b51

const auto& keyframe = keyframes.back();

   g2o::SparseBlockMatrix<Eigen::MatrixXd> spinv;

    graph_slam->computeMarginals(spinv, keyframe->node)
    g2o::SparseBlockMatrix<Eigen::MatrixXd>::SparseMatrixBlock* b = spinv.blockCols()[spinv.blockCols().size()-1].begin()->second;
    auto inverse_b = b->inverse();
    std::cout << "covariance\n" << spinv << std::endl;
    std::cout<< inverse_b<<std::endl; 
    

the result at the beginning

the result after a few state( 400+), the covariance of the pose(the diagonal) is very small

[koide3]

The covariance can be gets smaller after closing some loops and adding some other constraints (e.g., GPS). I've not tried to obtain marginals with g2o, and I'm not really sure, but I think your result may be correct.

[DarrenWong]

Thanks for your reply. But it seems too small... std 0.0009 if represents the translation. Is it possible the top-left (3x3) is the rotation std and the right bottom(3x3) is the translation std?

[koide3]

I got it. It's in the form of information matrix. You can take the inverse of it to get the distribution.

http://docs.ros.org/en/fuerte/api/re_vision/html/structg2o_1_1SparseOptimizer.html#a501617cdf217f9f50d16de939af46855

[DarrenWong]

I got it. It's in the form of information matrix. You can take the inverse of it to get the distribution.

http://docs.ros.org/en/fuerte/api/re_vision/html/structg2o_1_1SparseOptimizer.html#a501617cdf217f9f50d16de939af46855

Yes I have already inverted the output as the output is the information matrix. . But the result of inverse's std is very small 0.0009....
auto inverse_b = b->inverse();

[DarrenWong]

thanks this should be ok to recover the covariance, might be I assume the top left 3x3 belongs to the rotation should be fine~

[ahmedadamji]

graph_slam->computeMarginals

Hello, I have a problem with this line in the code, I get an error that there is no function computeMarginals. (which i verified by checking the graph_slam.cpp file), my question is how did you make this work

[DarrenWong]

Sorry for the late reply. You might need to change to sparse_opt solver to use the computeMarginals. You can refer to the git diff as shown below.

[ahmedadamji]

Sorry for the late reply. You might need to change to sparse_opt solver to use the computeMarginals. You can refer to the git diff as shown below.

Thank you so much Darren, this worked for me, do you also have any idea how i can access the individual covariences of observed points from scan matching, as this gives the covarience of the vehicle vertex?

[DarrenWong]

Might be you can refere to the code here for GICP point's covariance. https://github.com/PointCloudLibrary/pcl/blob/4766f4146b792ef833c773d35157fcc9c668110b/registration/include/pcl/registration/gicp.h#L388
or revise PCL's getFitnessScore to check the residual of the corresponding points.

[ahmedadamji]

Might be you can refere to the code here for GICP point's covariance. https://github.com/PointCloudLibrary/pcl/blob/4766f4146b792ef833c773d35157fcc9c668110b/registration/include/pcl/registration/gicp.h#L388 or revise PCL's getFitnessScore to check the residual of the corresponding points.

Thank you Darren!