Skip to main content
SpringerLink
Log in

Formalization of Camera Pose Estimation Algorithm based on Rodrigues Formula

  • Original Article
  • Published:
Formal Aspects of Computing

Abstract

Camera pose estimation is key to the proper functioning of robotic systems, supporting critical tasks such as robot navigation, target tracking, camera calibration, etc.Whilemultiple algorithms solving this problem have been proposed, their correctness has rarely been validated using formal techniques. This is true despite the fact that the adoption of formal verification is essential for the reliability of safety-critical systems, and for their certification to high assurance levels. In this article, we present an effort in formally verifying an algorithm for camera pose estimation in an interactive theorem prover. The algorithm leverages the power of Rodrigues formula to solve the pose estimation problem under conditions for which existing solutions cannot be applied. The technical ingredients include (but are not limited to) mechanized proofs of the Rodrigues formula (along with its Cayley decomposition form) and the least squares method for fitting data. Based on the formalization of the algorithm, we formally derive and verify its general solution and unique solution.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Similar content being viewed by others

References

  1. Affeldt R, Cohen C (2017) Formal foundations of 3d geometry to model robot manipulators. In: ACM Sigplan conference on certified programs and proofs, pp 30–42

  2. Brockett, R.W.: Robotic manipulators and the product of exponentials formula. Springer, Berlin (1984)

    Book  Google Scholar 

  3. Curtright Thomas, L., Fairlie David, B., Zachos Cosmas, K.: A compact formula for rotations as spin matrix polynomials. High Energy Phys Div 10, 084 (2014)

    MathSciNet  MATH  Google Scholar 

  4. Yang, Guo: A novel solution to the p4p problem for an uncalibrated camera. J Math Imaging Vis 45(2), 186–198 (2013)

    Article  MathSciNet  Google Scholar 

  5. John, Harrison: Formal proof–theory and practice. Not Am Math Soc 55(11), 1–2 (2008)

    MathSciNet  MATH  Google Scholar 

  6. John, Harrison: The HOL light theory of euclidean space. J Autom Reason 50(2), 173–190 (2013)

    Article  MathSciNet  Google Scholar 

  7. Hall, B.C.: Lie groups, lie algebras, and representations. Springer (2013)

  8. Kamali, M., Dennis, L.A., McAree, O., Fisher, M., Veres, S.M.: Formal verification of autonomous vehicle platooning. Sci Comput Program 148, 88–106 (2017)

    Article  Google Scholar 

  9. Khan, S., Hasan, O., Mashkoor, A., (2018) Formal verification and safety assessment of a hemodialysis machine. SOFSEM, : volume 10706 LNCS, pp. 241–254. Krems, Austria (2018)

  10. Li B, Heng L, Lee GH, Pollefeys M (2013) A 4-point algorithm for relative pose estimation of a calibrated camera with a known relative rotation angle. In: IEEE/RSJ international conference on intelligent robots and systems

  11. Lepetit, V., Moreno-Noguer, F., Fua, P.: Epnp: an accurate o(n) solution to the pnp problem. Int J Comput Vis 81(2), 155–166 (2009)

    Article  Google Scholar 

  12. Luvizon DC, Picard D, Tabia H (2018) 2d/3d pose estimation and action recognition using multitask deep learning. In: Proceedings of the IEEE computer society conference on computer vision and pattern recognition. Salt Lake City, UT, United states, pp 5137–5146

  13. Murray, R.M., Shankar, S.S., Li, Z.: A mathematical introduction to robotic manipulation. CRC Press Inc (1994)

  14. Multivariable calculus libraries (2020) https://github.com/jrh13/hol-light/tree/master/Multivariate

  15. Noll, T., Pagani, A., Stricker, D.: Markerless camera pose estimation–an overview. OpenAccess Ser Inf 19, 45–54 (2010)

    Google Scholar 

  16. Peng C (2017) An iterative camera pose estimation algorithm based on epnp. In: Chinese intelligent automation conference

  17. Pavllo D, Feichtenhofer C, Grangier D, Auli M (2018) 3d human pose estimation in video with temporal convolutions and semi-supervised training

  18. Projections onto subspaces (2011). https://ocw.mit.edu/courses/mathematics/18-06sc-linear-algebra-fall-2011/least-squares-determinants-and-eigenvalues/projections-onto-subspaces/MIT18_06SCF11_Ses2.2sum.pdf

  19. Rashid A, Hasan O (2018) Formal modeling of robotic cell injection systems in higher-order logic. In: CEUR workshop proceedings, vol 2307, Hagenberg, Austria

  20. Roy, K.K.: Elements of vector analysis. Springer, Berlin (2008)

    Book  Google Scholar 

  21. Sun, C., Dong, H., Zhang, B., Wang, P.: An orthogonal iteration pose estimation algorithm based on an incident ray tracking model. Meas Sci Technol 29(9), (2018)

  22. Schaub, H., Junkins, J.L.: Aas 95–137 stereographic orientation parameters for attitude dynamics: a generalization of the rodrigues parameters. J Astron Sci 44, 13–15 (1996)

    Google Scholar 

  23. Schaub H, Tsiotras P, Junkins JL (1995) Principal rotation representations of proper nxn orthogonal matrices. Int J Eng Sci 33(15):2277–2295. The Edelen Symposium

  24. Sun, F., Wang, B.: The solution distribution analysis of the p3p problem, pp. 2033–2036. Istanbul, Turkey (2010)

    Google Scholar 

  25. Shi Z, Yan Z, Liu Z, Kang X, Yong G, Jie Z, Song X (2014) Formalization of matrix theory in hol4. Adv Mech Eng (2014-8-28) 2014:195276–195276

  26. Tang, J., Chen, W., Wang, J.: A novel linear algorithm for p5p problem. Appl Math Comput 205(2), 628–634 (2008)

    MathSciNet  MATH  Google Scholar 

  27. Terzakis, G., Lourakis, M., Ait-Boudaoud, D.: Modified rodrigues parameters: an efficient representation of orientation in 3d vision and graphics. J Math Imaging Vis 2, 1–21 (2017)

    MATH  Google Scholar 

  28. Zhao Z (2014) Research on videometrics technology in the aircraft transformation collision avoidance system. Master thesis, National University of Defense Technology

Download references

Acknowledgements

This work was supported by the National Key R&D Plan (2019YFB1309900),National Natural Science Foundation of China (61876111, 61602325, 61877040), the National Technology Innovation Special Zone (18-163-11-ZT-005-038-05), Academy for Multidisciplinary Studies, Capital Normal University (19530012005), the Project of Beijing Municipal Education Commission (KM20190028005), and Capacity Building for Sci-Tech Innovation—Fundamental Scientific Research Funds (006195305000).

Author information

Authors and Affiliations

  1. Beijing Engineering Research Center of High Reliable Embedded System, Capital Normal University, Beijing, China

    Shanyan Chen & Guohui Wang

  2. Beijing Key Laboratory of Electronic System Reliability Technology, College of Information Engineering, Capital Normal University, Beijing, China

    Ximeng Li & Zhiping Shi

  3. International Science and Technology Cooperation Base of Electronic System Reliability and Mathematical Interdisciplinary, Capital Normal University, Beijing, China

    Ximeng Li, Qianying Zhang, Zhiping Shi & Yong Guan

  4. School of Mathematical Science, Capital Normal University, Beijing, China

    Shanyan Chen & Guohui Wang

  5. Beijing Advanced Innovation Center for Imaging Theory and Technology, Capital Normal University, Beijing, China

    Qianying Zhang & Yong Guan

Authors
  1. Shanyan Chen
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Guohui Wang
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Ximeng Li
    View author publications

    You can also search for this author in PubMed Google Scholar

  4. Qianying Zhang
    View author publications

    You can also search for this author in PubMed Google Scholar

  5. Zhiping Shi
    View author publications

    You can also search for this author in PubMed Google Scholar

  6. Yong Guan
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Guohui Wang.

Additional information

Naijun Zhan

Publisher’s Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Chen, S., Wang, G., Li, X. et al. Formalization of Camera Pose Estimation Algorithm based on Rodrigues Formula. Form Asp Comp 32, 417–437 (2020). https://doi.org/10.1007/s00165-020-00520-5

Download citation

  • Received:

  • Revised:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s00165-020-00520-5

Keywords

Search

Navigation