I am working on a pose estimation problem solved by Levenberg-Marquardt with g2o.

I want to build 3D rigid constraint between two pose, so I choose Edge_V_V_GICP and test the g2o/examples/icp/gicp_demo.cpp .

I changed the Vertex class of Edge_V_V_GICP from VertexSE3 to VertexSE3Expmap, meanwhile I rewrite some code to make original Edge_V_V_GICP compatible with VertexSE3Expmap. Run gicp_demo.cpp, the Optimization failed early ( it should iterate 10 times )...

amax:~/Project/g2o/bin$ ./gicp_demo 
0 0 0 | 0 0 0 1
0 0 1 | 0 0 0 1
[VertexSE3Expmap] GICPEdge
Initial Second vertex =   0   0 0.2
Initial chi2 = 7.885232
iteration= 0     chi2= 7.885232  time= 0.00560661    cumTime= 0.00560661     edges= 1000     schur= 0    lambda= 135142887383192720.000000   levenbergIter= 10
Iteration = 10

Second vertex should be near 0,0,1
0 0 0
  0   0 0.2
Final chi2 = 7.885232

Because using original vertex Class: VertexSE3 lead to successful optimization, I guess something wrong with VertexSE3Expmap...

Here are involved codes which I changed to make original Edge_V_V_GICP compatible with VertexSE3Expmap: gicp_demo.cpp, types_icp.h, types_icp.cpp

I have been confused by this question for days, Anybody know how to fix this? How to correctly use [VertexSE3Expmap] as two connecting Vertex in Edge_V_V_GICP ?

I have also made an issue in g2o github...

Thanks in advance!!


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.