Science China Information Sciences

, Volume 53, Issue 8, pp 1682–1696 | Cite as

Monocular vision-based iterative pose estimation algorithm from corresponding feature points

Research Papers


To determine the relative pose between an object and a single camera using 2D-to-3D point correspondences, a kind of iterative methods based on inverse projection ray approach is proposed. An iterative algorithm which is divided into depth recovery stage and absolute orientation stage is also proposed. In the first stage, the optimal translation vector is first computed in terms of rotation matrix via least square method, then the depths of the observed points are estimated by projecting the estimated point orthogonally to the inverse projection ray defined by the image point, and finally 3D points are reconstructed using the estimated depths from previous step. In the second stage, the optimal rotation matrix is estimated by applying Umeyama algorithm to fitting of the 3D model points and 3D estimated points. The above two stages are repeated until the result converges. The global convergence of the two-stage iterative algorithm is proven based on the global convergence theorem. Finally, a spacecraft docking application is implemented to test the effectiveness and convergence of the proposed algorithm by mathematical simulation and physical simulation.


monocular vision feature point relative pose two-stage iterative algorithm global convergence 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. 1.
    Philip N K, Ananthasayanam M R. Relative position and attitude estimation and control schemes for the final phase of an autonomous docking mission of spacecraft. Acta Astronaut, 2003, 52: 511–522CrossRefGoogle Scholar
  2. 2.
    Wu Y H, Hu Z Y. PnP problem revisited. J Math Imag Vision, 2006, 24: 131–141CrossRefMathSciNetGoogle Scholar
  3. 3.
    Gao X S, Hou X R, Tang J, et al. Complete solution classification for the perspective-three-point problem. IEEE Trans Patt Anal Mach Intell, 2003, 25: 930–943CrossRefGoogle Scholar
  4. 4.
    Hu Z Y, Wu C F. A note on the number of solutions of the noncoplanar P4P problem. IEEE Trans Patt Anal Mach Intell, 2002, 24: 550–555CrossRefGoogle Scholar
  5. 5.
    Nister D. An efficient solution to the five-point relative pose problem. IEEE Trans Patt Anal Mach Intell, 2004, 26: 756–770CrossRefGoogle Scholar
  6. 6.
    Quan L, Lan Z D. Linear n-point camera pose determination. IEEE Trans Patt Anal Mach Intell, 1999, 21: 1–7Google Scholar
  7. 7.
    Tang J L, Chen W S, Wang J. A novel linear algorithm for P5P problem. Appl Math Comput, 2008, 205: 628–634MATHCrossRefMathSciNetGoogle Scholar
  8. 8.
    Ansar A, Daniilidis K. Linear pose estimation from points or lines. IEEE Trans Patt Anal Mach Intell, 2003, 25: 578–589CrossRefGoogle Scholar
  9. 9.
    Kúkelová Z, Bujńák M, Pajdla T. Polynomial eigenvalue solutions to the 5-pt and 6-pt relative pose problems. In: BMVC 2008: Proceedings of the 19th British Machine Vision Conference. London: British Machine Vision Association, 2008. 565–574Google Scholar
  10. 10.
    Paul D F. Efficient linear solution of exterior orientation. IEEE Trans Patt Anal Mach Intell, 2001, 23: 140–148CrossRefGoogle Scholar
  11. 11.
    Lepetit V, Moreno-Noguer F, Fua P. EPnP: An accurate O(n) solution to the PnP problem. Int J Comput Vision, 2009, 81: 155–166CrossRefGoogle Scholar
  12. 12.
    Lee P Y, Moore J B. Gauss-Newton-on-manifold for pose estimation. J Indust Manag Opt, 2005, 1: 565–587MATHMathSciNetGoogle Scholar
  13. 13.
    Dementhon D, Davis L S. Model-based object pose in 25 lines of code. Int J Comput Vision, 1995, 15: 123–141CrossRefGoogle Scholar
  14. 14.
    Haralick R M, Joo H, Lee C N, et al. Pose estimation from corresponding point data. IEEE Trans Syst Man Cybernet, 1989, 19: 1426–1446CrossRefGoogle Scholar
  15. 15.
    Lu C P, Hager G D, Mjolsness E. Fast and globally convergent pose estimation from video images. IEEE Trans Patt Anal Mach Intell, 2000, 22: 610–622CrossRefGoogle Scholar
  16. 16.
    Schweighofer G, Pinz A. Robust pose estimation from a planar target. IEEE Trans Patt Anal Mach Intell, 2006, 28: 2024–2030CrossRefGoogle Scholar
  17. 17.
    Schweighofer G, Pinz A. Globally optimal O(n) solution to the PnP problem for general camera models. In: Everingham M, Needham C J, eds. Proceedings of the 19th British Machine Vision Conference. London: BMVA Press, 2008. 1–10Google Scholar
  18. 18.
    Faugeras O D, Hebert M. A 3-d recognition and positioning algorithm using geometric matching between primitive surfaces. Int Robot Res, 1986, 5: 27–52CrossRefGoogle Scholar
  19. 19.
    Horn B K P. Closed-form solution of absolute orientation using unit quaternion. J Opt Soc Am A, 1987, 4: 629–642CrossRefGoogle Scholar
  20. 20.
    Arun K S, Huang T S, Blostein S D. Least-squares fitting of two 3-d point sets. IEEE Trans Patt Anal Mach Intell, 1987, 9: 698–700CrossRefGoogle Scholar
  21. 21.
    Umeyama S. Least-squares estimation of transformation parameters between two point patterns. IEEE Trans Patt Anal Mach Intell, 1991, 13: 367–380Google Scholar
  22. 22.
    Luenberger D G, Ye Y Y. Linear and Nonlinear Programming. 3rd ed. New York: Springer, 2008. 201–208MATHGoogle Scholar
  23. 23.
    Zhang S J, Cao X B. Physical simulation system for spacecraft rendezvous and docking based on microSim (in Chinese). Aerospace Control, 2006, 24: 63–67, 96Google Scholar

Copyright information

© Science China Press and Springer-Verlag Berlin Heidelberg 2010

Authors and Affiliations

  1. 1.Research Center of Satellite TechnologyHarbin Institute of TechnologyHarbinChina
  2. 2.Shanghai Aerospace Control Engineering InstituteShanghaiChina

Personalised recommendations