## 4 Spatial Positioning and Postural Estimation Method
### Fine Registration Process  
The point cloud fine registration employs ICP [23] with a KD-tree [27], which obtains a set of point pairs of minimum Euclidian distances between the source and target point cloud. ICP requires a proper initial matching point to achieve high precision and convergency, as well as to reduce computational complexity. The rough matching results provide the initial matching points so the requirements can be met. The KD-tree provides an optimized mechanism that further accelerates the registration process. Meanwhile, RANSAC is employed to remove the incorrect matching point pairs to further improve the performance of ICP. The proposed method can effectively resolve the  
Figure 12: Original scene point cloud rendered by colors. Figure 13: Rough matching result. local optimal problem that occurs in the classic ICP algorithm. Since the precision of classic ICP is influenced by the initial position, the rough matching result is applied as the initial position of the fine registration to improve the effect of the iterative optimization of ICP. Assume the point set of the CPC is \(S=\{s_{1},s_{2},s_{3},\ldots\ s_{m}\}\), where \(m\) is the point count of the CPC, and the transformed MSPC is \(G=\{g_{1},g_{2},g_{3},\ldots\ g_{n}\}\), where \(n\) is the point count of the MSPC. The threshold of the mean squared error for each iteration is denoted by \(E\). The threshold of the mean squared error difference between two adjacent iterations is represented by \(e\). The maximum number of optimization iterations is \(k_{max}\). The iterative optimization process of the fine registration is performed according to the following steps:  
1. Obtain the optimized set of corresponding point pairs: each \(g_{i}\) in \(G\) is searched within \(S\) to obtain a preliminary corresponding point pair set by finding the minimum Euclidean distance via the KD-tree search algorithm, and then the RANSAC algorithm is used to remove point pairs with large matching errors. Hence, the optimized corresponding point pair set \(\{(s_{i},g_{i})\ |i<n\}\) is obtained. 2. Obtain the transform matrices \(R\) and \(T\) according to the point pair set obtained in Step 1: the transform matrices \(R\) and \(T\) are achieved by finding the minimum value of the target function \(f(R,T)\) as the optimization objective. The target function \(f(R,T)\) is as follows: \[f(R,T)=\frac{1}{n}{\sum\limits_{i=1}^{n}{\|g_{i}-(R\cdot s_{i}+T)\|^{2}}}\] (3)
3. Update the MSPC by the transform matrices \(R\) and \(T\) obtained in Step (2): the MSPC update formula is as follows, where \(k\) is the number of iterations, \(S_{k}\) is the point cloud before the update, and \(S_{k+1}\) is the point cloud after this update. \[S_{k+1}=R\cdot s_{k}+T\] (4)
4. Calculate iteratively and judge the threshold: The mean squared errors \(d_{k}\) and \(d_{k+1}\) after this iteration are calculated by Equations (5) and (6), and then the judgment of whether any of the threshold conditions (\(d_{k}<E\), \(d_{k+1}-d_{k}<e\), \(k\geq k_{max}\)) are met is performed. If any conditions are met, the iteration stops; otherwise, go back to Step 1. \[d_{k}=\frac{1}{n}{\sum\limits_{i=1}^{n}{\|g_{i}^{k}-g_{i}^{k}\|}} \tag{5}\]  
\[d_{k+1}=\frac{1}{n}{\sum\limits_{i=1}^{n}{\|g_{i}^{k+1}-g_{i}^{k+1}\|}} \tag{6}\]
5. Perform point cloud registration after iterative optimization: the result of the last iteration is used as the optimal matching, and the optimal transform matrices \(R\) and \(T\) are obtained. Fine registration results are exhibited in Figure 14.  
The MSPC is transformed by the matrices and registered to the CPC. Thus, the final registration is accomplished, as shown in Figure 14a, which is updated from the rough matching result shown in Figure 13. It can be observed that the matching precision between the MSPC and the SPC, including both the position and the posture, is significantly improved.