# HiGoReg: A Hierarchical Grouping Strategy for Point Cloud Registration
## 2 Methodology
### Hierarchical Grouping Solution  
Assuming that the data are divided into \(m\) groups of different observation vectors, each containing at least 3 pairs of points, the grouping of these vectors and the corresponding stochastic model are described below:  
\[ \begin{bmatrix}L_{1}\\ L_{2}\\ \vdots\\ L_{m}\end{bmatrix} = \begin{bmatrix}I_{1}&\cdots&I_{v}\\ I_{v+1}&\cdots&I_{u}\\ \vdots&&\vdots\\ I_{u+1}&\cdots&I_{n}\end{bmatrix} ,\quad \begin{cases}\mathbf{Q}_{1}=\text{diag}( \mathbf{Q}_{\epsilon_{1}}\!\cdots\!,\mathbf{Q}_{\epsilon_{v}})\\ \mathbf{Q}_{2}=\text{diag}(\mathbf{Q}_{\epsilon_{v+1}}\!\cdots\!,\mathbf{Q}_{\epsilon_{u}} )\\ \vdots\\ Q_{m}=\text{diag}(\mathbf{Q}_{\epsilon_{u+1}}\!\cdots\!,\mathbf{Q}_{\epsilon_{u}}) \end{cases}  \tag{9}\]  
where \(v\), \(u\) denote numerical values and satisfy \(n\geqslant u+3\geqslant v+6\geqslant 9\). Concerning Equations (4) and (9) as our theoretical backbone, for two adjacent groups, labeled as the \(k-1\) and \(k\), we have  
\[ \begin{cases}\mathbf{L}_{k-1}-\mathbf{J}_{k-1}\mathbf{e}_{k-1}=\mathbf{A}_{k-1}\text{d}\mathbf{ \xi}\\ \mathbf{L}_{k}-\mathbf{J}_{k}\mathbf{e}_{k}=\mathbf{A}_{k}\text{d}\mathbf{\xi}\end{cases}  \tag{10}\]  
The corresponding stochastic models are  
\[\mathbf{e}_{k-1}\sim N\Big{(}0,\sigma_{0}^{2}\mathbf{Q}_{k-1}\Big{)},\mathbf{e}_{k}\sim N \Big{(}0,\sigma_{0}^{2}\mathbf{Q}_{k}\Big{)} \tag{11}\]  
Based on Section 2.1, the batch solution for both sets of data can thus be stated as  
\[\text{d}\mathbf{\hat{\xi}}= \tag{12}\]  
where \(\mathbf{Q}_{\xi_{k-1}}=A_{k-1}^{\top}\mathbf{Q}_{I_{k-1}}^{-1}\mathbf{A}_{k-1}\). Then, for group \(k-1\), its solution follows:  
\[\text{d}\mathbf{\hat{\xi}}_{k-1}=\mathbf{Q}_{\xi_{k-1}}^{-1}\mathbf{A}_{k-1}^{\top}\mathbf{Q}_ {I_{k-1}}^{-1}\mathbf{L}_{k-1} \tag{13}\]  
The cofactor matrix of the batch solution for the two sets of data is  
\[\mathbf{Q}_{\xi}=\mathbf{Q}_{\xi_{k-1}}+\mathbf{A}_{k}^{\top}\mathbf{Q}_{I_{k}}^{-1}\mathbf{A}_{k} \tag{14}\]  
Consequently, Equation (12) can be reorganized as  
\[\text{d}\mathbf{\hat{\xi}}=\mathbf{Q}_{\xi}^{-1}\Big{(}A_{k-1}^{\top}\mathbf{Q}_{I_{k-1}}^ {-1}\mathbf{L}_{k-1}\Big{)}+\mathbf{Q}_{\xi}^{-1}\Big{(}\mathbf{A}_{k}^{\top}\mathbf{Q}_{I_{k} }^{-1}\mathbf{L}_{k}\Big{)} \tag{15}\]  
Equations (13) and (14) are reformulated into  
\[\mathbf{A}_{k-1}^{\top}\mathbf{Q}_{I_{k-1}}^{-1}\mathbf{L}_{k-1}=\mathbf{Q}_{\xi_{k-1}}\text{d }\mathbf{\hat{\xi}}_{k-1} \tag{16}\]  
\[\mathbf{Q}_{\xi_{k-1}}=\mathbf{Q}_{\xi}-\mathbf{A}_{k}^{\top}\mathbf{Q}_{I_{k}}^{-1}\mathbf{A}_{k} \tag{17}\]  
By substituting \(\mathbf{Q}_{\xi_{k-1}}\) from Equation (17) into Equation (16),  
\[\mathbf{A}_{k-1}^{\top}\mathbf{Q}_{I_{k-1}}^{-1}\mathbf{L}_{k-1}=\mathbf{Q}_{\xi}\text{d}\mathbf{ \hat{\xi}}_{k-1}-\mathbf{A}_{k}^{\top}\mathbf{Q}_{I_{k}}^{-1}\mathbf{A}_{k}\text{d}\mathbf{ \hat{\xi}}_{k-1} \tag{18}\]and subsequently incorporating the resulting expression into Equation (15), we can derive the fundamental solution of the HiGoReg method:  
\[ \begin{split}\mathrm{d}\hat{\mathbf{\xi}}&=\mathrm{d}\hat{ \mathbf{\xi}}_{k-1}-\mathbf{Q}_{\xi}^{-1}\mathbf{A}_{k}^{\top}\mathbf{Q}_{Ik}^{-1}\mathbf{A}_{k} \mathrm{d}\hat{\mathbf{\xi}}_{k-1}+\mathbf{Q}_{\xi}^{-1}\mathbf{A}_{k}^{\top}\mathbf{Q}_{Ik}^{- 1}\mathbf{L}_{k}\\ &=\mathrm{d}\hat{\mathbf{\xi}}_{k-1}+\mathbf{Q}_{\xi}^{-1}\mathbf{A}_{k}^{ \top}\mathbf{Q}_{Ik}^{-1}\Big{(}\mathbf{L}_{k}-\mathbf{A}_{k}\mathrm{d}\hat{\mathbf{\xi}}_{k-1} \Big{)}\end{split}  \tag{19}\]  
Upon scrutinizing the preceding equation, it becomes evident that the batch solution for two sequential datasets can be efficiently attained by leveraging the computational outcomes of the current dataset in conjunction with those from the preceding one. Nevertheless, a hindrance arises in the form of the elusive matrix \(\mathbf{Q}_{\xi}^{-1}\) present in Equation (19).