Formalization of camera pose estimation algorithm based on Rodrigues formula. (English) Zbl 1458.68226

Summary: Camera pose estimation is key to the proper functioning of robotic systems, supporting critical tasks such as robot navigation, target tracking, camera calibration, etc. While multiple 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.


68T45 Machine vision and scene understanding
68Q60 Specification and verification (program logics, model checking, etc.)
68T40 Artificial intelligence for robotics
68V15 Theorem proving (automated and interactive theorem provers, deduction, resolution, etc.)


HOL Light; EPnP; GitHub
Full Text: DOI


[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, RW, Robotic manipulators and the product of exponentials formula (1984), Berlin: Springer, Berlin · Zbl 0535.93034
[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) · Zbl 1328.81124
[4] Yang, Guo, A novel solution to the p4p problem for an uncalibrated camera, J Math Imaging Vis, 45, 2, 186-198 (2013) · Zbl 1276.68154
[5] John, Harrison, Formal proof-theory and practice, Not Am Math Soc, 55, 11, 1-2 (2008) · Zbl 1154.03303
[6] John, Harrison, The HOL light theory of euclidean space, J Autom Reason, 50, 2, 173-190 (2013) · Zbl 1260.68373
[7] Hall, B.C.: Lie groups, lie algebras, and representations. Springer (2013)
[8] Kamali, M.; Dennis, LA; McAree, O.; Fisher, M.; Veres, SM, Formal verification of autonomous vehicle platooning, Sci Comput Program, 148, 88-106 (2017)
[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)
[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) · Zbl 0858.70001
[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)
[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, KK, Elements of vector analysis (2008), Berlin: Springer, Berlin
[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, JL, Aas 95-137 stereographic orientation parameters for attitude dynamics: a generalization of the rodrigues parameters, J Astron Sci, 44, 13-15 (1996)
[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 · Zbl 0900.70028
[24] Sun, F.; Wang, B., The solution distribution analysis of the p3p problem, 2033-2036 (2010), Turkey: Istanbul, Turkey
[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) · Zbl 1173.68829
[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) · Zbl 1433.68471
[28] Zhao Z (2014) Research on videometrics technology in the aircraft transformation collision avoidance system. Master thesis, National University of Defense Technology
This reference list is based on information provided by the publisher or from digital mathematics libraries. Its items are heuristically matched to zbMATH identifiers and may contain data conversion errors. It attempts to reflect the references listed in the original paper as accurately as possible without claiming the completeness or perfect precision of the matching.