# Research on Visual Positioning of a Roadheader and Construction of an Environment Map
## 3 Pose Estimation and Optimization of the Roadheader
### Pose Solution  
Based on image feature extraction and matching, an initial motion transform with a high confidence is iteratively solved using the RANSAC method, which aims to provide reliable initial values for pose estimation based on ICP. The process is as follows: (1) According to the matched 3D point set, the initial model is generated by randomly sampling some point pairs; (2) the model is evaluated according to the given threshold by calculating the point-pair error; and (3) through calculating the number of interior point pairs and the average value of the error, the model is updated and iterated based on the given threshold. After several iterations, a relatively optimal motion transformation can be obtained [26; 27]. After depth mapping and RANSAC iteration, a set of 3D point sets with exact matching is obtained, i.e., \(P=\{p_{1},\,p_{2},\,\ldots,\,p_{n}\}\), \(Q=\{q_{1},\,q_{2},\,\ldots,\,q_{n}\}\), the body pose transformation between the \(i\)-th pair of 3D matching points in the point sets can be written as:  
\[p_{i}=Rq_{j}+\mathbf{t} \tag{8}\]  
Figure 5: Effect of ORB feature extraction and matching. According to the SLAM mathematical model of a roadheader (Equation (7)), there are noises in the motion equation and observation equation of the airborne camera; thus, the state estimation is uncertain. The inter-frame error function, \(e_{\text{(3D-3D)}i}\), is positioned as [28]:  
\[e_{\text{(3D-3D)}i}=p_{i}-(\mathbf{R}q_{i}+\mathbf{t}) \tag{9}\]  
For \(n\) pairs of 3D matching points in the point set, the least squares expression of the objective function, \(F(\mathbf{R},\mathbf{t})\), can be expressed as [29; 30]:  
\[F(\mathbf{R},\mathbf{t})=\text{argmin}_{\mathbf{R},\mathbf{t}}\frac{1}{2}\sum\limits_{i=1}^{n} \left\|p_{i}-(\mathbf{R}q_{i}+\mathbf{t})\right\|^{2} \tag{10}\]  
To solve Equation (10), let the de-centroid coordinates of point sets \(P\) and \(Q\) be \(p_{i}^{\prime}\) and \(q_{i}^{\prime}\), respectively, that is:  
\[\left\{ \begin{array}{l}p_{i}^{\prime}=p_{i}-\frac{1}{\mathrm{n}}\sum\limits _{i=1}^{n}p_{i}\\ q_{i}^{\prime}=q_{i}-\frac{1}{\mathrm{n}}\sum\limits_{i=1}^{n}q_{i}\end{array} \right. \tag{11}\]  
Combining Equations (10) and (11), the de-centroid form can be obtained as:  
\[F(\mathbf{R},\mathbf{t})=\text{argmin}_{\mathbf{R},\mathbf{t}}\frac{1}{2}\sum\limits_{i=1}^{n} \left(\left\|p_{i}^{\prime}-\mathbf{R}q_{i}^{\prime}\right\|^{2}+\left\|\frac{1}{ \mathrm{n}}\sum\limits_{i=1}^{n}p_{i}-\left(\mathbf{R}\cdot\frac{1}{\mathrm{n}} \sum\limits_{i=1}^{n}q_{i}+\mathbf{t}\right)\right\|^{2}\right) \tag{12}\]  
Equation (12) can be decomposed into two functions, \(f_{1}(\mathbf{R})\) and \(f_{2}(\mathbf{t})\), namely:  
\[f_{1}(\mathbf{R})=\text{argmin}_{\mathbf{R}}\frac{1}{2}\sum\limits_{i=1}^{n}\left\|p_ {i}^{\prime}-\mathbf{R}q_{i}^{\prime}\right\|^{2} \tag{13}\]  
\[f_{2}(\mathbf{t})=\text{argmin}_{\mathbf{t}}\frac{1}{2}\sum\limits_{i=1}^{n}\left\| \frac{1}{\mathrm{n}}\sum\limits_{i=1}^{n}p_{i}-\mathbf{R}\cdot\frac{1}{\mathrm{n }}\sum\limits_{i=1}^{n}q_{i}-\mathbf{t}\right\|^{2} \tag{14}\]  
Further, Equation (13) can be expanded and simplified as:  
\[g_{1}(\mathbf{R})=\text{argmin}_{\mathbf{R}}\left[-\text{tr}\left(\mathbf{R}\sum\limits_ {i=1}^{n}q_{i}^{\prime}\big{(}p_{i}^{\prime}\big{)}^{\mathrm{T}}\right)\right] \tag{15}\]  
where, \(\text{tr}()\) is the trace operator of matrix. To understand the \(\mathbf{R}\) contained in Equation (15), a method based on singular value decomposition is adopted, that is, let [31; 32]:  
\[\sum\limits_{i=1}^{n}q_{i}^{\prime}\big{(}p_{i}^{\prime}\big{)}^{\mathrm{T}}= \mathbf{U}\mathbf{D}\mathbf{V}^{\mathrm{T}} \tag{16}\]  
where \(\mathbf{D}\), \(\mathbf{U}\), and \(\mathbf{V}\) are all diagonal matrices, and matrix \(\mathbf{D}\) is composed of singular values.