Pose Optimization

[aggelen]

I'm trying to implement a simple pose optimizer with 3 frames. I've calculated 3D points by triangulation. Then, I projected some of these points onto the 3rd frame. I have 3D pts. and projected points. I need to refine the camera pose at third frame. I've already looked through the examples but I couldn't figure out how to optimize it.

• Added a vertex as VertexSE3Expmap with a predicted pose for 3rd frame. (Vertex ID 0)

• Added all 3d points as VertexSBAPointXYZ. (Vertex IDs 1, 2, ...)

• Added an edge for all projected points as EdgeProjectXYZ2UV wtih

  • set_vertex(0, vertex(i+1)) -> looping with i
  • set_vertex(1, vertex(0))
  • set_parameter_id(0, 0)

I'm not sure if this is the right method. It doesn't work for my data. It is like the image below. Green: actual point, red: projected point with predicted pose, magenta: projected point with optimized pose. Can you suggest anything? Thanks!

Verbose Output for 10 iter:
iteration= 0 chi2= 29.056211 time= 0.00258552 cumTime= 0.00258552 edges= 1935 schur= 1 lambda= 440.950594 levenbergIter= 1
iteration= 1 chi2= 22.221547 time= 0.00128042 cumTime= 0.00386594 edges= 1935 schur= 1 lambda= 146.983531 levenbergIter= 1
iteration= 2 chi2= 15.924198 time= 0.0012289 cumTime= 0.00509483 edges= 1935 schur= 1 lambda= 48.994510 levenbergIter= 1
iteration= 3 chi2= 10.866332 time= 0.00117517 cumTime= 0.00627001 edges= 1935 schur= 1 lambda= 16.331503 levenbergIter= 1
iteration= 4 chi2= 7.161353 time= 0.00117359 cumTime= 0.00744359 edges= 1935 schur= 1 lambda= 5.443834 levenbergIter= 1
iteration= 5 chi2= 4.878983 time= 0.00118739 cumTime= 0.00863098 edges= 1935 schur= 1 lambda= 1.814611 levenbergIter= 1
iteration= 6 chi2= 3.324435 time= 0.0011277 cumTime= 0.00975869 edges= 1935 schur= 1 lambda= 0.604870 levenbergIter= 1
iteration= 7 chi2= 2.154175 time= 0.00114915 cumTime= 0.0109078 edges= 1935 schur= 1 lambda= 0.201623 levenbergIter= 1
iteration= 8 chi2= 1.328496 time= 0.00118647 cumTime= 0.0120943 edges= 1935 schur= 1 lambda= 0.067208 levenbergIter= 1
iteration= 9 chi2= 0.794137 time= 0.00113145 cumTime= 0.0132258 edges= 1935 schur= 1 lambda= 0.022403 levenbergIter= 1
Total time elapsed: 1.6252547540025262

[shaoxh]

I still cannot use this package. when import an error shows up. #50
Do you have any idea how to tickle it?

[LukasBommes]

Take a look here: https://github.com/raulmur/ORB_SLAM2/blob/master/src/Optimizer.cc
The method Optimizer::PoseOptimization performs pose optimization.