An Efficient Algebraic Solution to the Perspective-Three-Point Problem

1.2 MB·

by  Tong Ke

An Efficient Algebraic Solution to the
Perspective-Three-Point Problem
Tong Ke Stergios Roumeliotis
UniversityofMinnesota UniversityofMinnesota
Minneapolis,MN55455 Minneapolis,MN55455
[email protected] [email protected]
ABSTRACT employ the law of cosines in its P3P problem formulation is that
7
of Kneip et al. [15], and later on that of Masselli and Zell [18].
1 Inthiswork,wepresentanalgebraicsolutiontotheclassicalperspective-
Specifically,[15]and[18]followageometricapproachforavoid-
0 3-point (P3P) problem for determining the position and attitude
ingcomputingthefeatures’distancesandinsteaddirectlysolvefor
2 of a camera from observations of three known reference points.
the camera’s pose. In both cases, however, several intermediate
Incontrasttopreviousapproaches,wefirstdirectlydeterminethe
n terms(e.g., tangentsandcotangentsofcertainangles)needtobe
camera’sattitudebyemployingthecorrespondinggeometriccon-
a computed,whichnegativelyaffectthespeedandnumericalpreci-
J straintstoformulateasystemoftrigonometricequations. Thisis
sionoftheresultingalgorithms.
thenefficientlysolved, followinganalgebraicapproach, todeter-
8 Similar to [15] and [18], our proposed approach does not re-
minetheunknownrotationmatrixandsubsequentlythecamera’s
2 quire first computing the features’ distances. Differently though,
position. As compared to recent alternatives, our method avoids
computingunnecessary(andpotentiallynumericallyunstable)in- inourderivation,wefirsteliminatethecamera’spositionandthe
]
features’distancestoresultintoasystemofthreeequationsinvolv-
V termediateresults,andthusachieveshighernumericalaccuracyand
ing only the camera’s orientation. Then, we follow an algebraic
robustnessatalowercomputationalcost. Thesebenefitsarevali-
C processforsuccessivelyeliminatingtwooftheunknown3-dofand
datedthroughextensiveMonte-Carlosimulationsforbothnominal
. arrivingintoaquarticpolynomial. Ouralgorithm(summarizedin
s andclose-to-singulargeometricconfigurations.
c Alg.1)requiresfeweroperationsandinvolvessimplerandnumer-
[ icallymorestableexpressions,ascomparedtoeither[15]or[18],
1. INTRODUCTION
and thus performs better in terms of efficiency, accuracy, and ro-
1
ThePerspective-n-Point(PnP)istheproblemofdeterminingthe bustness.Specifically,themainadvantagesofourapproachare:
v
3Dpositionandorientation(pose)ofacamerafromobservationsof
7
knownpointfeatures. ThePnPistypicallyformulatedandsolved • Ouralgorithm’simplementationtakesabout40%ofthetime
3 linearly by employing lifting (e.g., [1]), or as a nonlinear least- requiredbythecurrentstateoftheart[15].2
2
squaresproblemminimizediteratively(e.g.,[10])ordirectly(e.g.,[12]).
8
0 TheminimalcaseofthePnP(forn=3)isoftenusedinpractice,in • Our method achieves better accuracy than [15, 18] under
. conjunctionwithRANSAC,forremovingoutliers[5]. nominal conditions. Moreover, we are able to further im-
1 ThefirstsolutiontotheP3PproblemwasgivenbyGrunert[9] provethenumericalprecisionbyapplyingrootpolishingto
0
in1841. Sincethen,severalmethodshavebeenintroduced,some thesolutionsofthequarticpolynomialwhileremainingfaster
7
of which [9, 4, 19, 5, 17, 8] were reviewed and compared, in than[15,18].
1
terms of numerical accuracy, by Haralick et al. [11]. Common
:
v tothesealgorithmsisthattheyemploythelawofcosinestofor- • Ouralgorithmismorerobustthan[15,18]whenconsidering
i mulateasystemofthreequadraticequationsinthefeatures’dis- close-to-singularconfigurations(thethreepointsarealmost
X
tancesfromthecamera. Theydiffer, however, intheelimination collinearorveryclosetoeachother).
r processfollowedforarrivingataunivariatepolynomial. Lateron,
a
QuanandLan[21]andmorerecentlyGaoetal.[6]employedthe Theremainingofthispaperisstructuredasfollows. Section2
sameformulationbutinsteadusedtheSylvesterresultant[3]and presentsthedefinitionoftheP3Pproblem, aswellasourderiva-
Wu-Ritz’szero-decompositionmethod[23],respectively,tosolve tionsforestimatingfirsttheorientationandthenthepositionofthe
the resulting system of equations, and, in the case of [6], to de- camera. InSection3,weassesstheperformanceofourapproach
terminethenumberofrealsolutions. Regardlessoftheapproach against [15]and[18]insimulationforbothnominalandsingular
followed,oncethefeature’sdistanceshavebeencomputed,finding configurations.Finally,weconcludeourworkinSection4.
thecamera’sorientation,expressedasaunitquaternion[13]oraro-
tationmatrix[14],oftenrequirescomputingtheeigenvectorsofa solvingthegeneralizedP3Presultingintoanocticunivariatepoly-
4×4matrix(e.g.,[21])orperformingsingularvaluedecomposition nomial whose odd monomials vanish for the case of the central
P3P.
(SVD)ofa3×3matrix(e.g.,[6]),respectively,bothofwhichare
2Although Masselli and Zell [18] claim that their algorithm runs
time-consuming. Furthermore, numerical error propagation from
fasterthanKneipetal.’s[15],ourresults(seeSection3)showthe
thecomputeddistancestotherotationmatrixsignificantlyreduces
opposite to be true (by a small margin). The reason we arrive at
theaccuracyofthecomputedposeestimates. adifferentconclusionisthatoursimulationrandomlygeneratesa
To the best of our knowledge, the first method1 that does not newgeometricconfigurationforeachrun,whileMasselliemploys
onlyoneconfigurationduringtheirentiresimulation,inwhichthey
1Nister and Stewenius [20] also follow a geometric approach for savetimeduetocaching.
whichwesolvebyemployingRodrigues’rotationformula[16]:4
C(k ,θ )=cosθ I−sinθ (cid:98)k (cid:99)+(1−cosθ )k kT (8)
2 2 2 2 2 2 2 2
toget
π
θ =arccos(kTk )± (9)
2 1 3 2
Notethatweonlyneedtoconsideroneofthesetwosolutions[in
ourcase,weselectθ =arccos(kTk )− π;seeFig.2],sincethe
2 1 3 2
otheronewillresultinthesameGC(seeAppendix5.2foraformal
C
proof).
Figure 1: The camera {C}, whose position, Gp , and orien-
C Inwhatfollows,wedescribetheprocessforeliminatingθ from
tation, GC, we seek to determine, observes unit-vector bear- 3
C (3) and (4), and eventually arriving into a quartic polynomial in-
ing measurement Cb of a feature f , whose position, Gp , is
i i i volving a trigonometric function of θ . To do so, we once again
known. 1
substitutein (3)and(4) thefactorizationof GCdefinedin (5)to
C
get(fori=1,2):
2. PROBLEMFORMULATIONANDSOLU-
uTC(k ,θ )C(k ,θ )C(k ,θ )v =0 (10)
TION i 1 1 2 2 3 3 i
where
2.1 ProblemDefinition
u (cid:44)Gp −Gp , v (cid:44)Cb ×Cb , i=1,2, (11)
Giventhepositions,Gp ,ofthreeknownfeaturesf , i=1,2,3, i i 3 i i 3
i i
withrespecttoareferenceframe{G},andthecorrespondingunit- andemploythefollowingpropertyofrotationmatrices
vector,bearingmeasurements,Cb ,i=1,2,3,ourobjectiveisto
i
estimatetheposition,Gp ,andorientation,i.e.,therotationmatrix C(k ,θ )C(k ,θ )CT(k ,θ )=C(C(k ,θ )k ,θ )
C 1 1 2 2 1 1 1 1 2 2
GC,ofthecamera{C}.
C
torewrite(10)inasimplerformas
2.2 Solvingfortheorientation
uTC(k ,θ )C(C(k ,θ )k ,θ )C(k ,θ )v =0
From the geometry of the problem (see Fig. 1), we have (for i 1 1 2 2 3 3 2 2 i
i=1,2,3): ⇒uTiC(k1,θ1)C(k(cid:48)3,θ3)vi(cid:48) =0 (12)
where
Gp =Gp +d GCCb (1)
i C iC i v(cid:48) (cid:44)C(k ,θ )v , i=1,2
i 2 2 i
whered (cid:44)(cid:107)Gp −Gp (cid:107)isthedistancebetweenthecameraand
i C i k(cid:48) (cid:44)C(k ,θ )k =k ×k (13)
thefeaturef . 3 2 2 3 2 1
i
Inordertoeliminatetheunknowncameraposition,Gp ,andfea-
C The last equality in (13) is geometrically depicted in Fig. 2 and
turedistance,d , i = 1,2,3,wesubtractpairwisethethreeequa-
i algebraicallyderivedinAppendix5.1. Analogously,itisstraight-
tionscorrespondingto(1)for(i,j)=(1,2), (1,3)and(2,3),and
forwardtoshowthat
projectthemonthevectorGC(Cb ×Cb )toyieldthefollowing
C i j
systemof3equationsintheunknownrotationGCC: k(cid:48)1 (cid:44)CT(k2,θ2)k1 =k3×k2
(Gp −Gp )TGC(Cb ×Cb )=0 (2)
1 2 C 1 2 Next,byemployingRodrigues’rotationformula[see(8)],forex-
(Gp −Gp )TGC(Cb ×Cb )=0 (3) pressing the product of a rotation matrix and a vector as a linear
(Gp1−Gp3)TCGC(Cb1×Cb3)=0 (4) functionoftheunknown(cid:2)cosθ sinθ(cid:3)T,i.e.,
2 3 C 2 3
Next, and in order to compute one of the 3 unknown degrees of C(k,θ)v=(−cosθ(cid:98)k(cid:99)2−sinθ(cid:98)k(cid:99)+kkT)v
rotationalfreedom,weintroducethefollowingfactorizationofGC: (cid:20) (cid:21)
C =(cid:2)−(cid:98)k(cid:99)2v −(cid:98)k(cid:99)v(cid:3) cosθ +(kTv)k (14)
GC=C(k ,θ )C(k ,θ )C(k ,θ ) (5) sinθ
C 1 1 2 2 3 3
where3 in(12)yields(fori=1,2):
k1 (cid:44) (cid:107)GGpp11−−GGpp22(cid:107), k3 (cid:44) (cid:107)CCbb11××CCbb22(cid:107), k2 (cid:44) (cid:107)kk11××kk33(cid:107)(6) ·(cid:18)(cid:18)(cid:2)(cid:2)−−(cid:98)(cid:98)kk1(cid:48)(cid:99)(cid:99)22uv(cid:48)i −(cid:98)k(cid:98)1k(cid:99)(cid:48)u(cid:99)iv(cid:3)(cid:48)(cid:20)(cid:3)cs(cid:20)oicnsoθθs11θ(cid:21)3(cid:21)++(k(T1ku(cid:48)Ti)vk(cid:48)1)(cid:19)k(cid:48)T(cid:19)=0 (15)
3 i 3 i sinθ 3 i 3
3
Substituting(5)in(2),yieldsascalarequationintheunknownθ :
2
kTC(k ,θ )k =0 (7) 4(cid:98)k(cid:99)denotesthe3×3skew-symmetricmatrixcorrespondingto
1 2 2 3 ksuchthat(cid:98)k(cid:99)a = k×a, ∀k,a ∈ R3. Notealsothatifkis
3C(k,θ)denotestherotationmatrixdescribingtherotationabout aunitvector,then(cid:98)k(cid:99)2 = kkT −I,whilefortwovectorsa,b,
theunitvector,k,byanangleθ. Notethatintheensuingderiva- (cid:98)a(cid:99)(cid:98)b(cid:99)=baT−(aTb)I.Lastly,itiseasytoshowthat(cid:98)(cid:98)a(cid:99)b(cid:99)=
tions,allrotationanglesaredefinedusingtheleft-handrule. baT −abT.
⇒(cid:2)cosφ sinφ(cid:3)= 1(cid:2)uTk uTk(cid:48)(cid:3) (21)
δ 1 2 1 3
where
′′ δ(cid:44)(cid:112)(uT1k(cid:48)3)2+(uT1k2)2 =(cid:107)u1×k1(cid:107) (22)
1 𝒌𝒌2 𝒌𝒌3 andthus[from(19)using(8)]
𝒖𝒖
k(cid:48)(cid:48) =cosφk(cid:48) −sinφ(cid:98)k (cid:99)k(cid:48) +(1−cosφ)k kTk(cid:48)
3 3 1 3 1 1 3
𝜑𝜑 =(k(cid:48)kTu −k k(cid:48)Tu )/δ=u ×(k(cid:48) ×k )/δ
3 2 1 2 3 1 1 3 2
u ×k
= 1 1 (23)
(cid:107)u ×k (cid:107)
1 1
𝜃𝜃2 𝒌𝒌3
Now,wecanexpand(18)using(14)togetanequationanalogous
to(16):
𝒌𝒌1 𝜋𝜋1 ′ (cid:20)cosθ(cid:48)(cid:21)T(cid:20)uT(cid:98)k (cid:99)2(cid:98)k(cid:48)(cid:48)(cid:99)2v(cid:48)(cid:48) uT(cid:98)k (cid:99)2(cid:98)k(cid:48)(cid:48)(cid:99)v(cid:48)(cid:48)(cid:21)(cid:20)cosθ (cid:21)
1 i 1 3 i i 1 3 i 3
𝜋𝜋2 𝒌𝒌3 sinθ1(cid:48) uTi(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:48)(cid:99)2vi(cid:48)(cid:48) uTi(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:48)(cid:99)vi(cid:48)(cid:48) sinθ3
′ (cid:20) (cid:21)
Fkig,urke, 2k: , kG(cid:48),eokm(cid:48)e(cid:48)𝒌𝒌,traicn1druela.tioNnotebetthwaetenk ,unkit, avnedctokr(cid:48)s +(kT1ui)(cid:2)−kT1(cid:98)k(cid:48)3(cid:48)(cid:99)2vi(cid:48)(cid:48) −kT1(cid:98)k(cid:48)3(cid:48)(cid:99)vi(cid:48)(cid:48)(cid:3) csoinsθθ33 =
1 2 3 3 3 1 1 3 3
belongtoaplaneπ1whosenormalisk2. Also,k2, k(cid:48)3,andk(cid:48)3(cid:48) (k(cid:48)(cid:48)Tv(cid:48)(cid:48))(cid:2)uT(cid:98)k (cid:99)(cid:98)k(cid:48)(cid:48)(cid:99)k uT(cid:98)k (cid:99)k(cid:48)(cid:48)(cid:3)(cid:20)cosθ1(cid:48)(cid:21) (24)
lieonaplane,π ,normaltoπ . 3 i i 1 3 1 i 1 3 sinθ(cid:48)
2 1 1
Substituting uT(cid:98)k (cid:99)(cid:98)k(cid:48)(cid:48)(cid:99) = 0 [see (17)] in (24) and renaming
i 1 3
Expanding(15)andrearrangingterms,yields(fori=1,2) terms,yields(fori=1,2):
(cid:20)cosθ1(cid:21)T(cid:20)uTi(cid:98)k1(cid:99)2(cid:98)k(cid:48)3(cid:99)2vi(cid:48) uTi(cid:98)k1(cid:99)2(cid:98)k(cid:48)3(cid:99)vi(cid:48)(cid:21)(cid:20)cosθ3(cid:21) (cid:20)cosθ1(cid:48)(cid:21)T(cid:20)f¯i1 f¯i2(cid:21)(cid:20)cosθ3(cid:21)+(cid:2)f¯ f¯ (cid:3)(cid:20)cosθ3(cid:21)
sinθ1 uTi(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:99)2vi(cid:48) uTi(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:99)vi(cid:48) sinθ3 sinθ1(cid:48) 0 0 sinθ3 i4 i5 sinθ3
+(kTu )(cid:2)−kT(cid:98)k(cid:48)(cid:99)2v(cid:48) −kT(cid:98)k(cid:48)(cid:99)v(cid:48)(cid:3)(cid:20)cosθ3(cid:21) =(cid:2)0 f¯ (cid:3)(cid:20)cosθ1(cid:48)(cid:21)
1 i 1 3 i 1 3 i sinθ i3 sinθ(cid:48)
3 1
(cid:20) (cid:21) (cid:20) (cid:21)
=(k(cid:48)3Tvi(cid:48))(cid:2)uTi(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:99)k1 uTi(cid:98)k1(cid:99)k(cid:48)3(cid:3) csoinsθθ1 (16) ⇒(cid:2)f¯i1cosθ1(cid:48) +f¯i4 f¯i2cosθ1(cid:48) +f¯i5(cid:3) csoinsθθ3 =f¯i3sinθ1(cid:48)
1 3
(25)
NoticethatthetermuT(cid:98)k (cid:99)(cid:98)k(cid:48)(cid:99)appearsthreetimesin(16),and
i 1 3
where5
uT(cid:98)k (cid:99)(cid:98)k(cid:48)(cid:99)=uTk(cid:48)kT
1 1 3 1 3 1
=(Gp1−Gp3)T(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:99) f¯i1 (cid:44)uTi(cid:98)k1(cid:99)2(cid:98)k(cid:48)3(cid:48)(cid:99)2vi(cid:48)(cid:48) =δviTk2
=(Gp1−Gp2+Gp2−Gp3)T(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:99) f¯i2 (cid:44)uTi(cid:98)k1(cid:99)2(cid:98)k(cid:48)3(cid:48)(cid:99)vi(cid:48)(cid:48) =δviTk(cid:48)1
=(Gp −Gp )T(cid:98)k (cid:99)(cid:98)k(cid:48)(cid:99) f¯ (cid:44)(k(cid:48)(cid:48)Tv(cid:48)(cid:48))uT(cid:98)k (cid:99)k(cid:48)(cid:48) =δvTk
2 3 1 3 i3 3 i i 1 3 i 3
=uTk(cid:48)kT =uT(cid:98)k (cid:99)(cid:98)k(cid:48)(cid:99) (17) f¯ (cid:44)−(uTk )kT(cid:98)k(cid:48)(cid:48)(cid:99)2v(cid:48)(cid:48) =(uTk )(vTk(cid:48))
2 3 1 2 1 3 i4 i 1 1 3 i i 1 i 1
Thismotivatestorewrite(12)as(fori=1,2): f¯i5 (cid:44)−(uTik1)kT1(cid:98)k(cid:48)3(cid:48)(cid:99)vi(cid:48)(cid:48) =−(uTik1)(viTk2)
0=uTC(k ,θ )C(k(cid:48),θ )v(cid:48) Fori=1,2,(25)resultsintothefollowingsystem:
i 1 1 3 3 i
==uuTiTiCC((kk11,,θθ11)−C(φk)C1,(−Cφ()kC1(,kφ1)k,φ(cid:48)3),Cθ3()kC(cid:48)3(,kθ31),vφi(cid:48))vi(cid:48) (cid:20)ff¯¯1211ccoossθθ11(cid:48)(cid:48) ++ff¯¯1244 ff¯¯1222ccoossθθ11(cid:48)(cid:48) ++ff¯¯1255(cid:21)(cid:20)csoinsθθ33(cid:21)=(cid:20)ff¯¯2133(cid:21)sinθ1(cid:48)
(26)
=uTiC(k1,θ1(cid:48))C(k(cid:48)3(cid:48),θ3)vi(cid:48)(cid:48) (18) Notethatsincef¯ f¯ +f¯ f¯ =0,wecanfurthersimplify(26)
11 14 12 15
byintroducingθ(cid:48),where
where 3
θ1(cid:48) (cid:44)θ1−φ, vi(cid:48)(cid:48) (cid:44)C(k1,φ)vi(cid:48), k(cid:48)3(cid:48) (cid:44)C(k1,φ)k(cid:48)3 (19) (cid:20)csoinsθθ(cid:48)3(cid:48)(cid:21)(cid:44)(cid:104)f¯11co√sfθ¯32++f¯f1¯22sinθ3 −f¯14co√sθf¯32++f¯f1¯52sinθ3(cid:105)T (27)
3 11 12 14 15
Tosimplifytheequationanalogousto(16)thatwillresultfrom(18)
[insteadof(16)],weseektofindaφ(notnecessarilyunique)such Replacingθ3byθ3(cid:48) in(26),wehave
thatuT1k(cid:48)3(cid:48) =0,andhence,uTi(cid:98)k1(cid:99)(cid:98)k(cid:48)3(cid:48)(cid:99)=0[see(17)],i.e., (cid:20) f cosθ(cid:48) f (cid:21)(cid:20)cosθ(cid:48)(cid:21) (cid:20)f (cid:21)
11 1 15 3 = 13 sinθ(cid:48)
0=uT1k(cid:48)3(cid:48) =uT1C(k1,φ)k(cid:48)3 (20) f21cosθ1(cid:48) +f24 f22cosθ1(cid:48) +f25 sinθ3(cid:48) f23 1
=uT(cosφI−sinφ(cid:98)k (cid:99)+(1−cosφ)k kT)k(cid:48) (28)
1 1 1 1 3
=cosφuTk(cid:48) −sinφuT(cid:98)k (cid:99)k(cid:48) 5The simplified expressions for the following terms, shown after
1 3 1 1 3
thesecondequality,requirelengthyalgebraicderivationswhichwe
=cosφuTk(cid:48) −sinφuTk
1 3 1 2 omitduetospacelimitations.
where which, in general, will result in two different solutions for CC.
G
Notethoughthatonlyoneofthemisvalidifweusethefactthat
f (cid:44)δkTCb (29)
11 3 3 d >0(seeAppendix5.3).
i
f21 (cid:44)δ(CbT1Cb2)(kT3Cb3) (30) Next, for each pair of (cosθ1(cid:48),sinθ1(cid:48)), we compute cosθ3(cid:48) and
sinθ(cid:48) from(37),whichcanbewrittenas
f (cid:44)δ(kTCb )(cid:107)Cb ×Cb (cid:107) (31) 3
f22 (cid:44)f¯ 3=δv3Tk 1 2 (32) (cid:20)cosθ3(cid:48)(cid:21)= sinθ1(cid:48) (cid:20)g1cosθ1(cid:48) +g2(cid:21) (52)
f13 (cid:44)f¯13 =δv1Tk3 (33) sinθ3(cid:48) g5cos2θ1(cid:48) +g6cosθ1(cid:48) +g7 g3cosθ1(cid:48) +g4
23 23 2 3 Lastly,insteadoffirstcomputingθ from(19)andθ from(27)
1 3
f24 (cid:44)(uT2k1)(kT3Cb3)(cid:107)Cb1×Cb2(cid:107) (34) to find GCC using (5), we hereafter describe a faster method for
f15 (cid:44)−(uT1k1)(kT3Cb3) (35) recoveringGCC.Specifically,from(5),(12)and(18),wehave
f25 (cid:44)−(uT2k1)(CbT1Cb2)(kT3Cb3) (36) GCC=C(k1,θ1)C(k2,θ2)C(k3,θ3)
=C(k ,θ )C(k(cid:48),θ )C(k ,θ )
From(28),wehave 1 1 3 3 2 2
=C(k ,θ(cid:48))C(k(cid:48)(cid:48),θ )C(k ,φ)C(k ,θ ) (53)
(cid:20)cosθ(cid:48)(cid:21) (cid:18)(cid:20) f cosθ(cid:48) f (cid:21)(cid:19)−1 1 1 3 3 1 2 2
sinθ3(cid:48)3 =d(cid:20)etf cfo21sθc1o(cid:48)1s+θ1(cid:48)f+1f24 f−22fcosθ1(cid:21)51(cid:48)(cid:20)+ff2(cid:21)5 SC¯inscuechk1thiastperpendiculartok(cid:48)3(cid:48),wecanconstructarotationmatrix
· −(f2221cosθ11(cid:48) +f2254) f11co1s5θ1(cid:48) f1233 sinθ1(cid:48) C¯ =(cid:2)k1 k(cid:48)3(cid:48) k1×k(cid:48)3(cid:48)(cid:3)
(37)
andhence
Computingthenormofbothsidesof(37),resultsin k =C¯e , k(cid:48)(cid:48) =C¯e (54)
1 1 3 2
(cid:13)(cid:13)(cid:13)(cid:13)(cid:20)−f(f2221ccoossθθ1(cid:48)1(cid:48)++ff2254) f11−cfo1s5θ1(cid:48)(cid:21)(cid:20)ff1233(cid:21)(cid:13)(cid:13)(cid:13)(cid:13)2(1−cos2θ1(cid:48)) where (cid:2)e e e (cid:3)(cid:44)I
(cid:18)(cid:20) f cosθ(cid:48) f (cid:21)(cid:19)2 1 2 3 3
=det f c1o1sθ(cid:48) +1f f cosθ15(cid:48) +f Substituting(54)in(53),wehave
21 1 24 22 1 25
GC=C¯C(e ,θ(cid:48))C(e ,θ )C¯TC(k ,φ)C(k ,θ )
which is a 4th-order polynomial in cosθ(cid:48) that can be compactly C 1 1 2 3 1 2 2
1
writtenas: =C¯C(e ,θ(cid:48))C(e ,θ )C(e ,θ(cid:48) −θ )C¯¯
1 1 2 3 2 3 3
(cid:88)4 α cosjθ(cid:48) =0 (38) =C¯C(e1,θ1(cid:48))C(e2,θ3(cid:48))C¯¯ (55)
j 1
j=0 where
with C¯¯ (cid:44)C(e ,θ −θ(cid:48))C¯TC(k ,φ)C(k ,θ )
2 3 3 1 2 2
α4 (cid:44)g52+g12+g32 (39) =C(e2,θ3−θ3(cid:48))(cid:2)k(cid:48)1 k3 k(cid:48)1×k3(cid:3)T
α3 (cid:44)2(g5g6+g1g2+g3g4) (40) (=27)(cid:2)Cb1 k3 Cb1×k3(cid:3)T
α2 (cid:44)g62+2g5g7+g22+g42−g12−g32 (41) Theadvantagesof(55)are:(i)ThematrixproductC(e1,θ1(cid:48))C(e2,θ3(cid:48))
α (cid:44)2(g g −g g −g g ) (42) canbecomputedanalytically;(ii)C¯,C¯¯ areinvarianttothe(upto)
1 6 7 1 2 3 4
four possible solutions and thus, we only need to construct them
α (cid:44)g2−g2−g2 (43)
0 7 2 4 once.
g (cid:44)f f (44)
1 13 22 2.3 Solvingfortheposition
g (cid:44)f f −f f (45)
2 13 25 15 23 Substitutingin(1)theexpressionford from(62)andrearrang-
3
g3 (cid:44)f11f23−f13f21 (46) ingterms,yields
g4 (cid:44)−f13f24 (47) Gp =Gp − δsinθ1(cid:48)GCCb (56)
g5 (cid:44)f11f22 (48) C 3 kT3Cb3C 3
g (cid:44)f f −f f (49) Note that we only use (1) for i = 3 to compute Gp from GC.
6 11 25 15 21 C C
Alternatively,wecouldfindthepositionusingaleast-squaresap-
g (cid:44)−f f (50)
7 15 24 proachbasedon(1)fori = 1,2,3(seeAppendix5.4),ifwecare
Wecomputetherootsof(38)inclosedformtofindcosθ(cid:48). Simi- moreforaccuracythanspeed.Lastly,theproposedP3Psolutionis
1
larlyto[15]and[18],weemployFerrari’smethod[2]toattainthe summarizedinAlg.1.
resolventcubicof(38),whichissubsequentlysolvedbyCardano’s
formula[2]. Oncethe(upto)fourrealsolutionsof(38)havebeen 3. EXPERIMENTALRESULTS
determined, an optional step is to apply root polishing following Ouralgorithmisimplemented6inC++usingthesamelinearal-
Newton’smethod,whichimprovesaccuracyforminimalincrease
gebralibrary,TooN[22],as[15].Weemploysimulationdatatotest
intheprocessingcost(seeSection3.2). Regardless,foreachsolu-
ourcodeandcompareittothesolutionsof[15]and[18].Foreach
tionofcosθ(cid:48),wewillhavetwopossiblesolutionsforsinθ(cid:48),i.e.,
1 1
6Ourcodeissubmittedalongwiththepaperassupplementalma-
sinθ(cid:48) =±(cid:112)1−cos2θ(cid:48) (51) terial.
1 1
Algorithm1:Solvingforthecamera’spose
Input:Gp , i=1,2,3thefeatures’positions;
i
Cb , i=1,2,3bearingmeasurements
i
Output:Gp ,thepositionofthecamera;CC,theorientation
C G
ofthecamera
1 Computek1,k3using(6) #104
2 Computeuiandviusing(11),i=1,2 2.5
3 Computeδandk(cid:48)3(cid:48)using(22)and(23) PPrrooppoosseedd mmeetthhoodd+Root polishing
4 Computethefij’susing(29)-(36) Masselli's method
5 Computeαi,i=0,1,2,3,4using(39)-(50) 2 Kneip's method
6 Solve(38)togetn(n=2or4)realsolutionsforcosθ1(cid:48),
denotedascosθ(cid:48)(i),i=1...n
1 1.5
7 fori=1:ndo
(cid:113) tn
8 sinθ1(cid:48)(i) ←sign(kT3Cb3) 1−cos2θ1(cid:48)(i) uoc
9 Computecosθ3(cid:48)(i)andsinθ3(cid:48)(i)using(52) 1
10 ComputeCGC(i)using(55)
11 ComputeGp(Ci)using(56) 0.5
12 end
0
-18 -16 -14 -12 -10 -8 -6
position orientation log10(error) (Orientation)
Kneip’smethod 1.18E-05 1.02E-05
Masselli’smethod 1.84E-08 4.89E-10 Figure3:Nominalcase:Histogramoforientationerrors.
Proposedmethod 1.66E-10 5.30E-12
Proposedmethod+Rootpolishing 5.07E-11 1.53E-13
Table1:Nominalcase:Posemeanerrors.
single P3P problem, we randomly generate three 3D landmarks,
whichareuniformlydistributedina0.4×0.3×0.4cuboidcen-
teredaroundtheorigin. ThepositionofthecameraisGp = e ,
C 3
anditsorientationisCC=C(e ,π).
G 1
3.1 Numericalaccuracy
Wegeneratesimulationdatawithoutaddinganynoiseorround-
18000
ingerrortothebearingmeasurements,andrunallthreealgorithms
Proposed method+Root polishing
on 50,000 randomly-generated configurations to assess their nu- 16000 Proposed method
mericalaccuracy. Notethatthepositionerroriscomputedasthe Masselli's method
Kneip's method
normofthedifferencebetweentheestimateandthegroundtruth 14000
ofGp . Asfortheorientationerror,wecomputetherotationma-
C
trixthattransformstheestimatedGCtothetrueone,convertitto 12000
C
theequivalentaxis-anglerepresentation,andusetheabsolutevalue
10000
of the angle as the error. Since there are multiple solutions to a tn
u
o
P3Pproblem, wecomputetheerrorsforallofthemandpickthe c 8000
smallestone(i.e.,therootclosesttothetruesolution).
Thedistributionsandthemeansofthepositionandorientation 6000
errorsaredepictedinFig.s3-4andTable3.1. Asevident,weget
4000
similarresultstothosepresentedin[18]forKneipetal.’s[15]and
MasselliandZell’smethods[18],whileourapproachoutperforms 2000
bothofthembytwoordersofmagnitudeintermsofaccuracy.This
canbeattributedtothefactthatouralgorithmrequiresfeweroper- 0
-18 -16 -14 -12 -10 -8 -6
ationsandthusexhibitslowernumerical-errorpropagation. log10(error) (Position)
Furthermore, and as shown in the results of Table 3.1, we can
further improve the numerical precision by applying root polish- Figure4:Nominalcase:Histogramofpositionerrors.
ing. Typically,twoiterationsofNewton’smethod[24]leadtosig-
nificantlybetterresults,especiallyfortheorientation,whiletaking
only0.01µsperiteration,orabout4%ofthetotalprocessingtime.
3.2 Processingcost
Weuseatestprogramthatsolves100,000randomlygenerated
position orientation
18000
Kneip’smethod 1.42E-14 1.34E-14
Proposed method
Masselli’smethod 7.13E-15 6.15E-15 16000 Masselli's method
Proposedmethod 5.16E-15 3.73E-15 Kneip's method
14000
Table2:Singularcase1:Posemedianerrors.
12000
position orientation
10000
Kneip’smethod 8.10E-14 8.85E-14 tn
u
o
Masselli’smethod 7.24E-14 6.07E-14 c 8000
Proposedmethod 6.73E-14 1.75E-14
6000
Table3:Singularcase2:Posemedianerrors.
4000
2000
P3Pproblemsandcalculatesthetotalexecutiontimetoevaluatethe
computationalcostofthethreealgorithmsconsidered.Weruniton 0
-18 -16 -14 -12 -10 -8 -6
a2.0GHz×4Corelaptopandtheresultsshowthatourcodetakes log10(error) (Position)
0.54 µs on average (0.52 µs without root polishing) while [15]
and[18]take1.3µsand1.5µs,respectively.Thiscorrespondstoa Figure5:Singularcase1:Histogramofpositionerrors.
2.5×speedup(or40%ofthetimeof[15]). Notealso,incontrast
to what is reported in [18], Masselli’s method is actually slower
thanKneip’s. Asmentionedearlier, Masselli’sresultsin[18]are 18000
basedon1,000runsofthesamefeatures’configuration,andtake Proposed method
advantageofdatacachingtooutperformKneip. 16000 Masselli's method
Kneip's method
3.3 Robustness 14000
Therearetwotypicalsingularcasesthatleadtoinfinitesolutions 12000
intheP3Pproblem:
10000
• Singularcase1:The3landmarksarecollinear. tn
u
o
c 8000
• Singular case 2: Any two of the 3 bearing measurements
coincide. 6000
Inpractice,itisalmostimpossiblefortheseconditionstoholdex- 4000
actly, but we may still have numerical issues when the geomet-
ric configuration is close to these cases. To test the robustness 2000
of the three algorithms considered, we generate simulation data
0
correspondingtosmallperturbations(uniformlydistributedwithin -18 -16 -14 -12 -10 -8 -6
[−0.05 0.05])ofthefeatures’positionswheninsingularconfigu- log10(error) (Orientation)
rations.TheerrorsaredefinedasinSection3.1,whilewecompute
Figure6:Singularcase1:Histogramoforientationerrors.
themediansofthemtoassesstherobustnessofthethreemethods.
Forfairness,wedonotapplyrootpolishingtoourcodehere. Ac-
cordingtotheresultsshowninFig.s5-8andTables3.3-3.3,our
method achieves the best accuracy in these two close-to-singular 12000
cases. The reason is that we do not compute any quantities that Proposed method
Masselli's method
may suffer from numerical issues, such as cotangent and tangent 10000 Kneip's method
in[15]and[18],respectively.
4. CONCLUSIONANDFUTUREWORK 8000
Inthispaper,wehaveintroducedanalgebraicapproachforcom-
putingthesolutionsoftheP3Pprobleminclosedform. Similarly tnu 6000
o
c
to [15] and [18], our algorithm does not solve for the distances
first, and hence reduces numerical-error propagation. Differently
4000
though, it does not involve numerically-unstable functions (e.g.,
tangent,orcotangent)andhassimplerexpressionsthanthetwore-
centalternativemethods[15,18],andthusitoutperformsthemin 2000
termsofspeed,accuracy,androbustnesstoclose-to-singularcases.
As part of our ongoing work, we are currently extending our
0
approach to also address the case of the generalized (non-central -18 -16 -14 -12 -10 -8 -6
log10(error) (Position)
camera)P3P[20].
Figure7:Singularcase2:Histogramofpositionerrors.
5. APPENDIX
12000 Ifweuseθ(2)tofindGC,
2 C
Proposed method
10000 MKnaesispe'sll im's emtheothdod GCC=C(k1,θ1(2))C(k2,θ2(2))C(k3,θ3(2))
(=59)C(k ,θ(2))C(k ,π)C(k ,θ(1))C(k ,π)C(k ,θ(2))
1 1 1 2 2 3 3 3
8000 =C(k ,θ(2)+π)C(k ,θ(1))C(k ,θ(2)+π) (60)
1 1 2 2 3 3
Note that GC in (60) is of the same form as that in (5), so any
tnuo 6000 solutionsoCfGCcomputedusingθ(2) willbefoundbyusingθ(1).
c C 2 2
Thus,wedonotneedtoconsideranyothersolutionsforθ andθ
1 3
4000 beyondtheonesfoundforGC.
C
5.3 Determiningthesignofsinθ(cid:48)
1
2000
From (51), we have two solutions for sinθ(cid:48), and thus for θ(cid:48),
1 1
withθ(cid:48)(2) = −θ(cid:48). Thiswillalsoresultintotwosolutionsforθ(cid:48)
1 1 3
0 [see(52)]and,hence,twosolutionsforθ : θ andθ(2) =θ +π.
-18 -16 -14 -12 -10 -8 -6 3 3 3 3
log10(error) (Orientation) Considering these two options, we get two distinct solutions for
GC[see(53)]:
C
Figure8:Singularcase2:Histogramoforientationerrors.
C (cid:44)C(k ,θ(cid:48))C(k(cid:48)(cid:48),θ )C(k ,φ)C(k ,θ )
1 1 1 3 3 1 2 2
C (cid:44)C(k ,−θ(cid:48))C(k(cid:48)(cid:48),θ +π)C(k ,φ)C(k ,θ )
2 1 1 3 3 1 2 2
5.1 Proofofk(cid:48) =k ×k
3 2 1 Then,noticethat
First,notethatk ×k isaunitvectorsincek isperpendicular
tok1.Also,from(123)an1d(7)wehave 2 C2CT1 =C(k1,−θ1(cid:48))C(k(cid:48)3(cid:48),π)C(k1,−θ1(cid:48))
=C(k(cid:48)(cid:48),π)C(CT(k(cid:48)(cid:48),π)k ,−θ(cid:48))C(k ,−θ(cid:48))
3 3 1 1 1 1
kT1k(cid:48)3 =kT1C(k2,θ2)k3 =0 (57) =C(k(cid:48)(cid:48),π)C(−k ,−θ(cid:48))C(k ,−θ(cid:48))
3 1 1 1 1
Then, we can prove k(cid:48)3 = k2 ×k1 by showing that their inner =C(k(cid:48)3(cid:48),π)
productisequalto1:
IfC =C ,thiswouldrequire
1 2
(k2×k1)Tk(cid:48)3 =kT1(k(cid:48)3×k2) C(k(cid:48)3(cid:48),π)=C2CT1 =I
=(6) kT1(k(cid:48)3×(k1×k3)) which cannot be true, hence C1 and C2 cannot be equal. Thus,
(cid:107)k ×k (cid:107) therearealwaystwodifferentsolutionsofGC.
1 3 C
=(9) kT1(k1(kT3k(cid:48)3)−k3(kT1k(cid:48)3)) canIf,dehtoewrmevineer,twheesuisgenthoefsfainctθt(cid:48)h,aatnddic(hio=ose1,th2e,3v)aliisdpoonseitiavme,ownge
cosθ 1
2 thetwosolutionsofGC.Subtracting(1)pairwisefor(i=3)from
kTC(k ,θ )k C
= 3 2 2 3 =1 (i=1),wehave
cosθ
2
Gp −Gp =d GCCb −d GCCb
1 3 1C 1 3C 3
5.2 Equivalence between the two solutions of ⇒Gp −Gp =C(k ,θ(cid:48))C(k(cid:48)(cid:48),θ )C(k ,φ)
1 3 1 1 3 3 1
θ
2 ·C(k ,θ )(d Cb −d Cb ) (61)
2 2 1 1 3 3
When solving for θ [see (9)], we have two possible solutions
2
θ2(1) = arccos(kT1k3)− π2 andθ2(2) = θ2(1) +π. Next, wewill Multiplyingbothsidesof(61)withk(cid:48)3(cid:48)TC(k1,−θ1(cid:48))fromtheleft,
provethatusingθ(2) tofindGCisequivalenttousingθ(1). First, yields
2 C 2
notethat(seeFig.2) k(cid:48)(cid:48)TC(k ,−θ(cid:48))(Gp −Gp )
3 1 1 1 3
π π =k(cid:48)(cid:48)TC(k ,φ)C(k ,θ )(d Cb −d Cb )
C(k ,θ(1)+ )k =C(k , )k(cid:48) =−k ×k(cid:48) 3 1 2 2 1 1 3 3
2 2 2 3 2 2 3 2 3 ⇒k(cid:48)(cid:48)T(cosθ(cid:48)I+sinθ(cid:48)(cid:98)k (cid:99)+(1−cosθ(cid:48))k kT)u
=−k ×(k ×k )=k (58) 3 1 1 1 1 1 1 1
2 2 1 1
=k(cid:48)TC(k ,θ )(d Cb −d Cb )
3 2 2 1 1 3 3
Then,wecanwriteC(k2,θ2(2))as (⇒23)sinθ1(cid:48)k(cid:48)3(cid:48)T(cid:98)k1(cid:99)u1 =kT3(d1Cb1−d3Cb3)
⇒−sinθ(cid:48)uT(cid:98)k (cid:99)k(cid:48)(cid:48) =−d kTCb
C(k ,θ(2)) 1 1 1 3 3 3 3
2 2 ⇒δsinθ(cid:48) =d (kTCb ) (62)
π π 1 3 3 3
=C(k ,θ(1)+ )C(k , )
2 2 2 2 2 Usingthefactthatd > 0andδ > 0,weselectthesignofsinθ(cid:48)
=C(k2,θ2(1)+ π2)C(k3,π)C(k2,−π2)C(k3,−π) tobethesameastha3tofkT3Cb3. 1
=C(C(k ,θ(1)+ π)k ,π)C(k ,θ(1)+ π)C(k ,−π)C(k ,π) 5.4 Least-squaressolutionfortheposition
2 2 2 3 2 2 2 2 2 3
Gp canalsobesolvedfollowingaleast-squaresapproach,which
C
(=58)C(k ,π)C(k ,θ(1))C(k ,π) (59) isslowerbutmoreaccuratethan(56).Specifically,(1)canresultin
1 2 2 3
thefollowingsystem: From(63)and(27),weget
GCCCb1 GCCCb2 GC Cb III ddd123 =GGGppp12 cosψ= (cid:112)f¯12f1¯1+1 vf¯1T22k=−(cid:112)f¯12f4¯1+5 f¯125
C 3 Gp 3 = 1 2
C (cid:112)
(vTk )2+(vTk(cid:48))2
1 2 1 1
CbTk(cid:48)
Then,weonlyneedtoapplyQRdecomposition[7]andbacksolve = (cid:112) 1 1
forGp (i.e.,wedonotneedtocomputed , i=1,2,3). (CbT1k2)2+(CbT1k(cid:48)1)2
C i
CbTk(cid:48)
= 1 1
(cid:107)k3×Cb1(cid:107)
5.5 Derivationoff¯ij =CbTk(cid:48)
1 1
f¯ (cid:44)uT(cid:98)k (cid:99)2(cid:98)k(cid:48)(cid:48)(cid:99)2v(cid:48)(cid:48)
i1 i 1 3 i
f¯ f¯
=uTi(cid:98)k1(cid:99)(k(cid:48)3(cid:48)kT1 −(kT1k(cid:48)3(cid:48))I)(cid:98)k(cid:48)3(cid:48)(cid:99)vi(cid:48)(cid:48) sinψ= (cid:112)f¯21+2 f¯2 = (cid:112)f¯21+4 f¯2
=(uT(cid:98)k (cid:99)k(cid:48)(cid:48))(kT(cid:98)k(cid:48)(cid:48)(cid:99)v(cid:48)(cid:48)) 11 12 14 15
i 1 3 1 3 i vTk(cid:48)
=((ui×k1)Tk(cid:48)3(cid:48))(kT1C(k1,φ)C(k2,θ2)(cid:98)k3(cid:99)vi) = (cid:112)(vTk )21+1(vTk(cid:48))2
1 2 1 1
=δk(cid:48)T(cid:98)k (cid:99)v
1 3 i =−CbTk
1 2
=δvTk
i 2
f¯i2 (cid:44)uTi(cid:98)k1(cid:99)2(cid:98)k(cid:48)3(cid:48)(cid:99)vi(cid:48)(cid:48) Then,from(26)and(28),wederivetheexpressionsoffij:
=(uT(cid:98)k (cid:99)k(cid:48)(cid:48))(kTv(cid:48)(cid:48))
i 1 3 1 i
=δ(kT1C(k1,φ)C(k2,θ2)vi) f11 =f¯11cosψ+f¯12sinψ
=δviTk(cid:48)1 =δ(CbT3k3)((CbT1k2)2+(CbT1k(cid:48)1)2)
f¯i3 (cid:44)(k(cid:48)3(cid:48)Tvi(cid:48)(cid:48))uTi(cid:98)k1(cid:99)k(cid:48)3(cid:48) =δ(CbT3k3)
=δkT3C(k2,−θ2)C(k1,−φ)C(k1,φ)C(k2,θ2)vi f21 =f¯21cosψ+f¯22sinψ
=δviTk3 =δ(CbT3k3)((CbT2k2)(CbT1k2)+(CbT2k(cid:48)1)(CbT1k(cid:48)1))
f¯i4 (cid:44)−(uTik1)kT1(cid:98)k(cid:48)3(cid:48)(cid:99)2vi(cid:48)(cid:48) =δ(CbT3k3)(CbT2(k2kT2 +k(cid:48)1k(cid:48)1T)Cb1)
=−(uTk )k (k(cid:48)(cid:48)k(cid:48)(cid:48)T −I)v(cid:48)(cid:48) =δ(CbTk )(CbT(I−k kT)Cb )
i 1 1 3 3 i 3 3 2 3 3 1
=(uTk )(k v(cid:48)(cid:48)) =δ(CbTk )(CbTCb )
i 1 1 i 3 3 2 1
=(uTk )(vTk(cid:48)) f =−f¯ sinψ+f¯ cosψ
i 1 i 1 22 21 22
f¯i5 (cid:44)−(uTik1)kT1(cid:98)k(cid:48)3(cid:48)(cid:99)vi(cid:48)(cid:48) =−(uTik1)(viTk2) =δ(CbT3k3)((CbT2k(cid:48)1)(CbT1k2)−(CbT2k2)(CbT1k(cid:48)1))
=δ(CbTk )(CbT(k kT +k(cid:48)k(cid:48)T)Cb )
3 3 2 2 2 1 1 1
5.6 Derivationoffij andC¯¯ =δ(CbT3k3)(Cb2×Cb1)T(k(cid:48)1×k2)
First,notethat =δ(CbT3k3)(cid:107)Cb2×Cb1(cid:107)kT3k3
=δ(CbTk )(cid:107)Cb ×Cb (cid:107)
3 3 2 1
viTk2 =(Cbi×Cb3)T(k(cid:48)1×k3) f15 =f¯15cosψ−f¯14sinψ
=(CbTik(cid:48)1)(CbT3k3)−(CbTik3)(CbT3k(cid:48)1) =−(uT1k1)f11/δ
=(CbTik(cid:48)1)(CbT3k3) =−(uT1k1)(CbT3k3)
viTk(cid:48)1 =−(Cbi×Cb3)T(k2×k3) f24 =f¯25sinψ+f¯24cosψ
=−(CbTik2)(CbT3k3)+(CbTik3)(CbT3k2) =(uT2k1)f22/δ
=−(CbTik2)(CbT3k3) =(uT2k1)(CbT3k3)(cid:107)Cb2×Cb1(cid:107)
f =f¯ cosψ−f¯ sinψ
25 25 24
Letψ(cid:44)θ3−θ3(cid:48),andthus =−(uT2k1)f21/δ
=−(uTk )(CbTk )(CbTk(cid:48))
2 1 3 3 1 1
(cid:20)cosθ(cid:48)(cid:21) (cid:20)cosψcosθ +sinψsinθ (cid:21)
3 = 3 3 (63)
sinθ3(cid:48) cosψcosθ3+sinψsinθ3 Additionally,wecanderivetheexpressionofC¯¯,whichisdefined
15000 14000
Gao' method Gao' method
Proposed method Proposed method
Masselli's method 12000 Masselli's method
Kneip's method Kneip's method
10000
10000
8000
tn tn
u u
o o
c c 6000
5000
4000
2000
0 0
-18 -16 -14 -12 -10 -8 -6 -18 -16 -14 -12 -10 -8 -6
log10(error) (Orientation) log10(error) (Position)
Figure9:Nominalcase:Histogramoforientationerrors. Figure10:Nominalcase:Histogramofpositionerrors.
in(55):
problem.IEEETransactionsonPatternAnalysisand
C¯¯ =C(e ,θ −θ(cid:48))C¯TC(k ,φ)C(k ,θ ) MachineIntelligence,25(8):930–943,2003.
2 3 3 1 2 2
[7] G.H.GolubandC.F.VanLoan.Matrixcomputations,
=C(e2,ψ)(cid:2)k1 k(cid:48)3 k2(cid:3)TC(k2,θ2) volume3.JHUPress,2012.
=C(e ,ψ)(cid:2)k(cid:48) k k (cid:3)T [8] E.W.Grafarend,P.Lohse,andB.Schaffrin.
2 1 3 2
Dreidimensionalerrückwärtsschnittteili:Dieprojektiven
=(cid:2)cosψk(cid:48)1−sinψk2 k3 sinψk(cid:48)1+cosψk2(cid:3)T gleichungen.ZeitschriftfürVermessungswesen,pages1–37,
=(cid:2)(k(cid:48)k(cid:48)T +k kT)Cb k sinψk(cid:48) +cosψk (cid:3)T 1989.
1 1 2 2 1 3 1 2 [9] J.A.Grunert.Daspothenotischeprobleminerweiterter
=(cid:2)(I−k3kT3)Cb1 k3 sinψk(cid:48)1+cosψk2(cid:3)T gestaltnebstüberseineanwendungenindergeodäsie.
=(cid:2)Cb k sinψk(cid:48) +cosψk (cid:3)T Grunertsarchivfürmathematikundphysik,1:238–248,
1 3 1 2 1841.
=(cid:2)Cb k Cb ×k (cid:3)T [10] R.M.Haralick,H.Joo,C.-N.Lee,X.Zhuang,V.G.Vaidya,
1 3 1 3
andM.B.Kim.Poseestimationfromcorrespondingpoint
5.7 ComparisonwiththeP3PcodeinOpenCV data.IEEETransactionsonSystems,Man,andCybernetics,
WealsocomparedtheperformanceofourcodewiththatinOpenCV 19(6):1426–1446,1989.
[11] R.M.Haralick,D.Lee,K.Ottenburg,andM.Nolle.
(basedon[6]),usingthesamesetupasSec.3. Theerrordistribu-
Analysisandsolutionsofthethreepointperspectivepose
tionsareshowedinFig.9andFig.10.Itisobviousthatthecodein
estimationproblem.InProc.oftheIEEEConferenceon
OpenCVhaslowernumericalaccuracycomparingtoours.Also,it
ComputerVisionandPatternRecognition,pages592–598,
takesaround3µsonaveragetocomputeP3Ponce,whichismuch
Lahaina,HI,June3–61991.
slowerthanours(0.52µsaccordingtoSec.3).
[12] J.A.HeschandS.I.Roumeliotis.Adirectleast-squares
In conclusion, our code performs much better than the one in
(DLS)methodforPnP.InProc.ofthe13thInternational
OpenCV.
ConferenceonComputerVision,pages383–390,Barcelona,
Spain,Nov.6–132011.
6. REFERENCES
[13] B.K.Horn.Closed-formsolutionofabsoluteorientation
[1] A.AnsarandK.Daniilidis.Linearposeestimationfrom usingunitquaternions.JournaloftheOpticalSocietyof
pointsorlines.IEEETransactionsonPatternAnalysisand AmericaA,4(4):629–642,1987.
MachineIntelligence,25(5):578–589,2003. [14] B.K.Horn,H.M.Hilden,andS.Negahdaripour.
[2] G.Cardano,T.R.Witmer,andO.Ore.TheRulesofAlgebra: Closed-formsolutionofabsoluteorientationusing
ArsMagna,volume685.CourierCorporation,2007. orthonormalmatrices.JournaloftheOpticalSocietyof
[3] D.A.Cox,J.Little,andD.O’Shea.Usingalgebraic AmericaA,5(7):1127–1135,1988.
geometry,volume185.SpringerScience&BusinessMedia, [15] L.Kneip,D.Scaramuzza,andR.Siegwart.Anovel
2006. parametrizationoftheperspective-three-pointproblemfora
[4] S.FinsterwalderandW.Scheufele.Das directcomputationofabsolutecamerapositionand
rückwärtseinschneidenimraum.Verlagd.Bayer.Akad.d. orientation.InProc.oftheIEEEConferenceonComputer
Wiss.,1903. VisionandPatternRecognition,pages2969–2976,Colorado
[5] M.A.FischlerandR.C.Bolles.Randomsampleconsensus: Springs,CO,June21–252011.
aparadigmformodelfittingwithapplicationstoimage [16] D.Koks.Aroundaboutroutetogeometricalgebra.
analysisandautomatedcartography.Communicationsofthe ExplorationsinMathematicalPhysics:TheConceptsbehind
ACM,24(6):381–395,1981. anElegantLanguage,pages147–184,2006.
[6] X.-S.Gao,X.-R.Hou,J.Tang,andH.-F.Cheng.Complete [17] S.Linnainmaa,D.Harwood,andL.S.Davis.Pose
solutionclassificationfortheperspective-three-point determinationofathree-dimensionalobjectusingtriangle
pairs.IEEETransactionsonPatternAnalysisandMachine
Intelligence,10(5):634–647,1988.
[18] A.MasselliandA.Zell.Anewgeometricapproachforfaster
solvingtheperspective-three-pointproblem.InProc.ofthe
IEEEInternationalConferenceonPatternRecognition,
pages2119–2124,Stockholm,Sweden,Aug.24–282014.
[19] E.Merritt.Explicitthree-pointresectioninspace.
PhotogrammetricEngineering,15(4):649–655,1949.
[20] D.NistérandH.Stewénius.Aminimalsolutiontothe
generalised3-pointposeproblem.JournalofMathematical
ImagingandVision,27(1):67–79,2007.
[21] L.QuanandZ.Lan.Linearn-pointcamerapose
determination.IEEETransactionsonPatternAnalysisand
MachineIntelligence,21(8):774–780,1999.
[22] TooN.C++LinearAlgebraLibrary.Available:https://
www.edwardrosten.com/cvd/toon/html-user/.
AccessedNov.15,2016.
[23] W.Wen-Tsun.Basicprinciplesofmechanicaltheorem
provinginelementarygeometries.JournalofAutomated
Reasoning,2(3):221–252,1986.
[24] T.J.Ypma.HistoricaldevelopmentoftheNewton-Raphson
method.SIAMreview,37(4):531–551,1995.