# A Point Cloud Registration Algorithm Based on Weighting Strategy for 3D Indoor Spaces
## 3 The ICP Algorithm with Weighting Strategy
### Derivation of Solution  
The rigid transformation process of a point clouds is defined as:  
\[\mathbf{q}_{i}=\mathbf{R}(\alpha_{k},\beta_{k},\gamma_{k})\mathbf{p}_{i}+\mathbf{t}_{k} \tag{18}\]  
where \(\mathbf{p}_{i}\) is the \(i\)th point in the source point cloud and \(\mathbf{q}_{i}\) is the corresponding point after rigid transformation. \(\mathbf{R}(\alpha_{k},\beta_{k},\gamma_{k})\) is the rotation matrix with rotation angle \(\alpha_{k}\), \(\beta_{k}\), and \(\gamma_{k}\), and \(\mathbf{t}_{k}\) is the corresponding translation value. \(k\) refers to the iterations of the ICP algorithm. Based on Equation (18), the objective function can be written as:  
\[ \begin{array}{l}\underset{\text{min}}{F}(\alpha_{k+1},\beta_{k+1},\gamma_{k +1},\mathbf{t}_{k+1})=\sum_{i=1}\big{(}\mathbf{n}_{i}^{T}[\mathbf{R}(\alpha_{k+1},\beta_{ k+1},\gamma_{k+1})\mathbf{p}_{i}+\mathbf{t}_{k+1}-\mathbf{p}_{i}^{\prime}]\big{)}^{2}\end{array}  \tag{19}\]  
Considering that \(\mathbf{n}_{i}\), \(\mathbf{p}_{i}\), \(\mathbf{t}_{k+1}\), \(\mathbf{p}_{i}^{\prime}\in R^{3}\), and that \(\mathbf{R}(\alpha,\beta,\gamma)\) is a 3 \(\times\) 3 matrix, the objective function can be denoted as:  
\[ \begin{array}{rl}\underset{\text{min}}{F}(\alpha_{k+1},\beta_{k+1},\gamma_{k +1},\mathbf{t}_{k+1})&=\sum_{i=1}\big{(}\mathbf{n}_{i}^{T}[\mathbf{R}(\alpha_{k+1},\beta_ {k+1},\gamma_{k+1})\mathbf{p}_{i}+\mathbf{t}_{k+1}-\mathbf{p}_{i}^{\prime}]\big{)}^{T}(\bm {n}_{i}^{T}[\mathbf{R}(\alpha_{k+1},\beta_{k+1},\gamma_{k+1})\mathbf{p}_{i}+\mathbf{t}_{k+1 }-\mathbf{p}_{i}^{\prime}]\big{)})\\ &=\sum_{i=1}\big{[}\mathbf{R}(\alpha_{k+1},\beta_{k+1},\gamma_{k+1})\mathbf{p}_{i}+\bm {t}_{k+1}-\mathbf{p}_{i}^{\prime}]^{T}\mathbf{n}_{i}\mathbf{n}_{i}^{T}[\mathbf{R}(\alpha_{k+1 },\beta_{k+1},\gamma_{k+1})\mathbf{p}_{i}+\mathbf{t}_{k+1}-\mathbf{p}_{i}^{\prime}]\end{array}  \tag{20}\]  
Then, the weight coefficient is introduced into Equation (18):  
\[\underset{\text{min}}{F}(\alpha_{k+1},\beta_{k+1},\gamma_{k+1},\mathbf{t}_{k+1}) =\sum_{i=1}\big{[}\mathbf{R}(\alpha_{k+1},\beta_{k+1},\gamma_{k+1})\mathbf{p}_{i}+\bm {t}_{k+1}-\mathbf{p}_{i}^{\prime}]^{T}\omega_{i}\mathbf{n}_{i}\mathbf{n}_{i}^{T}\big{[} \mathbf{R}(\alpha_{k+1},\beta_{k+1},\gamma_{k+1})\mathbf{p}_{i}+\mathbf{t}_{k+1}-\mathbf{p}_{i} ^{\prime}\big{]} \tag{21}\]  
where \(\omega_{i}\) is the weight coefficient of the \(i\)th point whose value is calculated according to the weighting strategy in Section 3. The traditional method for solving this nonlinear objective function is to linearize it, then use the least squares method to obtain a linearly approximate solution. These methods often have trouble obtaining exact closed-form solutions because of the nonlinearity introduced by the reference plane and the rigid transformation operator. This makes convergence times slower when working with large point cloud datasets. This paper proposes an exact closed-form solution to improve computational efficiency and enhance registration accuracy, by reducing the objective function into a matrix form with constraints and using Lagrange's multipliers. In weighted PP-ICP, the rigid transformation is decomposed into many small transformations, each producing a minimal rotation angle along the rotation axis, called \(\theta\), which theoretically should be an infinitesimal value that satisfies:  
\[0<\theta\leq 1^{\circ} \tag{22}\]  
Figure 1: Error metrics: (**a**) point-to-point; (**b**) point-to-plane. For each small transformation, the rotation angles \(\alpha_{k}\), \(\beta_{k}\), and \(\gamma_{k}\) have small values. Therefore, the rotation matrix \(\mathbf{R}(\alpha_{k},\beta_{k},\gamma_{k})\) can be simplified based on the approximation rule proposed in [8]:  
\[\cos\theta\approx 1,\sin\theta\approx 0,\theta^{2}\approx 0\text{ if }\theta\approx 0 \tag{23}\]  
Then, the rotation matrix \(\mathbf{R}(\alpha_{k},\beta_{k},\gamma_{k})\) can be simplified to  
\[\mathbf{R}(\alpha_{k},\beta_{k},\gamma_{k})\approx