# Total Least-Squares Iterative Closest Point Algorithm Based on Lie Algebra
## 2 ICP Algorithm Based on Lie Algebra
### Jacobian of the GHM
In this situation, the LS algorithm is nearly as accurate as the TLS algorithm, and we can see this result in the first column of Figures 1 and 2. However, the TLS algorithm performs much better than the LS algorithm when variances between the x-, y-, and z-axes become large, as shown in the fourth column and the eighth column in Figures 1 and 2. Method 1 is more accurate than method 2 in estimating the rotation matrix, but method 2 is more accurate than method 1 in estimating the translation vector. This phenomenon is caused by different parameterization approaches in these two methods. The parameterization of method 2 uses the Lie algebra of \(SE\) (3), which is a six-dimensional vector. The \begin{table}
\begin{tabular}{c c c c c c c c} \hline \hline
**Point No.** & **Source x** & **Source y** & **Source z** & **Target x\({}^{\prime}\)** & **Target y\({}^{\prime}\)** & **Target z\({}^{\prime}\)** \\ \hline
1 & 63 & 84 & 21 & 290 & 150 & 15 \\
2 & 210 & 84 & 21 & 420 & 80 & 2 \\
3 & 210 & 273 & 21 & 540 & 200 & 20 \\
4 & 63 & 273 & 21 & 390 & 300 & 5 \\ \hline \hline \end{tabular}
\end{table} 
Table 2: A numerical example. \begin{table}
\begin{tabular}{c c c c c c c c c} \hline \hline
**Noise** & **Noise Type** & **x** & **y** & **z** & **Noise** & **Noise** & **Type** & **x** & **y** & **z** \\ \hline
2 & Normal & 0.1 & 0.5 & 1 & 6 & Uniform & 5 & 10 & 20 \\
3 & Normal & 0.1 & 0.5 & 0.5 & 7 & Uniform & 5 & 10 & 10 \\
4 & Normal & 0.1 & 1 & 1 & 8 & Uniform & 5 & 20 & 20 \\ \hline \hline \end{tabular}
\end{table} 
Table 3: Noise type and variance for each class. first three elements in this vector describe the translation and rotation matrices. The last three elements only describe the rotation matrix. The first three elements are optimized during the TLS iterative process, and then we recover the translation vector using the last three elements in the Lie algebra vector. The benefit of this is that the Jacobian matrix conditional number is not so large, which means that the estimated parameters equally decide the direction of optimization. In Method 1, the Jacobian matrix of \(\phi\) is \(-(Rp)^{\wedge}\), and the Jacobian matrix of t is always the identity matrix. If \(p\) is very large, then the Jacobian matrix of \(\phi\) mainly decides the direction of optimization, so that method 1 has a smaller rotation error than method 2. Here, we directly use data from [12], which is an actual experiment of fitting two surfaces surveyed in two reference systems. Data are shown in Table 2, and the result is shown in Table 4. We give results of LS, TLS based on Euler angle, method 1, and method 2. Our two methods have much smaller sums of residual errors than the LS method. This is mainly because the TLS algorithm considers noise from  
Figure 1: The rotation error is compared between least-squares and our proposed methods. The blue, green, and yellow bars are the rotation errors of least-squares, method 1, and method 2, respectively. The \(x\)-axis of this figure is a different noise class shown in Table 3.  
Figure 2: The translation error is compared between least-squares method and our proposed methods. The blue, green, and yellow bars are the translation errors of least-squares, method 1, and method 2, respectively. The \(x\)-axis of this figure is a different noise class shown in Table 3.  
the source and target coordinate systems. Further discussion of errors and the model are beyond the scope of this paper; refer to [PERSON] [18]. Compared to the Euler angle method, our method has smaller SSE after optimization, which proves that Lie algebra is more suitable for the TLS-ICP algorithm. We should note that our method also can be used in similar transformation estimation. The difference is that the Jacobian of scale s is needed for additional calculations. TLS-ICP algorithm often uses 4-8 points to calculate transformation between two coordinates in geological mapping. Thus, in previous experiments, datasets from [9] are used to evaluate accuracy.