# Efficient Model-Based Object Pose Estimation Based on Multi-Template Tracking and PnP Algorithms
## 4 Model-Based 3D Pose Estimation
### Initial Pose Solver  
Suppose that each template patch in the database corresponds to a 3D planar model of the OOI. When working with the template tracking result, the correspondences between 2D image features of the tracked template patch and 3D control points of the CAD model of the object can be determined efficiently. For the \(j\)-th 3D planar model, we define \(N_{c}\) coplanar 3D control points \(\mathbf{C}_{n}^{(j)}\in\Re^{3}\) on this model for \(n=1\)-\(N_{c}\) and \(N_{c}\geq 4\). Let \(\mathbf{C}_{n}^{(j)}(i)\) denote the \(i\)-th element of the \(n\)-th 3D control point, where \(i=1\)-\(3\). If the OOI has been tracked using the \(j\)-th template, then we calculate the mean vector of the corresponding 3D control points of the planar model using Equation (8):  
\[\overline{\mathbf{C}}^{(j)}=N_{c}^{-1}{\sum_{m=1}^{N_{c}}\mathbf{C}_{m}^{(j)}}, \tag{8}\]  
where \(\overline{\mathbf{C}}^{(j)}\) denotes the center location of the \(j\)-th 3D planar model. Next, a three-by-three covariance matrix of the 3D control points is calculated using Equation (9)  
\[\mathbf{M}_{c}^{(j)}={\sum_{m=1}^{N_{c}}\left(\mathbf{C}_{m}^{(j)}- \overline{\mathbf{C}}^{(j)}\right)\left(\mathbf{C}_{m}^{(j)}-\overline{ \mathbf{C}}^{(j)}\right)^{T}}, \tag{9}\]which can be decomposed by Singular-Value-Decomposition (SVD) such that:  
\[\mathbf{M}_{c}^{(j)}=\mathbf{U}_{c}^{(j)}\sum_{c}^{(j)}\left[\mathbf{V}_{c}^{(j)} \right]^{T}, \tag{10}\]  
where \(\mathbf{U}_{c}^{(j)}\) and \(\mathbf{V}_{c}^{(j)}\) are real unitary matrices and \(\sum_{c}^{(j)}\) is a rectangular diagonal matrix with non-negative singular values on the diagonal. According to the SVD result from Equation (10), the control points of the 3D planar model are transformed by a unitary transformation such that:  
\[\mathbf{\hat{C}}_{n}^{(j)}=\left[\mathbf{V}_{c}^{(j)}\right]^{T}(\mathbf{C}_{ n}^{(j)}-\overline{\mathbf{C}}^{(j)}). \tag{11}\]  
As a result, the \(Z\)-value of the transformed 3D control point is fixed to zero, or \(\mathbf{\hat{C}}_{n}^{(j)}(3)=0\). Let \(\mathbf{p}_{n}^{(j)}\) denote 2D feature points corresponding to the projection of the 3D planar control points onto the image plane. Define a mapping function \(\rho:\Re^{3}\rightarrow\Re^{2}\) by \(\rho(x_{1},x_{2},x_{3})=(x_{1}/x_{3},x_{2}/x_{3})\). Then, an optimal homography matrix \(\mathbf{H}_{opt}\) between the transformed 3D planar model and the 2D image plane can be obtained by solving the following nonlinear optimization problem:  
\[\mathbf{H}_{opt}=\arg\underset{\mathbf{H}}{\min}\mathbf{\sum}_{n=1}^{N_{c}} \left\|\mathbf{p}_{n}^{(j)}-\rho(\mathbf{H}\mathbf{\widetilde{C}}_{n}^{(j)}) \right\|^{2}, \tag{12}\]  
where \(\mathbf{\widetilde{C}}_{n}^{(j)}=\left[ \begin{array}{cc}\mathbf{\hat{C}}_{n }^{(j)}(1)&\mathbf{\hat{C}}_{n}^{(j)}(2)&1\end{array} \right]^{T}\) is formed by the result of Equation (11). Suppose that the camera was calibrated previously. By using the camera intrinsic matrix \(\mathbf{K}\) and Equation (12), the pose matrix of the transformed 3D planar model in the unitary coordinate system can be solved by the perspective HD method [18] such that:  
\[\mathbf{H}_{p}=\mathbf{K}^{-1}\mathbf{H}_{opt}=\left[ \begin{array}{cc} \mathbf{h}_{1}&\mathbf{h}_{2}&\mathbf{t}_{3}\end{array} \right], \tag{13}\]  
which can be used to recover the rotation matrix of the transformed 3D planar model such that:  
\[\mathbf{R}_{p}=[ \begin{array}{cc}\|\mathbf{h}_{1}\|^{-1}\mathbf{h}_{1}&\| \mathbf{h}_{2}\|^{-1}\mathbf{h}_{2}&(\|\mathbf{h}_{1}\|\|\mathbf{h}_{2}\|)^{- 1}(\mathbf{h}_{1}\times\mathbf{h}_{2})\end{array} ], \tag{14}\]  
where the symbol \(\|\mathbf{x}\|\) denotes the two-norm value of the vector \(\mathbf{x}\) and the operator \(\times\) represents the cross-product of two vectors.