Accurate and Linear Time Pose Estimationfrom Points and LinesAlexander Vakhitov1, Jan Funke2 & Francesc Moreno-Noguer2

1 St. Petersburg State University, St. Petersburg, Russia; Skolkovo Institute of Science and Technology, Moscow, Russia, alexander.vakhitov@gmail.com2 Institut de Robòtica i Informàtica Industrial, UPC-CSIC, Barcelona, Spain, {jfunke,fmoreno}@iri.upc.edu

Camera Pose From Lines andPoints

Model :• 3D points• line segments (3D

endpoints)

Observation:• noisy projections of

3D points• noisy projections of

line endpointsshifted along theline

Algebraic Line Error

Point projectionx̃ = π(θ, X)

• X 3D point,• x̃ ∈ R

3

homogeneouscoordinates of thepoint projection,

• θ pose parameters.

Line projection

l̂i = p̃id×q̃i

d, li = l̂i

|̂li| ∈ R3.

• p̃id, q̃i

d 2D segmentendpoints

• li normalized linecoefficients

Point-to-line error:Epl(θ, X, li) = (li)�π(θ, X). (1)

Using 3D segment endpoints {Pi, Qi} in (1) as X,Algebraic line segment error (ALSE):

El(θ, Pi, Qi, li) =E2

pl(θ, Pi, li)+

E2pl(θ, Qi, li).

• linear in 3Dsegment endpoint

• but: depends onsegment shift

Segment Shift Problem

• p, q - projected 3Dendpoints

• pd, qd - detected2D endpoints

• d1 + d4 ALSE• d2 + d3 optimal

error

3 step method:

1 Initial estimate with E/OPnPL2 Shift correction3 Final estimate with E/OPnPL

EPnPL

Unknowns are 4 control points Cj.Unknowns vector μ = [C�

1 , . . . , C�4 ]�

Point projection: πEPnP(θ, X) = ∑4j=1 αjCc

j

Points cost: ‖Mpμ‖2 → minDenote mi

l(Xi) = ([α1, . . . , α4] ⊗ li)�

Point-to-line error: Epl,EPnP = (mil(Pi))�μ

Lines cost: Elines = ∑i

⎛⎜⎝(mi

l(Pi))�μ⎞⎟⎠2+

+⎛⎜⎝(mi

l(Qi))�μ⎞⎟⎠2

.Total cost: ‖Mpμ‖2 + Elines =

= ‖M̄μ‖2 → minμ

OPnPL

Unknowns are quaternion parameters a, b, c, dCamera pose R, tVectorized pose λR = r̂, λt = t̂Point projection: πOPnP(θ, X) = λ(R̂X + t̂).Point-to-line error: Epl,OPnP = l�i λ(R̂X + t̂)Knowns (point/line) Gp/l, Hp/l, kp/lPoints/lines cost: Ep/l = ‖Gp/l̂r + Hp/l̂t12 + kp/l‖2

Total cost: E = Epoints + Elines → minExpress t̂12 from ∂E/∂t̂ = 0Solve 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 theline 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 4Noise, std.dev. (pix.)

0

0.5

1

1.5

Rot

atio

n E

rror

(deg

rees

)

Median Rotation

0 0.5 1 1.5 2 2.5 3 3.5 4Noise, std.dev. (pix.)

0

2

4

6

Rot

atio

n E

rror

(deg

rees

)

Mean Rotation

0 0.5 1 1.5 2 2.5 3 3.5 4Noise, std.dev. (pix.)

0

0.5

1

1.5

2

Tran

slat

ion

Err

or (%

)

Median Translation

0 0.5 1 1.5 2 2.5 3 3.5 4Noise, std.dev. (pix.)

0

1

2

3

4

5

Tran

slat

ion

Err

or (%

)

Mean TranslationMirzaeiRPnLPlueckerEPnP_GNOPnPDLTEPnPLOPnPLOPnP*

(b) Accuracy w.r.t. feature num., nonplanar: σnoise = 1

6 7 8 9 10 11 12 13 14 15n

p, n

l

0

0.5

1

Rot

atio

n E

rror

(deg

rees

)

Median Rotation

6 7 8 9 10 11 12 13 14 15n

p, n

l

0

0.5

1

Rot

atio

n E

rror

(deg

rees

)

Mean Rotation

6 7 8 9 10 11 12 13 14 15n

p, n

l

0

0.5

1

1.5

Tran

slat

ion

Err

or (%

)

Median Translation

6 7 8 9 10 11 12 13 14 15n

p, n

l

0

0.5

1

1.5

2

Tran

slat

ion

Err

or (%

)

Mean TranslationMirzaeiRPnLPlueckerEPnP_GNOPnPDLTEPnPLOPnPLOPnP*

(c) Accuracy w.r.t. noise level, nonplanar, only lines

0 0.5 1 1.5 2 2.5 3 3.5 4Noise, std.dev. (pix.)

0

0.5

1

1.5

Rot

atio

n E

rror

(deg

rees

)

Median Rotation

0 0.5 1 1.5 2 2.5 3 3.5 4Noise, std.dev. (pix.)

0

2

4

6

Rot

atio

n E

rror

(deg

rees

)

Mean Rotation

0 0.5 1 1.5 2 2.5 3 3.5 4Noise, std.dev. (pix.)

0

0.5

1

1.5

2

Tran

slat

ion

Err

or (%

)

Median Translation

0 0.5 1 1.5 2 2.5 3 3.5 4Noise, std.dev. (pix.)

0

1

2

3

4

5

Tran

slat

ion

Err

or (%

)

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 np+nl

0

0.02

0.04

0.06

0.08

Aver

age

Runt

ime

(sec

)

Time

20 220 420 620np+nl

0

0.005

0.01

0.015

0.02

Tim

e, s

.

Mean processing time

20 220 420 620np+nl

0

0.01

0.02

0.03

0.04

Tim

e, s

.

Mean solving timeMirzaeiRPnLPlueckerEPnP_GNOPnPDLTEPnPLOPnPL

See other experiments (planar case etc.) in the paper.

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, andM. Okutomi.Revisiting the PnP problem: a fast, general and optimalsolution.In Computer Vision (ICCV), 2013 IEEE InternationalConference 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.