# Total Least-Squares Iterative Closest Point Algorithm Based on Lie Algebra
## 2 ICP Algorithm Based on Lie Algebra
### Jacobian of the GHM
\begin{array} []{cc}\frac{\partial f_{1}}{\partial\phi_{3}}&\frac{\partial g_{1}}{\partial \phi_{3}}&\frac{\partial h_{1}}{\partial\phi_{3}}\\ \frac{\partial f_{2}}{\partial\phi_{3}}&\frac{\partial g_{2}}{\partial\phi_{3} }&\frac{\partial h_{2}}{\partial\phi_{3}}\\ \frac{\partial f_{n}}{\partial\phi_{3}}&\frac{\partial g_{n}}{\partial\phi_{3} }&\frac{\partial h_{n}}{\partial\phi_{3}}\\ \end{array} \right], \tag{29}\]  
The estimated parameters in method 1 are the Lie algebra of \(SO\left(3\right)\) and the translation vector. Thus, the update parameter is equal to \(\left[ \begin{array}{cc}\Delta\phi&\Delta t\end{array} \right]^{T}\). The translation vector is in Euclidean space, so it is easy to update the new translation variable: \(t_{k+1}=t_{k}+\Delta t\). \(\Delta\phi\) belongs to the Lie algebra of \(SO\left(3\right)\), and the updated formula is:  
\[\phi_{k+1}=ln(\textit{exp}(\Delta\phi^{\wedge})R_{k})^{\vee}, \tag{30}\]  
\(R_{k}\) is the kth step rotation matrix. The operator \"\(\wedge\)\" is defined in Section 2.4. The \"\(ln()\)\" operator transforms the Lie group of \(SO\left(3\right)\) to the Lie algebra of \(SO\left(3\right)\). We do not provide specific expressions for \"\(ln()\)\"\(\wedge\)\" here; these can be found in [17]. In Appendix A, we deduce the Jacobian of Equation (25) using left multiplication so that update Formula (30) also uses left multiplication. Thus, the method 1 parameter update formula is:  
\[\xi_{k+1}=\xi_{k}\oplus\delta\xi=\left[ \begin{array}{cc}t_{k}+\Delta t&ln( \textit{exp}(\Delta\phi^{\wedge})R_{k})^{\vee}\end{array} \right]^{T}, \tag{31}\]  
accurate. Then we use real measurement data from Table 2 and demonstrate that the TLS algorithm has a smaller residual error than the LS method. First, we compare results from TLS and LS using simulated data. A true translation vector is set to (190, 110, -15), and a rotation matrix is constructed by the Euler angle. The true rotation angles around the z-, y-, and x-axes are set to 45 degrees, 90 degrees, and 60 degrees, respectively. Then, we generate controlled points in target coordinates according to the true rotation matrix, true translation, and controlled points in the source coordinate. We add noise in the source and target coordinates according to two distributions: normal and uniform. We classify noise into eight classes, as shown in Table 3. The normal distribution is a Gaussian distribution with zero mean and a covariance. We use the uniform distribution to generate samples in some range, and every sample is generated by equal chance. In the first row of Table 3, 0.1 is the variance of the Gaussian distribution, and a noise number of 5 means that the uniform distribution generates samples ranging from -5 to 5. The rotation error is:  
\[Error_{R}=acos(max(min(0.5*(\Delta R(0,0)+\Delta R(1,1)+\Delta R(2,2)),1),-1), \tag{38}\]  
where \(\Delta R=R_{true}^{T}R_{TLS}\), \(R_{TLS}\) is the rotation matrix from the TLS algorithm, and \(R_{true}\) is the true rotation matrix. The error measure in Equation (38) describes how close \(\Delta R\) is to the identity matrix. If \(\Delta R\) is closer to the identity matrix, then our algorithm is more accurate. The translation error is evaluated as \(Error_{t}=\|t_{true}-t_{TLS}\|\), where \(t_{TLS}\) is the translation from the TLS algorithm, and \(t_{true}\) is the true translation. If noise is generated by a normal distribution, then the information matrix \(p\) is set to the inverse of the variance. If noise is generated by a uniform distribution, then the information matrix \(p\) is set to the identity matrix. Figures 1 and 2 show the rotation error and translation error, respectively, of least-squares, method 1, and method 2. We see from these figures that larger variances between the x-, y-, and z-axes lead to a larger error of the least-squares method. This is because the rotation matrix is obtained by the Procrustes approach [9], which assumes that noise in the x-, y-, and z-axes is equal.