Comparison of PnP algorithms for 2D-3D matching problems: DLT, P3P, EPnP

1. Problem definition

First of all, you need to know what is PnP (Perspective-n-Point)? What problem is it to solve?
Known information:

• The coordinates of n 3D points in the A coordinate system (which can be considered as the world coordinate system) { p 1 , p 2 , . . . , pn } \{p_1, p_2, ..., p_n\}{ p1​,p2​,...,pn​} , and the coordinates of these 2D points projected on the image in the image coordinate system{ u 1 , u 2 , . . . , un } \{u_1, u_2, ..., u_n\}{ u1​,u2​,...,un​}

• The matching relationship between the n 3D reference points and the 2D projection points on the image (3D positions are usually determined by triangulation or RGBD depth maps, for binocular or RGBD odometers, PnP can be directly used to estimate camera motion, and monocular Visual odometry needs to be initialized first)

• Camera internal reference K, distortion coefficient

variable to be requested:

• A coordinate system (it can be the world coordinate system, or another camera coordinate system) to the pose transformation of the camera coordinate system , that is, rotation and translation

The illustration is from the official website of opencv . In the camera coordinate system here, the x-axis points up, the y-axis points down, and the z-axis points forward.

Why use 3D-2D method for pose estimation?
This method does not require epipolar constraints, and can obtain better motion estimation in few matching points.

Optional methods and prerequisites:
There are many solutions to this type of problem, such as P3P, direct linear transformation (DLT), EPnP, UPnP, etc., and non-linear optimization can also be used to construct least squares problems for iterative solutions, that is, beam Adjustment method (BA).

options	DLT	P3P	EPnP	Nonlinear Optimization (BA)
求解思路	Construct an augmented matrix (R|t), and construct a 12-dimensional equation system through the projection matrix	Simultaneous equations via similar triangles and the law of cosines via three point pairs	Transform by control point, the complexity is O(n)	Both the camera pose and the spatial point position are regarded as optimization variables to minimize the reprojection error
概况	At least 6 point pairs are required (12 unknowns, each point pair constructs two equations), and when there are more than 6, you can use SVD, etc. to find the least squares solution to the overdetermined equation	4 point pairs are required, 3 are used to solve, one point pair is used to verify, and the only solution is returned	It is better for noisy feature points; it is a closed-form solution that does not require iterations and initial estimates
不足	Ignoring the connection between T matrices, the calculated R does not necessarily satisfy SO(3), and it is necessary to find a rotation matrix to approximate it	It is difficult to use more information on matching point pairs, and it will fail when encountering noise or mismatching. 4 point pairs are required, and if these points are coplanar		may fall into a local minimum

2. Direct Linear Transformation DLT

The homogeneous coordinates of a 3D point in the world coordinate system are known, the homogeneous coordinates of the corresponding 2D projection point, and the internal parameters of the camera (if you don’t know, you can use PnP to estimate K, R, t, and the result is slightly worse if there are more unknowns. point), then you can write the projection from 3D point to 2D point:
$$l\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}R|t\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$
In this method, the orthogonal constraint of the rotation matrix itself is ignored, R and t are equivalent to 12 unknowns, $(R|t)$ is a 3*4 matrix.
At this time, you can do it yourself, expand this formula completely, and then eliminate$\lambda \; \lambda$ , in the following case:
$$\left(\begin{array}{cccccccccccc}X{f}_x& Y{f}_x& Z{f}_x& f_x& 0& 0& 0& 0& X{c}_x-ux& Y{c}_x-uY& Z{c}_x-andZ& c_x-u\\ 0& 0& 0& 0& X{f}_y& Y{f}_y& Z{f}_y& f_y& X{c}_y-vx& Y{c}_y-v& Z{c}_y-vZ& c_y-v\end{array}\right)\left(\begin{array}{c}{a}_1\\ a_2\\ .\\ .\\ .\\ a_{12}\end{array}\right)=0$$
can see that a point pair can provide 2 equations, then 12 unknowns need at least 6 point pairs.

When there are more than 6 point pairs, there will be overdetermined, then use SVD to solve: read related

There is a problem here. When the unknowns are assumed, there is no correlation between the 12 unknowns, and the obtained rotation matrix may not meet the requirements of positive definiteness. So at this time, we need to find the best rotation matrix to approximate it. Here we can use QR decomposition, which is equivalent to reprojecting the result from the matrix space onto the SE(3) manifold, converting it into two parts of rotation and translation.
$$R\leftarrow (R{R}^T{)}^{-\frac{1}{2}}R$$

References:

3. P3P

Known information:

• The coordinates of the three points ABC in the world coordinate system can be used to find the lengths of AB, BC, and AC

• $\angle BOA\left(a\right),\angle AOC\left(b\right),\angle AOB\left(\gamma \right)$ : These three angles are known quantities. I personally understand it this way. First of all, you can’t directly use OA, OB, OC plus the law of cosines to find it, because you don’t know the coordinates of the optical center O in the world$.$ position under the system. In the case where the world coordinates of the 3D point and the 2D point are known to match, the above picture can be drawn. It can be found that the 2D point and the point in the corresponding camera coordinate system can also form the same angle. In this way, the internal reference and pixel coordinates are known. You can find the included angle.

  At this point, the pixel coordinates can be converted into normalized camera coordinate system coordinates first , such as $(\frac{x}{z},\frac{y}{z},1)=(\frac{u}{f_x}-\frac{c_x}{f_x},\frac{v}{f_y}-\frac{c_y}{f_y},1)$ , then we can find the modulus length from the optical center O to this normalized coordinate point, and then classify the vector length as a unit length, that is, let${}^k{X}_1^s{,}^k{X}_2^s{,}^k{X}_3^s$Both are unit vectors pointing from the optical center to the camera coordinate point . With the unit vector, you can directly do the inner product to find the cosine value, and you can get three angles.

The ultimate goal:
through the law of cosines, we can get the lengths of OA, OB, and OC, and we can also get the coordinates of the three points ABC in the camera coordinate system . Afterwards, it can be transformed into a 3D (camera system)-3D (world system) point-to-point problem to calculate the pose R,t of the camera in the world coordinate system. Proceed as follows:

From the above known conditions (green), what we want to calculate is s1, s2, s3 in the figure.
Starting from the first formula, define $u=\frac{s_2}{s_1},v=\frac{s_3}{s_1}$并代入：
$a^2=s_1^2(u^2+v^2-2uv\cos \alpha )$ , so we can get$s_1^2=\frac{a^2}{u^2+v^2-2uv\cos a}$.Similarly
, substituting u and v into the second and third formulas can get $s_1^2$After substituting the other two representations of , a quartic polynomial is obtained:

find v to get $s_1,s_2,s_3$, there is a problem here, that is, there will be 4 sets of solutions, so additional measures are needed to eliminate this ambiguity:

• Use known approximate solutions from GPS
• Verification using the fourth point pair yields a unique solution

Give an example of four sets of solutions: the three included angles are all equal, and the distances between the 3D points are also equal.

At this time, the coordinates of the three points ABC in the camera and the world coordinate system are known, and the 3D-3D can be solved based on this ICP problem, so as to solve the pose of the camera.

4. EPnP

Corresponding paper "EPnP: Accurate Non-Iterative O(n) Solution to the PnP Problem"

Convert the 2D image point to the 3D point of the camera coordinate system through the internal reference, and then use ICP to solve the 3D-3D transformation to obtain the pose. The core is: solve the coordinates of the 3D reference point in the camera coordinate system

The solution is: use 4 control points as the reference datum, so that all 3D points in the world coordinate system can be expressed as a linear combination of these 4 reference points, and the weight value is 1 . In this way, the 4 control points in the original world coordinate system (or camera coordinate system) can be used to represent all 3D points in the world coordinate system (or camera coordinate system). The core becomes: solve the coordinates of the 4 control points in the camera coordinate system .

• Selection of control points
  The original control points can be randomly selected. In order to have a stable result, the paper recommends the following fixed selection method: one of the control points is the center of mass of all points in the world coordinate system, and then the center of gravity is subtracted from the coordinates of all 3D points Coordinates, make $A^{In\; the\; form\; of\; T}A$, use SVD to find the three main directions, and add them to the coordinates of the center of gravity to get the other three control points.
  The coordinates of the 4 control points inthe world coordinate systemare$c_j^w,j=1,...,4.$ These four points can be obtained directly from 3D points. Let the coordinates of these 4 control points inthe camera coordinate systembe$c_j^c,j=1,...,4.$ Note that for different 3D points, the coordinate weight value corresponding to each point is different.

• The weight coefficient relationship between the camera coordinate system corresponding to the same 3D point and the 4 control points in the world coordinate system
  Weight $a_{ij}$是相同的，证明过程如下：
  $${\mathbf{p}}_i^c=R_{cw}{P}_i^w+t=R_{cw}\sum _{j=1}^4{a}_{ij}{\mathbf{c}}_j^w+\sum _{j=1}^4{a}_{ij}t=\sum _{j=1}^4{a}_{ij}(R_{cw}{c}_i^w+t)=\sum _{j=1}^4{a}_{ij}{c}_j^c$$

In order to find the coordinates of the control points in the camera coordinate system, the following equation can be written according to the camera model:
$$\forall i,w_i\left(\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right)=K{\mathbf{p}}_i^c=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\sum _{j=1}^4{a}_{ij}\left(\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right)$$

KK here$K$ is the internal reference matrix, and the camera system coordinates of each control point are$c_j^c=[x_j^c,y_j^c,z_j^c{]}^T,j=1,..4$ .$\_$This$w_i$It is the depth, and $w_i={\sum }_{j=1}^4{a}_{ij}{z}_j^c$. Subtract the last line from the first two lines to get:
$$\begin{array}{l}{\sum }_{j=1}^4{a}_{ij}{f}_x{x}_j^c+a_{ij}(c_x-u_i)z_j^c=0\\ {\sum }_{j=1}^4{a}_{ij}{f}_y{y}_j^c+a_{ij}(c_y-v_i)z_j^c=0\end{array}$$

In this equation system, there are camera coordinates of four control points, that is, there are 12 unknowns, and the unknown $x$ becomes a column vector and is written in the following matrix form. $The\; dimension\; of\; M$ is$2n\ast 12$，$n$ is the number of pairs.
$$M\mathbf{x}=0$$
For the specific solution, please refer to the second reference link of epnp below.

At this time, the weight coefficient is known, the camera system coordinates of the control points are also known, and the position of the 3D point in the camera coordinate system can be recovered. The world coordinates of the 3D point are known from the beginning, so the pose can be solved using ICP up.

Why is the complexity of the algorithm $O\left(n\right)$ ?
Reflected in solving$Mx=0$ , find$For\; x$ , the dimension must first be$12\ast 12$ MTMM$M^{T}M$, the complexity here is$O\left(n\right)$。

---

Reference:
DLT: https://zhuanlan.zhihu.com/p/58648937
P3P: https://zhuanlan.zhihu.com/p/140077137
Bonn University P3P courseware: https://www.ipb.uni-bonn.de /html/teaching/msr2-2020/sse2-15-p3p.pdf
EpnP: https://blog.csdn.net/zkk9527/article/details/107939991
https://zhuanlan.zhihu.com/p/59070440