1
$\begingroup$

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!!

$\endgroup$

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.