相机姿态估计（三）--P3P

原文：
http://iplimage.com/blog/p3p-perspective-point-overview/
直接复制过来了，排版乱七八糟，看原文吧。

P3P principle

Four 3D/2D correspondences are given: points in 3D that are defined in the 3D model coordinate system, and points in 2D that are defined in the image coordinate system (AA↔ uu,BB↔ vv,CC↔ww,DD↔zz).
Three points (AA,BB,CC)
are used to solve the P3P equations system (see later) and thus determine up to four possible sets of distances∥PA∥∥PA∥,∥PB∥∥PB∥
and ∥PC∥∥PC∥,
with PP
the camera optical center.
Then the four sets of distances are converted into four pose configurations.
The fourth point (DD)
is then used to select the best pose configuration against the “up to four” proposed.




Main P3P picture: A,B,C projected in u,v,w

Law of cosines

The law of cosines is the root of the P3P equations. It expresses the lengths of the sides of a triangle using angles. Using our triangleABCABC
(the one we want to use in our P3P problem), we consider first the
ABAB
side, the center of projection
PP
where the world coordinate system is placed, and
uu,vv
the two projections of AA
and BB




Law of cosines in 2D (u & v denote A & B projections on the image plane)

After simple calculations, we can see that

PA2+PB2–2.PA.PB.cosαu,v=AB2PA2+PB2–2.PA.PB.cosαu,v=AB2

P3P equation system

The P3P equation system is based on the law of cosines that links affine distances with angles. Applying law of cosines, we can obtain the P3P equation system:

⎧⎩⎨⎪⎪PB2+PC2–2.PB.PC.cosαv,w–BC2=0PA2+PC2–2.PA.PC.cosαu,w–AC2=0PA2+PB2–2.PA.PB.cosαu,v–AB2=0{PB2+PC2–2.PB.PC.cosαv,w–BC2=0PA2+PC2–2.PA.PC.cosαu,w–AC2=0PA2+PB2–2.PA.PB.cosαu,v–AB2=0

Some conditions have to be respected such as angles that must be comprised between00
and ππ.
Distances must stay strictly positive and
AA,BB,
and CC
not co-linear.

Then dividing by PC2PC2,
and introducing y=PB/PCy=PB/PC,x=PA/PCx=PA/PC
we obtain:

⎧⎩⎨⎪⎪y2x2x2+++11y2–––2.y.cosαv,w2.x.cosαu,w2.x.y.cosαu,v–––BC2/PC2=0AC2/PC2=0AB2/PC2=0{y2+1–2.y.cosαv,w–BC2/PC2=0x2+1–2.x.cosαu,w–AC2/PC2=0x2+y2–2.x.y.cosαu,v–AB2/PC2=0

and finally let v=AB2/PC2v=AB2/PC2, av=BC2/PC2av=BC2/PC2
and bv=AC2/PC2bv=AC2/PC2.
That gives:

⎧⎩⎨⎪⎪y2y2x2+++11y2–––2.y.cosαv,w2.x.cosαu,w2.x.y.cosαu,v–––av=0bv=0v=0{y2+1–2.y.cosαv,w–av=0y2+1–2.x.cosαu,w–bv=0x2+y2–2.x.y.cosαu,v–v=0

The last equation gives us the relation
v=x2+y2–2.x.y.cosαu,vv=x2+y2–2.x.y.cosαu,v.
Replacing vv
in the first two equations, we obtain the simplified version of our P3P system:

{(1−a)y2−ax2−cosαv,wy+2acosαu,vxy+1=0(1−b)x2−by2−cosαu,wx+2bcosαu,vxy+1=0{(1−a)y2−ax2−cosαv,wy+2acosαu,vxy+1=0(1−b)x2−by2−cosαu,wx+2bcosαu,vxy+1=0

The goal of the P3P problem is to solve the P3P equation system using 3 correspondences and the values of aa,bb,cosαu,vcosαu,v,cosαv,wcosαv,w,
and cosαu,wcosαu,w.
Up to four set of distances
{PA,PB,PC}{PA,PB,PC}
are obtained and used to determine the pose of the
ABCABC
triangle in the camera coordinate system (centered at P).

P3P simplified equation system can be solved using the Wu Ritt’s zero decomposition method (refer to thepaper written by Gao and all for
more details).

Here-under are presented the main steps to solve the P3P equation system starting from the 3D-2D correspondences.

Step 1: Normalizing the data

Project image plane points
uu,vv,ww
onto the a unit sphere (centered on
PP).

Remove unit from image points:

⎧⎩⎨⎪⎪⎪⎪⎪⎪u′x=ux–cxfxu′y=uy–cyfyu′z=1{ux′=ux–cxfxuy′=uy–cyfyuz′=1

with fxfx
and fyfy
the focal values expressed in pixels, and
cxcx,cycy
the image optical center (also expressed in pixels).




Projection on the z=1plane

Then normalize using L2 norm:

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Nu=(u′2x+u′2y+u′2z)−−−−−−−−−−−−−√ux”=u′xNuuy”=u′yNuuz”=u′zNu{Nu=(ux′2+uy′2+uz′2)ux”=ux′Nuuy”=uy′Nuuz”=uz′Nu




Projection on the unit sphere

In the following, we replace
u”u”,v”v”,w”w”
by uu,vv,ww.

Step 2: Preparing the P3P equation system

Must be calculated:

angles αu,vαu,v,αv,wαv,w
and αu,wαu,w:

⎧⎩⎨⎪⎪cosαu,v=(ux∗vx+uy∗vy+uz∗vz)cosαu,w=(ux∗wx+uy∗wy+uz∗wz)cosαv,w=(vx∗wx+vy∗wy+vz∗wz){cosαu,v=(ux∗vx+uy∗vy+uz∗vz)cosαu,w=(ux∗wx+uy∗wy+uz∗wz)cosαv,w=(vx∗wx+vy∗wy+vz∗wz)

distances: ABAB,BCBC,ACAC:

⎧⎩⎨⎪⎪AB=sqrt((vx−ux)∗(vx−ux)+(vy−uy)∗(vy−uy)+(vz−uz)∗(vz−uz))BC=sqrt((wx−vx)∗(wx−vx)+(wy−vy)∗(wy−vy)+(wz−vz)∗(wz−vz))AC=sqrt((wx−ux)∗(wx−ux)+(wy−uy)∗(wy−uy)+(wz−uz)∗(wz−uz)){AB=sqrt((vx−ux)∗(vx−ux)+(vy−uy)∗(vy−uy)+(vz−uz)∗(vz−uz))BC=sqrt((wx−vx)∗(wx−vx)+(wy−vy)∗(wy−vy)+(wz−vz)∗(wz−vz))AC=sqrt((wx−ux)∗(wx−ux)+(wy−uy)∗(wy−uy)+(wz−uz)∗(wz−uz))

then aa
and bb
coefficients:

⎧⎩⎨a=BC2AB2b=AC2AB2{a=BC2AB2b=AC2AB2

Step 3: Solving the P3P equation system

Wu Ritt’s zero decomposition method leads us to the (xx,yy)
solution of the simplified P3P equation system. Depending on the values of the input parameterscosαu,vcosαu,v,cosαv,wcosαv,w,cosαu,wcosαu,w,aa
and bb,
we can find multiple degenerated solutions to the system. However, in this article we will first focus on the main solution that happens 99% of the time when P3P is applied to detect a plane. Degenerated solutions (or branches) are discussed inGao’s
paper and i’m not going to detail too much of it for now.

xx
and yy
computation is now detailed. We obtain a quartic polynomial in
xx
using method explained in Gao’s paper:

{a4x4+a3x3+a2x2+a1x1+a0=0b1y−b0=0{a4x4+a3x3+a2x2+a1x1+a0=0b1y−b0=0

aiai
and bibi
coefficients are explained in the
Appendix at the bottom of the document.

This leads us to a quartic equation in
xx
that can be solve using standard solver. For example:

Ferrari’s approach to transform the quartic polynomial to a cubic polynomial (wolfram’s page for complete description),
Cardan/Fontana’s approach to solve the resulting cubic polynomial (wikipedia’s page for simple explanations).

Finally, we obtain a value for
xx
and yy
from which we can extract values of distances
PAPA,PBPB
and PCPC
using v=x2+y2–2.x.y.cosαu,vv=x2+y2–2.x.y.cosαu,v as
explained previously.

Step 4: finding 3D coordinates for points
AA,BB,CC

Using the “up to four” solutions we found for distances
PAPA,PBPB
and PCPC,
we can thus obtain 3D coordinates for points
AA,BB,CC
by multiplying uu,vv
and ww
vectors with the corresponding distances (eg:
A=u⃗.∥PA∥A=u→.∥PA∥).

Step 5: Computing the rotation matrix and the translation vector

AA, BB,CC
3D coordinates are expressed in the optical center coordinate system (centered at P). We wish to compute the affine transformation (RR
and tt
that transform 3D points expressed in the object coordinate system into 3D points expressed in the optical center coordinate system.

The solution to that problem is given in an article written by Nghia Ho: “finding optimal rotation and translation between corresponding 3D points”. I
will not detail the technique since Ho’s article is very clear. If you want more detail on that part, please refer to the paper written
by Besl and McKay in 1992: “A Method for Registration of 3-D Shapes”.

We then obtain up to four
RR,tt
configurations and use the projection (zz)
of point DD
using each RR,tt
(and the intrinsic parameters) to decide the best one.

Appendix: Wu Ritt’s coefficients

For simplification, we denote
p=2cosαv,wp=2cosαv,w,q=2cosαu,wq=2cosαu,w,
and
a2a4=
+b2–2a–2b+2(1–r2)ba+1r=2cosαu,v
a4a3a2a1a0b1b0
a3=−2qa2–rpb2+4qa+(2q+pr)b+(r2q–2q+rp)ab–2q
a2=(2+q2)a2+(p2+r2–2)b2–(4+2q2)a–(pqr+p2)b–(pqr+r2)ab+q2+2
a1=–2qa2–rpb2+4qa+(pr+qp2–2q)b+(rp+2q)ab–2q
a0=a2+b2–2a+(2–p2)b–2ab+1
b1=b((p2–pqr+r2)a+(p2−r2)b–p2+pqr–r2)2
b0=((1–a–b)x2+(a–1)qx–a+b+1))((r3(a2+b2–2a–2b+(2–r2)ab+1)x3+r2(p+pa2–2rqab+2rqb–2rq–2pa–2pb+pr2b+4rqa+qr3ab–2rqa2+2pab+pb2–r2pb2)x2+(r5(b2–ab)–r4pqb+r3(q2–4a–2q2a+q2a2+2a2–2b2+2)+r2(4pqa–2pqab+2pqb–2pq–2pqa2)+r(p2b2–2p2b+2p2ab–2p2a+p2+p2a2))x+(2pr2–2r3q+p3–2p2qr+pq2r2)a2+(p3–2pr2)b2+(4qr3–4pr2–2p3+4p2qr–2pq2r2)a+(−2qr3+pr4+2p2qr–2p3)b+(2p3+2qr3–2p2qr)ab+pq2r2–2p2qr+2pr2+p3–2r3q)