# An Accurate, Efficient, and Stable Perspective-n-Point Algorithm in 3D Space
## 2 Methods
### The Application of Hidden Variable Method to Solve Pose Parameters  
Due to the numerous polynomial manipulations, the simplification, and the deformation involved in this section, the application of a general calculation makes the derivation process rather complicated. Therefore, the extensively used symbolic operation in matlab software is adopted in this paper to simplify the mathematical description process of hidden variables, so that readers can easily and intuitively understand the derivation process. Among them, \(J(\cdot i)\), \(i=1\), \(2\), \(3\),..., \(10\) denotes all elements of the ith column of the \(J\) matrix. In the hidden variables, \(L=[1,b,c,d,b^{2},bc,bd,c^{2},cd,d^{2}]^{T}\), the new variables introduced in the previous section, are adopted, where b is set as a constant, and the remaining parameters are taken as unknowns. Then, the unknowns in Equation (12) are introduced as new variables to obtain the following equation in the form of a matrix:  
\[Q\left[ \begin{array}{c}c^{2}\\ d^{2}\\ cd\end{array} \right]=-T(b)\left[ \begin{array}{c}c\\ d\\ 1\end{array} \right] \tag{13}\]  
where \(Q=\) [\(J\)(:8), \(J\)(:10), \(J\)(:9)],  
\[T(b)=\left[ \begin{array}{ccccc}j_{13}+j_{16}b&j_{14}+j_{12}b&j_{11}+j_{12}b+ j_{15}b^{2}\\ j_{23}+j_{26}b&j_{24}+j_{27}b&j_{21}+j_{22}b+j_{25}b^{2}\\ j_{33}+j_{36}b&j_{34}+j_{37}b&j_{31}+j_{32}b+j_{35}b^{2}\\ \vdots&\vdots&\vdots\\ j_{2n,3}+j_{2n,6}b&j_{2n,4}+j_{2n,7}b&j_{2n,1}+j_{2n,2}b+j_{2n,5}b^{2}\end{array} \right]\]  
According to Equation (13), \(c^{2}\), \(d^{2}\), and \(cd\) is solved through the following mathematical calculation, exhibiting the following closed-form solution:  
\[\left[ \begin{array}{c}c^{2}\\ d^{2}\\ cd\end{array} \right]=GT(b)\left[ \begin{array}{c}c\\ d\\ 1\end{array} \right]\Leftrightarrow\left[ \begin{array}{c}c^{2}\\ d^{2}\\ cd\end{array} \right]=W(b)\left[ \begin{array}{c}c\\ d\\ 1\end{array} \right] \tag{14}\]  
where \(G=-(Q^{T}Q)^{-1}Q^{T}\). The calculation results show that G is a \(3\times 2n\) pure numerical matrix, and \(W(b)\) is a \(3\times 3\) polynomial matrix containing \(b\). At the same time,  
\[G=\left[\begin{array}{ccccc}g_{11}&g_{12}&g_{13}&\cdots&g_{1,2n}\\ g_{21}&g_{22}&g_{23}&\cdots&g_{2,2n}\\ g_{31}&g_{32}&g_{33}&\cdots&g_{\[ \begin{array}{l}\mbox{\small$W$}(b)=\left[\begin{array}{cc}w_{12}+w_{11}b&w_{14 }+w_{13}b&w_{17}+w_{16}b+w_{15}b^{2}\\ w_{22}+w_{21}b&w_{24}+w_{23}b&w_{27}+w_{26}b+w_{25}b^{2}\\ w_{32}+w_{31}b&w_{34}+w_{33}b&w_{37}+w_{36}b+w_{35}b^{2}\end{array}\right]= \left[\begin{array}{cc}W_{11}&W_{12}&W_{13}\\ W_{21}&W_{22}&W_{23}\\ W_{31}&W_{32}&W_{33}\end{array}\right]\end{array} \]  
where \(w_{11}=G(1\,:)(\,:\,)(\,:\,)\), \(w_{21}=G(2\,:)(\,:\,)(\,:\,)\), \(w_{31}=G(3\,:)(\,:\,)\), \(w_{12}=G(1\,:)(\,:\,)\)(\(\,:\,3\)), \(w_{22}=G(2\,:)(\,:\,)\)(\(\,:\,3\)), \(w_{13}=G(1\,:)(\,:\,)\)(\(\,:\,7\)), \(w_{23}=G(2\,:)(\,:\,)\)(\(\,:\,7\)), \(w_{33}=G(3\,:)(\,:\,)\)(\(\,:\,7\)), \(w_{14}=G(1\,:)(\,:\,)\)(\(\,:\,4\)), \(w_{24}=G(2\,:)(\,:\,)\)(\(\,:\,4\)), \(w_{34}=G(3\,:)\)(\(\,:\,4\)), \(w_{15}=G(1\,:)(\,:\,)\)(\(\,:\,5\)), \(w_{25}=G(2\,:)(\,:\,)\)(\(\,:\,5\)), \(w_{35}=G(3\,:)(\,:\,)\)(\(\,:\,5\)), \(w_{16}=G(1\,:)(\,:\,)\)(\(\,:\,2\)), \(w_{26}=G(2\,:)(\,:\,)\)(\(\,:\,2\)), \(w_{36}=G(3\,:)(\,:\,)\)(\(\,:\,2\)), \(w_{17}=G(1\,:)(\,:\,)\)(\(\,:\,1\)), \(w_{27}=G(2\,:)(\,:\,)\)(\(\,:\,1\)), \(w_{37}=G(3\,:)(\,:\,)\)(\(\,:\,1\)). Therefore, Equation (14) can be written as:  
\[ \begin{cases}c^{2}=cW_{11}+dW_{12}+W_{13}\\ d^{2}=cW_{21}+dW_{22}+W_{23}\\ cd=cW_{31}+dW_{32}+W_{33}\end{cases}  \tag{15}\]  
By observing the relationship between the unknowns \(c,d,c^{2},d^{2}\), and cd in Equation (14), the following three identity constraint relations are established:  
\[ \begin{cases}c^{2}d=(cd)c\\ (cd)d=d^{2}c\\ (cd)(cd)=(c^{2})(d^{2})\end{cases}  \tag{16}\]  
Equation (15) is substituted into Equation (16) to obtain:  
\[