Accurate and Linear Time Pose Estimation from Points and Lines Alexander Vakhitov 1 , Jan Funke 2 & Francesc Moreno-Noguer 2 1 St. Petersburg State University, St. Petersburg, Russia; Skolkovo Institute of Science and Technology, Moscow, Russia, [email protected] 2 Institut de Robòtica i Informàtica Industrial, UPC-CSIC, Barcelona, Spain, {jfunke,fmoreno}@iri.upc.edu Camera Pose From Lines and Points Model : • 3D points • line segments (3D endpoints) Observation: • noisy projections of 3D points • noisy projections of line endpoints shifted along the line Algebraic Line Error Point projection ˜ x = π(θ, X) • X 3D point, • ˜ x ∈ R 3 homogeneous coordinates of the point projection, • θ pose parameters. Line projection ˆ l i =˜ p i d ט q i d , l i = ˆ l i | ˆ l i | ∈ R 3 . • ˜ p i d , ˜ q i d 2D segment endpoints • l i normalized line coefficients Point-to-line error: E pl (θ, X, l i )=(l i ) π(θ, X). (1) Using 3D segment endpoints {P i , Q i } in (1) as X, Algebraic line segment error (ALSE): E l (θ, P i , Q i , l i )= E 2 pl (θ, P i , l i )+ E 2 pl (θ, Q i , l i ). • linear in 3D segment endpoint • but: depends on segment shift Segment Shift Problem • p, q - projected 3D endpoints • p d , q d - detected 2D endpoints • d 1 + d 4 ALSE • d 2 + d 3 optimal error 3 step method: 1 Initial estimate with E/OPnPL 2 Shift correction 3 Final estimate with E/OPnPL EPnPL Unknowns are 4 control points C j . Unknowns vector μ =[C 1 ,..., C 4 ] Point projection: π EPnP (θ, X)= ∑4 j=1 α j C c j Points cost: M p μ 2 → min Denote m i l (X i ) = ([α 1 , ...,α 4 ] ⊗ l i ) Point-to-line error: E pl,EPnP =(m i l (P i )) μ Lines cost: E lines = ∑ i ⎛ ⎜ ⎝ (m i l (P i )) μ ⎞ ⎟ ⎠ 2 + + ⎛ ⎜ ⎝ (m i l (Q i )) μ ⎞ ⎟ ⎠ 2 . Total cost: M p μ 2 +E lines = = ¯ Mμ 2 → min μ OPnPL Unknowns are quaternion parameters a, b, c, d Camera pose R, t Vectorized pose λR = ˆ r,λt = ˆ t Point projection: π OPnP (θ, X)= λ( ˆ RX + ˆ t). Point-to-line error: E pl,OPnP = l i λ( ˆ RX + ˆ t) Knowns (point/line) G p/l , H p/l , k p/l Points/lines cost: E p/l = G p/l ˆ r + H p/l ˆ t 12 + k p/l 2 Total cost: E=E points +E lines → min Express ˆ t 12 from ∂E/∂ ˆ t =0 Solve using GB ∂E ∂a = ∂E ∂b = ∂E ∂c = ∂E ∂d =0 Contributions Points Lines Lines+Points Model contours with estimated pose (white), detected lines and points (blue) • algebraic line error as linear constraints on the line endpoints • OPnP [1], EPnP [2] => OPnPL, EPnPL Key features: • camera pose accuracy increased • negligible runtime overhead Code publicly available at: github.com/alexander-vakhitov/pnpl Synthetic Experiments (a) Accuracy w.r.t. noise level, nonplanar: npoints = 6, nlines = 10 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 0.5 1 1.5 Rotation Error (degrees) Median Rotation 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 2 4 6 Rotation Error (degrees) Mean Rotation 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 0.5 1 1.5 2 Translation Error (%) Median Translation 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 1 2 3 4 5 Translation Error (%) Mean Translation Mirzaei RPnL Pluecker EPnP_GN OPnP DLT EPnPL OPnPL OPnP* (b) Accuracy w.r.t. feature num., nonplanar: σnoise =1 6 7 8 9 10 11 12 13 14 15 n p , n l 0 0.5 1 Rotation Error (degrees) Median Rotation 6 7 8 9 10 11 12 13 14 15 n p , n l 0 0.5 1 Rotation Error (degrees) Mean Rotation 6 7 8 9 10 11 12 13 14 15 n p , n l 0 0.5 1 1.5 Translation Error (%) Median Translation 6 7 8 9 10 11 12 13 14 15 n p , n l 0 0.5 1 1.5 2 Translation Error (%) Mean Translation Mirzaei RPnL Pluecker EPnP_GN OPnP DLT EPnPL OPnPL OPnP* (c) Accuracy w.r.t. noise level, nonplanar, only lines 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 0.5 1 1.5 Rotation Error (degrees) Median Rotation 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 2 4 6 Rotation Error (degrees) Mean Rotation 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 0.5 1 1.5 2 Translation Error (%) Median Translation 0 0.5 1 1.5 2 2.5 3 3.5 4 Noise, std.dev. (pix.) 0 1 2 3 4 5 Translation Error (%) Mean Translation Mirzaei RPnL Pluecker DLT EPnPL OPnPL OPnP* (d) Running time of the algorithms, of the phases of OPnPL, EPnPL 80 200 320 440 560 n0 0.02 0.04 0.06 0.08 Average Runtime (sec) Time 20 220 420 620 n0 0.005 0.01 0.015 0.02 Time, s. Mean processing time 20 220 420 620 n0 0.01 0.02 0.03 0.04 Time, s. Mean solving time Mirzaei RPnL Pluecker EPnP_GN OPnP DLT EPnPL OPnPL Real Experiments OPnP (pt) OPnPL (lin)/(pt+lin) OPnP (pt) OPnPL (lin)/(pt+lin) (21.3, 100.0) (9.3, 30.5)/(9.5, 28.2) (61.5, 605.4) (0.4, 6.0)/(0.3, 5.7) (1.3, 33.8) (0.6, 9.1)/(0.2, 2.6) (118.2, 328.0) (2.7, 11.5)/(2.7, 11.3) (3.2, 100.0) (0.8, 3.3)/(0.9, 5.6) (9.5, 100.0) (0.7, 3.0)/(0.2, 1.3) (31.8, 162.2) (1.3, 6.8)/(1.3, 6.8) (1.3, 22.0) (0.9, 10.1)/(0.2, 2.3) (Rot. Err. (deg), Translation Error (%)) [1] Y. Zheng, Y. Kuang, S. Sugimoto, K. Astrom, and M. Okutomi. Revisiting the PnP problem: a fast, general and optimal solution. In Computer Vision (ICCV), 2013 IEEE International Conference on, pages 2344–2351. IEEE, 2013. [2]V. Lepetit, F. Moreno-Noguer, and P. Fua. EPnP: An accurate O(n) solution to the PnP problem. International Journal of Computer Vision, 81(2):155–166, 2009.