# An Accurate, Efficient, and Stable Perspective-n-Point Algorithm in 3D Space
## 2 Methods  
### Description of the PnP Mathematical Model  
The imaging model of a camera projecting n 3D reference points under a 3D world onto a 2D image plane through a lens can be represented by the model in Figure 2. The equations of the projected imaging model from the world coordinate system to camera one can be expressed in the following form:  
\[\lambda_{i}P_{i}=Rq_{i}+t,(i=1,2,3,\ldots,n) \tag{1}\]  
where \(q_{i}=[X_{i},Y_{i},Z_{i}]^{\mathrm{T}}\), (\(i=1,2,3,\ldots,n\)) are the coordinates of \(n\) 3D spatial reference points in the world coordinate system, \(P_{i}=[u_{i},v_{i},1]^{\mathrm{T}}\), (\(i=1,2,3,\ldots,n\)) are the plane coordinates of \(n\) points in the camera coordinate system, and \(t=[t_{x},t_{y},t_{z}]^{\mathrm{T}}\) is the translation vector. Parametric Representation of Rotation Matrices and Translation Vectors, and the Elimination of Depth Factors  
Mathematically, the rotation matrix \(R\) is represented in more than one way, including direction cosine algorithm, Euler angle representation, unit quaternion representation, Cayley parameter representation (CGR), etc. This paper mainly employed the Cayley parameter representation (CGR), which is directly derived from the quaternion method. For example, the rotation matrix R, represented by the quaternion, is:  
\[R=\frac{1}{s}\!\left[ \begin{array}{ccc}1+b^{2}-c^{2}-d^{2}&2 bc-2d&2 bd+2c\\ 2 bc+2d&1-b^{2}+c^{2}-d^{2}&2 cd-2b\\ 2 bd-2c&2 cd+2b&1-b^{2}-c^{2}+d^{2}\end{array} \right]=\frac{1}{s}\!U \tag{2}\]  
Among them, (\(s=1+b^{2}+c^{2}+d^{2}\)). The rotation angle closing to 180\({}^{\circ}\) leads to an extremely large rotation matrix, represented by CGR, leading to a singular value in the \(R\) matrix and a large calculation error. In response, a rotation matrix \(R_{1}\) is constructed in the process of code implementation, \(R_{1}\) is adopted to process the rotation matrix \(R\), parameterized by CGR, and \(R\) is restored to its original state after obtaining the final result. In this way, the singularity caused by \(R\) parameterized by CGR is effectively avoided. Equation (1) is rewritten into matrix form:  
\[\lambda_{i}\!\left[ \begin{array}{c}u_{i}\\ v_{i}\\ 1\end{array} \right]=Rq_{i}+\left[ \begin{array}{c}t_{x}\\ t_{y}\\ t_{z}\end{array} \right],(i=1,2,3,\ldots,n) \tag{3}\]  
Meanwhile, the U matrix in Equation (2) is set to be:  
\[U=\left[ \begin{array}{c}r_{1}^{\mathrm{T}}\\ r_{2}^{\mathrm{T}}\\ r_{3}^{\mathrm{T}}\end{array} \right]=\left[ \begin{array}{ccc}1+b^{2}-c^{2}- d^{2}&2 bc-2d&2 bd+2c\\ 2 bc+2d&1-b^{2}+c^{2}-d^{2}&2 cd-2b\\ 2 bd-2c&2 cd+2b&1-b^{2}-c^{2}+d^{2}\end{array} \right]\]  
Multiply both sides of Equation (3) by s to obtain:  
\[\hat{\lambda}_{i}P_{i}=\left[ \begin{array}{c}r_{1}^{\mathrm{T}}\\ r_{2}^{\mathrm{T}}\\ r_{3}^{\mathrm{T}}\end{array} \right]q_{i}+\hat{t},(i=1,2,3,\ldots,n) \tag{4}\]  
where \(\hat{\lambda}_{i}=\lambda_{i}s\), \(P_{i}=[u_{i},v_{i},1]^{\mathrm{T}}\), \(\hat{t}_{i}=s[t_{x},t_{y},t_{z}]^{\mathrm{T}}=[\hat{t}_{1},\hat{t}_{2},\hat{t }_{3}]^{\mathrm{T}}\). Figure 2: Coordinate system diagram of camera imaging model. According to Equation (4), the depth factor satisfies:  
\[\hat{\lambda}_{i}=r_{3}^{T}q_{i}+f_{3},(i=1,2,3,\ldots,n) \tag{5}\]  
Therefore, Equation (5) is substituted into Equation (4) to eliminate all depth factors:  
\[\hat{t}_{1}-\hat{t}_{3}u_{i}=r_{3}^{T}q_{i}u_{i}-r_{1}^{T}q_{i} \tag{6}\]  
\[\hat{t}_{2}-\hat{t}_{3}v_{i}=r_{3}^{T}q_{i}v_{i}-r_{2}^{T}q_{i} \tag{7}\]  
The new variable \(L=[1,b,c,d,b^{2},bc,bd,c^{2},cd,d^{2}]^{T}\)is introduced, and Equations (6) and (7) are rewritten into matrix forms:  
\[A_{i}\hat{t}=\hat{N}_{i}L \tag{8}\]  
\[\text{where }A_{i}=\left[ \begin{array}{ccc}1&0&-u_{i}\\ 0&1&-v_{i}\end{array} \right],\]  
\[\hat{N}_{i}=\left[