# HiGoReg: A Hierarchical Grouping Strategy for Point Cloud Registration
## 2 Methodology
### Batch Solution  
Random errors arising from sensor noise and environmental variations affect both the target surface \(\mathbf{f}\) and the search surface \(\mathbf{g}\), represented by \(\mathbf{e_{f}}=\left[e_{X},e_{Y},e_{Z}\right]^{\top}\) and \(\mathbf{e_{g}}=\left[e_{X},e_{y},e_{z}\right]^{\top}\), respectively. With the aid of the transformation parameters, the registration function model for the \(i\)th surface element based on the GH model can be formulated as  
\[\mathbf{\Phi}_{i}=\mathbf{f}_{i}-\mathbf{e_{f_{i}}}-\mathbf{T}-\mathbf{R}\left(\mathbf{g}_{i}-\mathbf{e_{ g_{i}}}\right)=0, \tag{1}\]  
where \(\mathbf{T}\) is the translation parameter vector, \(\mathbf{T}=\left[\Delta x\quad\Delta y\quad\Delta z\right]^{\top}\); \(\mathbf{R}\) is the rotation matrix,  
Figure 1: The pipeline of the proposed methodology. \[\mathbf{R}=\mathbf{R}(\omega)\mathbf{R}(\varphi)\mathbf{R}(\kappa)= \begin{bmatrix}1&0&0\\ 0&\cos\omega&\sin\omega\\ 0&-\sin\omega&\cos\omega\end{bmatrix}\begin{bmatrix}\cos\varphi&0&-\sin\varphi \\ 0&1&0\\ \sin\varphi&0&\cos\varphi\end{bmatrix}\begin{bmatrix}\cos\kappa&\sin\kappa&0\\ -\sin\kappa&\cos\kappa&0\\ 0&0&1\end{bmatrix} .\]  
To perform the TLS estimation, suitable initial values are introduced of the transformation parameter vector (\(\mathbf{\xi}\)) and the random error vectors (\(\mathbf{e}_{f}\), \(\mathbf{e}_{g}\)), indicated by the superscript 0, namely, (\(\mathbf{\xi}^{0},\mathbf{e}_{f}^{0},\mathbf{e}_{g}^{0}\)). Equation (1) is then linearized by Taylor series:  
\[\mathbf{\Phi}_{i}=\mathbf{f}_{i}-\mathbf{e}_{f_{i}}-\mathbf{h}_{i}^{0}-A_{i}\mathrm{d}\mathbf{\xi} +\mathbf{B}_{i}\mathbf{e}_{g_{i}} \tag{2}\]  
where \(\mathbf{h}_{i}^{0}=T^{0}+R^{0}\mathbf{g}_{i}\), \(\mathbf{A}_{i}=\frac{\partial\mathbf{\xi}}{\partial\mathbf{\xi}^{\top}}= \begin{bmatrix} \frac{\partial\mathbf{\Phi}}{\partial\mathbf{T}^{\top}}&\frac{\partial\mathbf{\Phi}}{ \partial\mathbf{R}^{\top}}\end{bmatrix} = \begin{bmatrix}\mathbf{I}_{3}&\mathbf{C}\Big{(}\mathbf{ g}_{i}-\mathbf{e}_{g_{i}}^{0}\Big{)}\end{bmatrix} \), \(\mathbf{B}_{i}=\frac{\partial\mathbf{\Phi}}{\partial\mathbf{e}_{g_{i}}^{0}}=\mathbf{R}^{0}= \mathbf{R}^{0}=\mathbf{R}^{0}=\mathbf{R}^{0}=\mathbf{R}^{0}=\mathbf{R}^{0}=\mathbf{R}^{0}=\mathbf{C}= \begin{bmatrix}\frac{\partial\mathbf{\Phi}}{\partial\mathbf{\omega}}&\frac{\partial \mathbf{\Phi}}{\partial\mathbf{\varphi}}&\frac{\partial\mathbf{\Phi}}{\partial\mathbf{\kappa}} \end{bmatrix} \), and \(\mathrm{d}\mathbf{\xi}= \begin{bmatrix}\mathrm{d}\Delta x&\mathrm{d}\Delta y& \mathrm{d}\Delta z&\mathrm{d}\omega&\mathrm{d}\varphi&\mathrm{d}\mathbf{x}\end{bmatrix} ^ {\top}\). Then, Equation (2) can be rewritten as  
\[\mathbf{l}_{i}-\mathbf{I}_{i}\mathbf{e}_{i}=\mathbf{A}_{i}\mathrm{d}\mathbf{\xi} \tag{3}\]  
where \(\mathbf{l}_{i}=\mathbf{f}_{i}-\mathbf{h}_{i}^{0}\), \(\mathbf{I}_{i}= \begin{bmatrix}\mathbf{I}_{3}&-\mathbf{B}_{i}\end{bmatrix} \), \(\mathbf{e}_{i}= \begin{bmatrix}\mathbf{e}_{f_{i}}^{\top}&\mathbf{e}_{g_{i}}^{\top}\end{bmatrix} ^ {\top}\). Further, for all \(n\) point pairs, we have  
\[\mathbf{l}-\mathbf{J}\mathbf{e}=\mathbf{A}\mathrm{d}\mathbf{\xi} \tag{4}\]  
where \(\mathbf{I}= \begin{bmatrix}\mathbf{I}_{1}&\mathbf{I}_{2}&\cdots&\mathbf{I}_{n}\end{bmatrix} ^ {\top}\), \(\mathbf{J}= \begin{bmatrix}\mathbf{I}_{3n}&-\mathbf{B}\end{bmatrix} \), \(\mathbf{B}=\mathbf{R}^{0}\otimes\mathbf{I}_{n}\), \(\otimes\) denotes the [PERSON] inner product, \(\mathbf{e}= \begin{bmatrix}\mathbf{e}_{f}^{\top}&\mathbf{e}_{g}^{\top}\end{bmatrix} ^{\top}\), \(\mathbf{e}_{f}^{\top}= \begin{bmatrix}\mathbf{e}_{f_{1}}^{\top}&\mathbf{e}_{f_{2}}^{\top}& \cdots&\mathbf{e}_{f_{n}}^{\top}\end{bmatrix} \), and \(\mathbf{e}_{g}^{\top}= \begin{bmatrix}\mathbf{e}_{g_{1}}^{\top}&\mathbf{e}_{g_{2}}^{\top}& \cdots&\mathbf{e}_{g_{n}}^{\top}\end{bmatrix} \). For the error vector \(\mathbf{e}\),  
\[\mathbf{e}\sim N\Big{(}0,\sigma_{0}^{2}\mathbf{Q}\Big{)},\mathbf{Q}=\mathbf{P}^{-1}= \begin{bmatrix} \mathbf{Q}_{\mathbf{e}_{f}}&0\\ 0&\mathbf{Q}_{\mathbf{e}_{g}}\end{bmatrix}