# A Point Cloud Registration Algorithm Based on Weighting Strategy for 3D Indoor Spaces
## 3 The ICP Algorithm with Weighting Strategy
### Derivation of Solution
\begin{bmatrix}1&\alpha_{k} \beta_{k}-\gamma_{k}&\alpha_{k}\gamma_{k}+\beta_{k}\\ \gamma_{k}&\alpha_{k}\beta_{k}\gamma_{k}+1&\beta_{k}\gamma_{k}-\alpha_{k}\\ -\beta_{k}&\alpha_{k}&1\end{bmatrix} \approx \begin{bmatrix}1&-\gamma_{k}&\beta _{k}\\ \gamma_{k}&1&-\alpha_{k}\\ -\beta_{k}&\alpha_{k}&1\end{bmatrix}  \tag{24}\]  
A six-dimensional vector is defined  
\[\mathbf{x}=[t_{x},t_{y},t_{z},\alpha,\beta,\gamma] \tag{25}\]  
as the solution of the objective function. Given the \(i\)th point in the source point cloud  
\[\mathbf{p}_{i}=[p_{i0},p_{i1},p_{i2}] \tag{26}\]  
an auxiliary matrix is defined  
\[\mathbf{A}_{i}= \begin{bmatrix}1&0&0&0&p_{i2}&-p_{i1}\\ 0&1&0&-p_{i2}&0&p_{i0}\\ 0&0&1&p_{i1}&-p_{i0}&0\end{bmatrix}  \tag{27}\]  
Then, Equation (21) can be written as  
\[\underset{\text{min}}{F}(\mathbf{x})=\sum_{i}\left(\mathbf{A}_{i}\mathbf{x}-\mathbf{p}_{i1} \right)^{T}\omega_{i}\mathbf{n}_{i}\mathbf{n}_{i}^{T}(\mathbf{A}_{i}\mathbf{x}-\mathbf{p}_{i1}) \tag{28}\]  
by defining  
\[\mathbf{B}_{i}=\omega_{i}\mathbf{n}_{i}\mathbf{n}_{i}^{T} \tag{29}\]  
Ignoring constant terms, Equation (29) can be expanded as  
\[\underset{\text{min}}{F}(\mathbf{x})=\mathbf{x}^{T}(\sum_{i}\mathbf{A}_{i}^{T}\mathbf{B}_{i} \mathbf{A}_{i})\mathbf{x}+(\sum_{i}-2p_{i}^{T}\mathbf{B}_{i}\mathbf{A}_{i})\mathbf{x} \tag{30}\]  
by defining  
\[\mathbf{G}=\sum_{i}\mathbf{A}_{i}^{T}\mathbf{B}_{i}\mathbf{A}_{i} \tag{31}\]  
\[\mathbf{H}=\sum_{i}-2p_{i}^{T}\mathbf{B}_{i}\mathbf{A}_{i} \tag{32}\]  
\[\mathbf{J}= \begin{bmatrix}0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&I_{3\times 3}\end{bmatrix}  \tag{33}\]  
Finally, the objective function can be written as  
\[\underset{\text{min}}{F}(\mathbf{x})=\mathbf{x}^{T}\mathbf{G}\mathbf{x}+\mathbf{H}\mathbf{x},\ s.t.\bm {x}^{T}\mathbf{J}\mathbf{x}=0 \tag{34}\]  
where the constraint is the approximate matrix form of \(\alpha^{2}+\beta^{2}+\gamma^{2}\approx 0\). To solve the nonlinear minimization problem, Lagrange's multipliers are used for derivation. The Lagrange function is defined  
\[L(x)=\mathbf{x}^{T}\mathbf{G}\mathbf{x}+\mathbf{H}\mathbf{x}+\lambda\mathbf{x}^{T}\mathbf{J}\mathbf{x} \tag{35}\]The necessary condition for optimization is derived as  
\[\frac{\partial L}{\partial\mathbf{x}}=2\mathbf{x}^{T}\mathbf{G}+\mathbf{H}+2\lambda\mathbf{x}^{T}\mathbf{ J}=\mathbf{0}^{T} \tag{36}\]  
which is equivalent to  
\[\mathbf{x}=-{(2\mathbf{G}+2\lambda\mathbf{J})}^{-T}\mathbf{H}^{T} \tag{37}\]  
Equation (37) is substituted into the constraint of Equation (34) to obtain  
\[\mathbf{H}(2\mathbf{G}+2\lambda\mathbf{J})^{-1}\mathbf{J}(2\mathbf{G}+2\lambda\mathbf{J})^{-T}\mathbf{H}^{ T}=0 \tag{38}\]  
where \(\lambda\) is the only unknown parameter. According to [38], we can find the solutions in closed form for Equation (38), a sixth-order polynomial. Finally, Equation (37) yields \(\mathbf{x}\). In conclusion, we can summarize the weighted PP-ICP process in this paper as follows: Firstly, PointNet semantically segments the input point cloud data, generating labels for different types of items along with corresponding parameters: importance, IoU, and CPA. Next, we assign weights to these category labels using an objective weighting method, which employs an improved CRITIC as the weighting strategy to calculate the corresponding weight values. Finally, we use the calculated weight values as the weight parameters for the point-to-plane ICP, registering the input point cloud effectively. Figure 2 illustrates the algorithm's process of weighted PP-ICP.