一个三点（P3p）估计算法

三点算法

http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/QUAN/quan.html

Pre-processing of data

 

  For the pose determination problem, the camera is assumed to be calibrated, i.e. the calibration matrix is known. Given an image point , the direction vector of the viewing line of the image point is given as . The angle between the viewing lines of a pair of image points and is easily computed from

 

where is the image of the dual absolute conic.

 

The 3-point algebraic solutions

Given a camera at C and n correspondences between 3D reference points and their images , each pair of correspondences and gives a constraint on the unknown camera-point distances and :

where is the known distance between the i-th and j-th reference points and is the 3D viewing angle subtended at the perspective center by the i-th and j-th points, which can be measured from the calibrated images using (1).

This quadratic constraint can be rewritten as follows:

 

For n=3, the following polynomial system is obtained

for the three unknown distances . Using classical Sylvester resultants to eliminate between and to get a polynomial , then further eliminating between and gives an 8th degree polynomial in with only even terms, i.e. a 4th degree polynomial in :

This has at most four solutions for x and can be solved in closed form. As is positive, . Then and are uniquely determined from .

The coefficients of the 4th degree polynomial is provided in the appendix.

 

Resolving the ambiguity

To obtain a unique solution, we need to add a 4th point. One straightforward approach is to take subsets of 3 of the 4 points, solve the 4th degree polynomial equation for each subset and finally find the common solution. This is indeed the most frequent practice both in photogrammetry and computer vision. However there are several drawbacks. The first is that we have to solve several 4th degree polynomials. Secondly, we need to find the common solution which might be difficult due to noisy data. Finally, and probably the most important, we can not profit from the redundancy of the data which should increase stability.

 

Absolute orientation

The image coordinates and recovered depths give complete estimates of the 3D coordinates of the reference points in camera-centered coordinates. There remains the determination of a similarity transformation between two sets of 3D points . The best rotation in the least squares sense can be found in closed-form using quaternions [4,2]. The determination of the translation and scale immediately follow from the rotation.

 

The 4th degree polynomial

where

and