Edge_V_V_GICP connecting two [VertexSE3Expmap] leads to optimization failure.

[LongruiDong]

Hi, 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...

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? and How to correctly use [VertexSE3Expmap] as two connecting Vertex in Edge_V_V_GICP ?

[Patrixe]

Facing something similar, did you find out what went wrong?