# HiGoReg: A Hierarchical Grouping Strategy for Point Cloud Registration
## 2 Methodology
### Hierarchical Grouping Solution
To circumvent this obstacle and establish a recursive linkage for the covariance matrices associated with the parameter vectors across adjacent groups, we meticulously multiply both sides of Equation (17) by \(\mathbf{Q}_{\xi}^{-1}\) from the left and \(\mathbf{Q}_{\xi_{k-1}}^{-1}\) from the right. Consequently, the recursive relationship for the covariance matrices of the parameter vectors spanning the (\(k-1\))th and \(k\)th groups is elegantly formulated as follows:  
\[\mathbf{Q}_{\xi}^{-1}=\mathbf{Q}_{\xi_{k-1}}^{-1}-\mathbf{Q}_{\xi}^{-1}\mathbf{A}_{k}^{\top} \mathbf{Q}_{Ik}^{-1}\mathbf{A}_{k}^{\top}\mathbf{Q}_{\xi_{k-1}}^{-1} \tag{20}\]  
According to the Woodbury matrix transformation formula [64], where \(\mathbf{M}\) is an invertible \(m\times m\) matrix, \(\mathbf{C}\) is an invertible \(p\times p\) matrix, \(\mathbf{U}\in\mathbb{R}^{m\times p}\), and \(\mathbf{V}\in\mathbb{R}^{p\times m}\) are matrices with dimensions that ensure the operations within the formula are well-defined, the subsequent equation is valid:  
\[(\mathbf{M}+\mathbf{U}\mathbf{C}\mathbf{V})^{-1}=\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{U}(\mathbf{C}^{-1}+\bm {V}\mathbf{M}^{-1}\mathbf{U})^{-1}\mathbf{V}\mathbf{M}^{-1} \tag{21}\]  
For the detailed derivation process, please refer to Appendix A. Then, for Equation (14), it reads  
\[\mathbf{Q}_{\xi}^{-1}=\Big{(}\mathbf{Q}_{\xi_{k-1}}+\mathbf{A}_{k}^{\top}\mathbf{Q}_{Ik}^{-1} \mathbf{A}_{k}\Big{)}^{-1}=\mathbf{Q}_{\xi_{k-1}}^{-1}-\mathbf{Q}_{\xi_{k-1}}^{-1}\mathbf{A}_{ k}^{\top}\Big{(}\mathbf{Q}_{Ik}+\mathbf{A}_{k}\mathbf{Q}_{\xi_{k-1}}^{-1}\mathbf{A}_{k}^{\top} \Big{)}^{-1}\mathbf{A}_{k}\mathbf{Q}_{\xi_{k-1}}^{-1} \tag{22}\]  
Compare Equations (20) and (22), which states  
\[\mathbf{Q}_{\xi}^{-1}\mathbf{A}_{k}^{\top}\mathbf{Q}_{Ik}^{-1}=\mathbf{Q}_{\xi_{k-1}}^{-1}\mathbf{ A}_{k}^{\top}\Big{(}\mathbf{Q}_{Ik}+\mathbf{A}_{k}\mathbf{Q}_{\xi_{k-1}}^{-1}\mathbf{A}_{k}^{ \top}\Big{)}^{-1} \tag{23}\]  
Given \(\mathbf{G}_{k-1,k}=\mathbf{Q}_{\xi_{k-1}}^{-1}\mathbf{A}_{k}^{\top}\Big{(}\mathbf{Q}_{Ik}+\mathbf{ A}_{k}\mathbf{Q}_{\xi_{k-1}}^{-1}\mathbf{A}_{k}^{\top}\Big{)}^{-1}\) and \(\mathbf{L}_{k-1,k}=\mathbf{L}_{k}-\mathbf{A}_{k}\mathrm{d}\hat{\mathbf{\xi}}_{k-1}\), and rewriting Equation (19), the final form of HiGoReg can thus be obtained:  
\[\mathrm{d}\hat{\mathbf{\xi}}_{k}=\mathrm{d}\hat{\mathbf{\xi}}_{k-1}+\mathbf{G}_{k-1,k}\tilde {\mathbf{L}}_{k-1,k} \tag{24}\]  
\[\tilde{\mathbf{e}}_{f_{k}}=\mathbf{Q}_{\epsilon_{fk}}\mathbf{Q}_{Ik}^{-1}\Big{(}\mathbf{L}_{k} -\mathbf{A}_{k}\mathrm{d}\hat{\mathbf{\xi}}_{k}\Big{)} \tag{25}\]  
\[\tilde{\mathbf{e}}_{g_{k}}=-\mathbf{Q}_{\epsilon_{g_{k}}}\mathbf{B}_{k}^{\top}\mathbf{Q}_{Ik}^{ -1}\Big{(}\mathbf{L}_{k}-\mathbf{A}_{k}\mathrm{d}\hat{\mathbf{\xi}}_{k}\Big{)} \tag{26}\]  
here, \(k\geqslant 2\), so that for the first group of data, one still needs to perform the computation using the batch solution, cf. Section 2.1. Then the a posterior unit-weight median error is  
\[\hat{\sigma}_{0}=\sqrt{\frac{1}{3n-6}\left(\tilde{\mathbf{\varepsilon}}_{g}^{\top} \mathbf{Q}_{\epsilon_{g}}^{-1}\tilde{\mathbf{e}}_{g}+\tilde{\mathbf{\varepsilon}}_{f}^{\top }\mathbf{Q}_{f_{g}}^{-1}\tilde{\mathbf{\varepsilon}}_{f}\right)} \tag{27}\]Based on the above derivation process, the systematic computational steps of HiGoReg are outlined in Algorithm A1 in Appendix B. In practical applications, an insufficient number of observations within a group may result in rank deficiency. To avoid this, the minimum number of observations per group is typically determined by the number of parameters to be estimated. If rank deficiency still arises, additional point pairs can be randomly selected from other groups to supplement the deficient set. This is feasible because all point pairs share the same parameter structure. More commonly, observations within a group may exhibit strong linearity or planarity. This leads to an ill-conditioned coefficient matrix, compromising solution stability and accuracy.