Perspective-3-Point(P3P) Algorithm

Perspective-n-Point

Perspective-n-Point is the problem of estimating the pose of camera from given 3D points and their correspoinding 2D projections in the image. Assume we know Intrinsic Parameter and have corresponding 3D points from the images feature points.

Given the intrinsic Parameters, our goal is computing the 6 extrinsic parameters(3 rotation, 3 translation) of the camera.

 

Perspective-3-Point Algorithm

P3P(Perspective-3-Point) is an algorithm to solve the PnP problem with three point correspondences.

Cyrill Stachniss introduced P3P algorithm using Grunet's Method.  The algorithm consists of 2 step. 

1. Get Length of Projection Rays
2. Compute Orientation of the Camera

Step1. Get Length of Projection Rays

We will use cosine law to compute the length. So we need angle between the projection rays.  Since we know the intrinsic parameter, angle can be directly computed.

The 3D points from the environment is given, we can also directly compute the length between 3D points and apply the cosine law.

Now what we need is little bit of mathematical technique.

Then, solve one equation for u, put into other equations. As a result we can get 4-degree polynomial. By solving the polynomial, we can get 4 possible solutions for

s1, s2, s3.

Captured from the Cyrill Stachniss Lecture

So we need to eliminate this ambiguity to get the unique solution.

 

Step2. Compute Orientation of the Camera

There might be some method to solve get the unique solution, here we will use RANSAC. RANSAC is an iterative method to estimate parameters of mathematrical model from a set of observed data that contains outliers, by iterative random samplings and returning the model that has the best fit to remaining data. Assume we have more points than 3 points. 

1. Select 3 points randomly
2. Estimate parameters of P3P
3. Count the number of other points that support current hypothesis(return to 1)
4. Select best solution