Accurate Non-Iterative O( n ) Solution to the P n P Problem CVLab - Ecole Polytechnique Fédérale de Lausanne Francesc Moreno-Noguer Vincent Lepetit Pascal Fua

The Perspective- n -Point (P n P) Problem Internal parameters known 2D/3D correspondences known  Given: 2D/3D correspondences + Camera internal parameters  We want: Compute camera pose w.r.t. 3D points  How to take advantage of the internal parameters? World Coordinate System Rotation, Translation ?

Church 1945 DeMenthon & Davis 1995 Dhome et al 1989 Haralick et al 1991 Horaud et al 1997 Kumar & Hanson 1994 Oberkampf et al 1996 Quan & Lan 1999 Gao et al 2003 Abbel & Karara 1971 Fischler & Bolles 1981 Horaud et al 1989 Lowe 1991 Triggs 1999 Ansar & Daniilidis 2003 Fiore 2001 Lu et al 2000 Scherighofer & Pinz 2006 Old Problem with many References

Church 1945 DeMenthon & Davis 1995 Dhome et al 1989 Haralick et al 1991 Horaud et al 1997 Kumar & Hanson 1994 Oberkampf et al 1996 Quan & Lan 1999 Gao et al 2003 Abbel & Karara 1971 Fischler & Bolles 1981 Horaud et al 1989 Lowe 1991 Triggs 1999 Ansar & Daniilidis 2003 Fiore 2001 Lu et al 2000 Scherighofer & Pinz 2006 Iterative vs. Non-Iterative Methods Iterative Non-Iterative

Iterative vs. Non-Iterative Methods Iterative  Accurate, but slow  Risk of local minima  No risk of local minima  High Complexity: O(n ) and larger –Fast for small n –Slow for large n  n unknowns to solve 6 DoF !!!! 5

Current P n P Applications: small n  Many applications: Comp. Vision, Robotics, Augmented Reality  Example: Estimating the 3D position of markers [AR-Tracking Image (2D positions) Infrared LEDs + Infrared Camera Traditionally P n P is applied to a small number of points Target with markers (3D positions)

Dealing with Hundreds of Points  Tracking by Detection: −Process hundreds of noisy features −Real-time Existing P n P approaches are inefficient Model Image 3D positions New Image 2D positions [Williams et al 2007]  Potential Applications: −Structure from Motion −SLAM

EP n P: Efficient P n P  Fast: −Non-iterative algorithm: O ( n ) −Handle hundreds of points in real-time  Accurate −As accurate as iterative methods −Robust to noise

Computation Time [Matlab] EP n P: Fast and Accurate Accuracy non-iterative: Clamped DLT (DLT imposing known intrinsic parameters) iterative: [Lu, Hager and Mjolsness, PAMI'00] EP n P iterative non-iterative  As accurate as the iterative solution !!!  Much faster iterative EP n P non-iterative

 Unknowns: Camera coord. of the 4 control points World Coord. System c 4 world c 3 c 2 c 1 c 3 camera c 4 c 2 c 1 Introducing Control Points p i world   j=1 4  ij c j world p i camera   j=1 4  ij c j camera Number of unknowns = 12 c 1 camera c 2 c 3 c 4   3D points expressed as a weighted sum of 4 control points Known

EP n P: Algorithm Summary 1.The control points coordinates are the (12) unknowns. 2.Project the 3D points onto the image  Build a linear system in the control points coordinates. 3.The control points coordinates can be expressed as a linear combination of the null eigenvectors. 4.The weights ( the  i ) are the new unknowns (not more than 4). 5.Adding rigidity constraints gives quadratic equations in the  i. 6.Solving for the  i depends on their number (linearization or relinearization).

c 3 camera c 4 c 2 c 1 Point Reprojections give a Linear System p i camera u i u i 1 w i  p i A  j=1 4  ij c j camera  A  For each correspondence:  For all n correspondences: Mx=0 where c 1 camera c 2 c 3 c 4 x = and M is a constant matrix of coefficients {,, } u i  ij A

EP n P: Algorithm Summary 1.The control points coordinates are the (12) unknowns. 2.Project the 3D points onto the image  Build a linear system in the control points coordinates. 3.The control points coordinates can be expressed as a linear combination of the null eigenvectors. 4.The weights ( the  i ) are the new unknowns (not more than 4). 5.Adding rigidity constraints gives quadratic equations in the  i. 6.Solving for the  i depends on their number (linearization or relinearization).

 Transform to a system with small and constant size: Solution is in the Null-Space  M Mx = 0 T Mx = 0 12 x 12 = 12 x 1 x 0 M T O(n) : Computing is the most time consuming step of EP n P M T  x belongs to the null-space of : M T M x  = i=1 N  i v i M 2n x 12 = 12 x 1 2n x 1 x 0 Null Eigenvectors of M T M

EP n P: Algorithm Summary 1.The control points coordinates are the (12) unknowns. 2.Project the 3D points onto the image  Build a linear system in the control points coordinates. 3.Express the control points coordinates as a linear combination of the null eigenvectors. 4.The weights ( the  i ) are the new unknowns (not more than 4). 5.Adding rigidity constraints gives quadratic equations in the  i. 6.Solving for the  i depends on their number (linearization or relinearization).

Dimension Null-Space of : Unknown M T  We need to solve for N and  What is the dimension N of the null space of ? −Noise-Free Case: N=1 −In practice (noise): No zero eigenvalues, but several are small Dimension N of the Null-Space x   i=1 N  i v i Coefficients: Unknown Coordinates of Control Points M T  ‘s i Eigenvectors of : Known M T We found that only the cases N = 1, 2, 3 and 4 must be considered 62% N=1 28% N=2 9% N=3 1% N=4

Dimension Null-Space of : Unknown M T  We need to solve for N and Dimension N of the Null-Space x   i=1 N  i v i Coefficients: Unknown Coordinates of Control Points  ‘s i Eigenvectors of : Known M T  We fix N to 1, 2, 3 and 4 –Solve for ‘s independently for each case  i  1 v 1 x  N = 1:   1 v 1  2 v 2  x N = 2:  1 v 1  2 v 2  3 v 3  x  N = 3:  1 v 1  2 v 2  3 v 3   4 v 4  x  N = 4: –Take the “best” solution  minimizes reprojection error

EP n P: Algorithm Summary 1.The control points coordinates are the (12) unknowns. 2.Project the 3D points onto the image  Build a linear system in the control points coordinates. 3.Express the control points coordinates as a linear combination of the null eigenvectors. 4.The weights ( the  i ) are the new unknowns (not more than 4). 5.Adding rigidity constraints gives quadratic equations in the  i. 6.Solving for the  i depends on their number (linearization or relinearization).

 Impose Rigidity Constraints: Distances between control points must be preserved c 4 world c 2 c 3 c 1 Solving for  i c j world c i 2  c i camera c j 2 6 quadratic constraints on the ‘s :  i Known c 1 camera c 4 c 3 c 2 World Coordinate System

EP n P: Algorithm Summary 1.The control points coordinates are the (12) unknowns. 2.Project the 3D points onto the image  Build a linear system in the control points coordinates. 3.Express the control points coordinates as a linear combination of the null eigenvectors. 4.The weights ( the  i ) are the new unknowns (not more than 4). 5.Add rigidity constraints to get quadratic equations in the  i. 6.Solving for the  i depends on their number (linearization or relinearization).

Case N=1  We have: c i world c j d ij 2 And 6 quadratic equations :  c i camera c j 2 d ij c 1 camera c 2 c 3 c 4   1 v 1 Control Points Distances Known Control Points Coordinates Unknown Eigenvector of Known M T Coefficients Unknown

Case N=1 2 And 6 quadratic equations : c j world d ij  We have:  can be easily computed: –Absolute Value: Solution of a linear system  1   1 v 1 [1] v 1 [2] v 1 [3] v 1 [4]  1  v 1 [i] v 1 [j] 2 d ij –Sign: Chosen to preserve orientation in control points  c i camera c j 2 d ij c 1 camera c 2 c 3 c 4   1 v 1 c i world

Case N=2 2 And 6 quadratic equations : c j world d ij  We have:  Linearization technique: –Substitute quadratic by linear terms –6 linear equations in,,  1  1  11   1  2  12   2  2  22   11  12  22 6 equations 3 unknowns  c i camera c j 2 d ij c 1 camera c 2 c 3 c 4   1 v 1  2 v 2  Known from and v 2 v 1 l 13 l 23 l 33 l 43 l 53 l 63 l 11 l 21 l 31 l 41 l 51 l 61 l 12 l 22 l 32 l 42 l 52 l 62  11  12  22  d 12 2 d 13 2 d 14 2 d 23 2 d 24 2 d 34 2 c i world

Case N=3 2 c 1 camera c 2 c 3 c 4  And 6 quadratic equations :  c i camera c j 2 d ij c j world d ij  We have:  Same Linearization technique: –6 Linear Equations for 6 unknowns  1 v 1  2 v 2  3 v 3  Known from, and v 2 v 1 v 3 l 16 l 26 l 36 l 46 l 56 l 66 l 14 l 24 l 34 l 44 l 54 l 64 l 15 l 25 l 35 l 45 l 55 l 65 d 12 2 d 13 2 d 14 2 d 23 2 d 24 2 d equations 6 unknowns l 13 l 23 l 33 l 43 l 53 l 63 l 11 l 21 l 31 l 41 l 51 l 61 l 12 l 22 l 32 l 42 l 52 l 62  11  12  13  22  23  33  c i world

Case N=4  Six Quadratic Equations in,, and  The linearization produces 10 products –Do not have enough equations anymore !!!  Relinearization: –The are expressed as a linear combination of eigenvectors  ab   a  b  1  2  3  4   Check paper for details 6 equations 10 unknowns  11  12  13  14  22  23  24  33  34  44  l 1,10 l 2,10 l 3,10 l 4,10 l 5,10 l 6,10 l 17 l 27 l 37 l 47 l 57 l 67 l 19 l 29 l 39 l 49 l 59 l 69 l 16 l 26 l 36 l 46 l 56 l 66 l 54 l 14 l 24 l 34 l 44 l 64 l 15 l 25 l 35 l 45 l 55 l 65 l 13 l 23 l 33 l 43 l 53 l 63 l 51 l 11 l 21 l 31 l 41 l 61 l 12 l 22 l 32 l 42 l 52 l 62 d 12 2 d 13 2 d 14 2 d 23 2 d 24 2 d 34 2

EP n P: Algorithm Summary 1.The control points coordinates are the (12) unknowns. 2.Project the 3D points onto the image  Build a linear system in the control points coordinates. 3.Express the control points coordinates as a linear combination of the null eigenvectors. 4.The weights ( the  i ) are the new unknowns (not more than 4). 5.Add rigidity constraints to get quadratic equations in the  i. 6.Solve for the  i depending on their number (linearization or relinearization).

From ‘s to Camera Pose : Coordinates of Control Points x   i=1 N  i v i x p i camera   j=1 4  ij c j camera p i : Coordinates of all 3D Points  i  i {, } p i camera p i world : Camera Pose R, T Absolute Orientation [Horn’88]

Results  Detection in every single frame  No temporal consistency Model Image 3D coordinates in world reference New Image 2D image coordinates

Final Optimization  Refine Control Points Coordinates EP n P Refined control points coordinates  Gauss Newton: –Independent on the number of input correspondences –Converges in < 5 iterations 3D/2D correspondences Intrinsic parameters Initial control points coordinates  opt Gauss Newton  (Mean of 300 Experiments) [paper]

Improves Accuracy with No-Cost Gauss Newton Optimization:  Improves the accuracy  With almost No-Cost EP n P non-iterative iterative non-iterative iterative EP n P [paper] EP n P [paper] Computation Time [Matlab] Accuracy

Time vs. Accuracy Accuracy level EPnP Rotation Error (%) Computation Time (sec) EPnP : Same accuracy as iterative approach About 20 times faster !!! Iterative: [Lu, Hager and Mjolsness, PAMI'00]

Conclusions & Future Work  O(n) non-iterative solution to the P n P problem –As accurate as iterative methods –Much faster  Simple central idea: –Represent 3D coordinates as a weighted sum of 4 control points –Potentially applicable to other problems – Structure from Motion – Deformable surfaces

Code Available: Thanks !!