Abstract
The Perspective Three-Point Pose Problem (P3P) is an old and basic problem in the area of camera tracking. While methods for solving it have been largely successful, they are subject to erratic behavior near the so-called “danger cylinder.” Another difficulty with most of these methods is the need to select the physically correct solution from among various mathematical solutions. This article presents a new framework from which to study P3P for non-collinear control points, particularly near the danger cylinder. A multivariate Newton-Raphson method to approximately solve P3P is introduced. Using the new framework, this is then enhanced by adding special procedures for handling the problematic behavior near the danger cylinder. It produces a point on the cylinder, a compromise between two nearly equal mathematical solutions, only one of which is the camera’s actual position. The compromise diminishes the risk of accidentally converging to the other nearby solution. However, it does impose the need, upon receding from the danger cylinder vicinity, to make a selection between two possible approximate solution points. Traditional algebraic methods depend on correctly selecting from up to four points, each time the camera position is recomputed. In the new iterative method, selecting between just two points is only occasionally required. Simulations demonstrate that a considerable improvement results from using this revised method instead of the basic Newton-Raphson method.
Similar content being viewed by others
Notes
A Mathematica notebook with the simulation code and results is available from the author upon request. Equivalent C++ code can also be produced upon request.
References
DeMenthon, D., Davis, L.S.: Exact and approximate solutions of the perspective-three-point problem. IEEE Trans. Pattern Anal. Mach. Intell. 14(11), 1100–1105 (1992)
Faugère, J.-C., Moroz, G., Rouillier, F., El-Din, M.S.: Classification of the perspective-three-point problem, discriminant variety and real solving polynomial systems of inequalities. In: ISSAC’08, 21st Int. Symp. Symbolic and Algebraic Computation, pp. 79–86 (2008)
Fischler, M.A., Bolles, R.C.: Random sample consensus: a paradigm for model fitting with applications to image analysis and automatic cartography. In: Graphics and Image Processing, vol. 24, pp. 381–395 (1981)
Fung, Y.F., Wong, K.H.: A three-point model-based algorithm for pose estimation. In: 1998 Int. Symp. Image, Speech, Signal Processing and Robotics (ISSPR’98) (1998)
Gao, X.-S., Hou, X.-R., Tang, J., Cheng, H.-F.: Complete solution classification for the perspective-three-point problem. IEEE Trans. Pattern Anal. Mach. Intell. 25(8), 930–943 (2003)
Grunert, J.A.: Das pothenotische problem in erweiterter gestalt nebst über seine anwendungen in der geodäsie. In: Grunerts Archiv für Mathematik und Physik, vol. 1, pp. 238–248 (1841)
Hanning, T., Schöne, R., Graf, S.: A closed form solution for monocular re-projective 3d pose estimation of regular planar patterns. In: IEEE Int. Conf. Image Processing (ICIP), pp. 2197–2200 (2006)
Haralick, R.M., Lee, C.-N., Ottenberg, K., Nölle, N.: Review and analysis of solutions of the three point perspective pose estimation problem. Int. J. Comput. Vis. 13(3), 331–356 (1994)
Merritt, E.L.: Explicit three-point resection in space. Photogramm. Eng. 15(4), 649–655 (1949)
Müller, F.J.: Direkte (exakte) lösung des einfachen rückwärtsein-schneidens im raume. In: Allegemaine Vermessungs-Nachrichten (1925)
Nistér, D.: A minimal solution to the generalized 3-point pose problem. J. Math. Imaging Vis. 27(1), 67–79 (2007)
Pisinger, G., Hanning, T.: Closed form monocular re-projection pose estimation. In: ISIP’07, IEEE Int. Conf. Image Processing, vol. 5, pp. 197–200 (2007)
Rieck, M.Q.: Handling repeated solutions to the perspective three-point pose problem. In: VISAPP’10, Int. Conf. Computer Vision Theory and Appl., vol. 1, pp. 395–399 (2010)
Rieck, M.Q.: An algorithm for finding repeated solutions to the general perspective three-point pose problem. J. Math. Imaging Vis. 42(1), 92–100 (2011)
Rieck, M.Q.: Solving the three-point camera pose problem in the vicinity of the danger cylinder. In: VISAPP’12, Int. Conf. Computer Vision Theory and Appl., vol. 2, pp. 335–340 (2012)
Smith, A.D.N.: The explicit solution of the single picture resolution problem, with a least squares adjustment to redundant control. Photogramm. Rec. 5(26), 113–122 (1965)
Sun, F.-M., Wang, B.: The solution distribution analysis of the p3p problem. In: SMC’10, Int. Conf. Systems, Man and Cybernetics, pp. 2033–2036 (2010)
Tang, J., Liu, N.: The unique solution for p3p problem. In: SIGAPP’09, ACM Symp. Applied Computing, pp. 1138–1139 (2009)
Tang, J., Chen, W., Wang, J.: A study on the p3p problem. In: ICIC’08, 4th Int. Conf. Intelligent Computing, vol. 5226, pp. 422–429 (2008)
Thompson, E.H.: Space resection: failure cases. Photogramm. Rec. 5(27), 201–204 (1966)
Wolfe, W.J., Mathis, D., Sklair, C.W., Magee, M.: The perspective view of three points. IEEE Trans. Pattern Anal. Mach. Intell. 13(1), 66–73 (1991)
Xiaoshan, G., Hangfei, C.: New algorithms for the perspective-three-point problem. J. Comput. Sci. Technol. 16(3), 194–207 (2001)
Zhang, C.-X., Hu, Z.-Y.: A general sufficient condition of four positive solutions of the p3p problem. J. Comput. Sci. Technol. 20(6), 836–842 (2005)
Zhang, C.-X., Hu, Z.-Y.: Why is the danger cylinder dangerous in the p3p problem? Acta Autom. Sin. 32(4), 504–511 (2006)
Author information
Authors and Affiliations
Corresponding author
Appendices
Appendix A: Iterative Algorithm
The following is pseudocode for the camera tracking algorithm introduced in Sect. 5.2. Prior to tracking, the values t j ,x j ,y j and d j (j=1,2,3) for the control points are obtained (with t 1+t 2+t 3=0), and the values t π =t 1 t 2 t 3, \(t_{\sigma}= t_{1}^{2} + t_{2}^{2} + t_{3}^{2}\) and ι=2(x 1 y 2+x 2 y 3+x 3 y 1−x 2 y 1−x 3 y 2−x 1 y 3) are calculated.
An estimate (x,y,z) of the camera’s position is maintained and updated with each iteration of the following loop. Along with this, corresponding distances to the control points (r 1,r 2,r 3), as in system (2), and matching cosines, as in system (1), are also maintained and updated. Each iteration also entails using up-to-date camera images and the cosines \(\hat{c}_{1},\hat{c}_{2},\hat{c}_{3}\) implied by these images.
The basic-iteration method is detailed in Appendix B. The lock-onto-cylinder method is detailed in Appendix C. These methods are also discussed in Sect. 5.2. The exit-cylinder method is outlined briefly in Sect. 5.2, and will be explained in more detail elsewhere. Details can also be found in the available Mathematica and C++ source code.
Appendix B: Basic-Iteration Method
Input: Control points data (d 1,d 2,d 3,x 1,x 2,x 3,y 1,y 2,y 3,ι), previously estimated position and distance data (x,y,z,r 1,r 2,r 3), and currently measured cosine data (\(\hat{c}_{1}, \hat{c}_{2}, \hat{c}_{3}\)). (See Appendix A.)
Output: Updated estimated position, distance and matching cosine data \((x', y', z', r_{1}', r_{2}', r_{3}', c'_{1}, c'_{2}, c'_{3})\).
-
Set \(R_{j} = r_{j}^{2}\) and \(D_{j} = d_{j}^{2}\) for j=1,2,3.
-
Set \(J = D_{1} D_{2} D_{3} - D_{1} R_{1}^{2} - D_{2} R_{2}^{2} - D_{3} R_{3}^{2} + (D_{1} + D_{2} - D_{3}) R_{1} R_{2} + (D_{2} + D_{3} - D_{1}) R_{2} R_{3} + (D_{3} + D_{1} - D_{2}) R_{3} R_{1}\). However, if |J|< JCLIP, then reset J to equal sign(J) JCLIP.
-
Set \(\Delta_{0} r_{3} = \frac{1}{2} r_{3} [ D_{1} D_{2} D_{3} - D_{3} (D_{1} + D_{2}) R_{3} + D_{1} (D_{2} - D_{3}) R_{1} + D_{2} (D_{1} - D_{3}) R_{2} + D_{3} R_{3}^{2} - D_{1} R_{1}^{2} - D_{2} R_{2}^{2} + (D_{3} - D_{1} - D_{2}) (R_{3} R_{1} + R_{3} R_{2} - 3 R_{1} R_{2}) + R_{3} R_{1} (R_{3} + R_{1}) + R_{3} R_{2} (R_{3} + R_{2}) + R_{1} R_{2} (R_{1} + R_{2}) - 6 R_{1} R_{2} R_{3}] + r_{2} (D_{3} - R_{1} + R_{2}) R_{3} (D_{2} + R_{1} - R_{3}) \hat{c}_{1} + r_{1} (D_{3} + R_{1} - R_{2}) R_{3} (D_{1} + R_{2} - R_{3}) \hat{c}_{2} - r_{1} r_{2} r_{3} (D_{2} + R_{1} - R_{3}) (D_{1} + R_{2} - R_{3}) \hat{c}_{3}\), and similarly for Δ0 r 1 and Δ0 r 2 (by cycling subscripts).
-
Set \(r'_{j} = r_{j} + \Delta_{0} r_{j} / J\) and \(R_{j} = r_{j}^{2}\) for j=1,2,3.
-
Set \(x' = [ (x_{3}^{2} + y_{1} y_{2}) (y_{1} - y_{2}) + (x_{1}^{2} + y_{2} y_{3}) (y_{2} - y_{3}) + (x_{2}^{2} + y_{3} y_{1}) (y_{3} - y_{1}) + (y_{3} - y_{2}) R_{1}' + (y_{1} - y_{3}) R_{2}' + (y_{2} - y_{1}) R_{3}' ] / \iota\).
-
Set \(y' = [ (y_{3}^{2} + x_{1} x_{2}) (x_{2} - x_{1}) + (y_{1}^{2} + x_{2} x_{3}) (x_{3} - x_{2}) + (y_{2}^{2} + x_{3} x_{1}) (x_{1} - x_{3}) + (x_{2} - x_{3}) R_{1}' + (x_{3} - x_{1}) R_{2}' + (x_{1} - x_{2}) R_{3}' ] / \iota\).
-
Set \(Z' = \frac{1}{3} [ R_{1}' + R_{2}' + R_{3}' - x_{1}^{2} - x_{2}^{2} - x_{3}^{2} - y_{1}^{2} - y_{2}^{2} - y_{3}^{2} + 2 x' (x_{1} + x_{2} + x_{3}) + 2 y' (y_{1} + y_{2} + y_{3}) - (x'^{2} + y'^{2}) ]\).
-
Set \(z' = \sqrt{Z'}\).
-
Compute cosine data \(c'_{1}, c'_{2}, c'_{3}\) corresponding to \(r'_{1}, r'_{2}, r'_{3}\).
Appendix C: Lock-Onto-Cylinder Method
Input: Control points data (d 1,d 2,d 3,x 1,x 2,x 3,y 1,y 2,y 3,t π ,t σ ), and currently measured cosine data (\(\hat{c}_{1}, \hat{c}_{2}, \hat{c}_{3}\)). (See Appendix A.)
Output: Updated estimated position, distance and matching cosine data \((x', y', z', r_{1}', r_{2}', r_{3}', c'_{1}, c'_{2}, c'_{3})\).
-
Set \(D_{j} = d_{j}^{2}\), \({\hat{C}}_{j} = {\hat {c}}_{j}^{2}\) and \({\hat{S}}_{j} = 1 - {\hat{C}}_{j}\) for j=1,2,3.
-
Set \(\eta= 1 - {\hat{C}}_{1} - {\hat{C}}_{2} - {\hat{C}}_{3} + 2 {\hat{c}}_{1} {\hat{c}}_{2} {\hat{c}}_{3}\).
-
Set \(\mu = 4 [ (t_{1} - t_{2}) (t_{1} - t_{3}) (t_{2} + t_{3} - 2 t_{1}) {\hat{S}}_{1} / (1 + t_{1}^{2}) + (t_{2} - t_{3}) (t_{2} - t_{1}) (t_{3} + t_{1} - 2 t_{2}) {\hat{S}}_{2} / (1 + t_{2}^{2}) + (t_{3} - t_{1})\times (t_{3} - t_{2}) (t_{1} + t_{2} - 2 t_{3}) {\hat{S}}_{3} / (1 + t_{3}^{2}) ] / (3 \eta^{2})\).
-
Set \(\nu= 4 [ (t_{1} - t_{2}) (t_{1} - t_{3}) (t_{2}^{2} + t_{3}^{2} - t_{1} t_{2} - t_{1} t_{3}) {\hat{S}}_{1} / (1 + t_{1}^{2}) + (t_{2} - t_{3}) (t_{2} - t_{1}) (t_{3}^{2} + t_{1}^{2} - t_{2} t_{3} - t_{2} t_{1}) {\hat{S}}_{2} / (1 + t_{2}^{2}) + (t_{3} - t_{1}) (t_{3} - t_{2}) (t_{1}^{2} + t_{2}^{2} - t_{3} t_{1} - t_{3} t_{2})\vadjust{\vspace{2pt}} {\hat{S}}_{3} / (1 + t_{3}^{2}) ] / (t_{\sigma}\eta^{2})\).
-
Set \(t' = [ 2 \nu^{3} t_{\pi} - \nu^{2} (\mu (t_{\sigma}+2) + 8 t_{\pi} (1- t_{\sigma})) + 8 \mu^{2} t_{\pi}\times (t_{\sigma }-3) + 3 \mu^{3} (t_{\sigma}+2) - 32 t_{\pi} (36 t_{\pi}^{2} + t_{\sigma }^{2}) - 4 \mu(2 t_{\sigma}^{2} + t_{\sigma}^{3} + 4 t_{\pi}^{2} (t_{\sigma }+30)) + \nu(10 \mu^{2} t_{\pi} + 8 t_{\pi} (4 (9 t_{\pi}^{2} - t_{\sigma }) + t_{\sigma}^{2}) - 4 \mu(t_{\sigma}^{2} + 2 (t_{\sigma}-14 t_{\pi }^{2}))) ] / [ 8 \mu^{3} t_{\pi} - 2 \nu^{3} (t_{\sigma}+2) + 48 \mu t_{\pi} (18 + 4 t_{\pi}^{2} + 11 t_{\sigma} + t_{\sigma}^{2}) - 4 \nu^{2} (t_{\pi}^{2} + 6 t_{\sigma} + 3 t_{\sigma}^{2}) + \mu^{2} (15 t_{\sigma}^{2} + 4 (27 + 20 t_{\pi}^{2} + 21 t_{\sigma})) - 16 (2 t_{\sigma}^{3} + t_{\sigma }^{4} + 4 t_{\pi}^{2} (t_{\sigma}^{2} - 3 (9 + 4 t_{\sigma}))) - \nu(6 \mu^{2} (2 + t_{\sigma}) + 84 \mu t_{\pi} (2 + t_{\sigma}) + 8 (6 t_{\sigma}^{2} + 3 t_{\sigma}^{3} + 4 t_{\pi}^{2} (15 + 8 t_{\sigma}))) ] \).
-
Set x′=(1−t′2)/(1+t′2) and y′=2t′/(1+t′2).
-
Set a j =x′−x j and b j =y′−y j for j=1,2,3.
-
Set \(Z' = [ (2 a_{1}^{2} + a_{2}^{2} + a_{3}^{2} + 2 b_{1}^{2} + b_{2}^{2} + b_{3}^{2} - D_{2} - D_{3}) (a_{2}^{2} - a_{3}^{2} + b_{2}^{2} - b_{3}^{2} + D_{2} - D_{3}) {\hat{C}}_{1} - (a_{1}^{2} + 2 a_{2}^{2} + a_{3}^{2} + b_{1}^{2} + 2 b_{2}^{2} + b_{3}^{2} - D_{1} - D_{3}) (a_{1}^{2} - a_{3}^{2} + b_{1}^{2} - b_{3}^{2} + D_{1} - D_{3}) {\hat{C}}_{2} + 4 (a_{1}^{2} - a_{2}^{2} + b_{1}^{2} - b_{2}^{2}) (a_{3}^{2} + b_{3}^{2}) {\hat{C}}_{1} {\hat{C}}_{2} + (a_{1}^{2} + a_{2}^{2} + 2 a_{3}^{2} + b_{1}^{2} + b_{2}^{2} + 2 b_{3}^{2} - D_{1} - D_{2}) (a_{1}^{2} - a_{2}^{2} + b_{1}^{2} - b_{2}^{2} + D_{1} - D_{2}) {\hat{C}}_{3} - 4 (a_{2}^{2} + b_{2}^{2}) (a_{1}^{2} - a_{3}^{2} + b_{1}^{2} - b_{3}^{2}) {\hat{C}}_{1} {\hat{C}}_{3} + 4 (a_{1}^{2} + b_{1}^{2}) (a_{2}^{2} - a_{3}^{2} + b_{2}^{2} - b_{3}^{2}) {\hat{C}}_{2} {\hat{C}}_{3} ] / [ 4 (a_{2}^{2} - a_{3}^{2} + b_{2}^{2} - b_{3}^{2} + D_{2} - D_{3}) {\hat{C}}_{1} - (a_{1}^{2} - a_{3}^{2} + b_{1}^{2} - b_{3}^{2} + D_{1} - D_{3}) {\hat{C}}_{2} + (a_{1}^{2} - a_{2}^{2} + b_{1}^{2} - b_{2}^{2}) {\hat{C}}_{1} {\hat{C}}_{2} + (a_{1}^{2} - a_{2}^{2} + b_{1}^{2} - b_{2}^{2} + D_{1} - D_{2}) {\hat{C}}_{3} - (a_{1}^{2} - a_{3}^{2} + b_{1}^{2} - b_{3}^{2}) {\hat{C}}_{1} {\hat{C}}_{3} + (a_{2}^{2} - a_{3}^{2} + b_{2}^{2} - b_{3}^{2}) {\hat{C}}_{2} {\hat{C}}_{3} ]\).
-
Set \(z' = \sqrt{Z'}\).
-
Compute distance data \(r'_{1}, r'_{2}, r'_{3}\) corresponding to point (x′,y′,z′).
-
Compute cosine data \(c'_{1}, c'_{2}, c'_{3}\) corresponding to \(r'_{1}, r'_{2}, r'_{3}\).
Rights and permissions
About this article
Cite this article
Rieck, M.Q. A Fundamentally New View of the Perspective Three-Point Pose Problem. J Math Imaging Vis 48, 499–516 (2014). https://doi.org/10.1007/s10851-013-0425-8
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10851-013-0425-8