# Total Least-Squares Iterative Closest Point Algorithm Based on Lie Algebra
## 2 ICP Algorithm Based on Lie Algebra
### Lie Algebra and Lie Group  
Euler angle constructs rotation matrix as following equation:  
\[R=\left[ \begin{array}{cccc}cos\theta_{z}&-sin\theta_{z}&0\\ sin\theta_{z}&cos\theta_{z}&0\\ 0&0&1\end{array} \right]\left[ \begin{array}{cccc}cos\theta_{y}&0&sin\theta_ {y}\\ 0&1&0\\ -sin\theta_{y}&0&cos\theta_{y}\end{array} \right]\left[ \begin{array}{cccc}1&0 &0\\ 0&cos\theta_{x}&-sin\theta_{x}\\ 0&sin\theta_{x}&cos\theta_{x}\end{array} \right], \tag{15}\]  
where \(\theta_{z}\), \(\theta_{y}\), and \(\theta_{x}\) are, respectively, the rotation angles around the z-, y-, and x-axes. If \(\theta_{y}=\frac{\pi}{2}\), then the Euler angle is unsuitable for interpolation because of the gimbal lock problem. When the second rotation angle is 90 degrees, the rotation matrix constructed by the Euler angle loses z- and \(x\)-axis freedom. In this paper, we introduce Lie algebra and a Lie group to replace the Euler angle. A group is a set of elements together with an operation that combines any two of its elements to form a third element also in the set. A Lie group is also a differential manifold, with the property that the group operations are smooth [17]. A Lie group must satisfy the four properties listed in Table 1.  
A rotation matrix is a special orthogonal group, denoted by \(SO\) (3), which also belongs to a Lie group. A rigid transformation is in a special Euclidean group, denoted by \(SE\) (3), which also belongs to a Lie group. Every Lie group is associated with a Lie algebra. For example, a rotation matrix is associated with a three-dimensional vector \(\phi\), which is the angle-axis representation of the rotation matrix R. The rigid transformation matrix \(\left[ \begin{array}{cc}R&t\\ 0&1\end{array} \right]\) is associated with the six-dimensional vector \(\xi\). \(SO\) (3) is the Lie algebra corresponding to the Lie group \(SO\) (3), and \(SE\) (3) is the Lie algebra corresponding to the Lie group \(SE\) (3). A Lie algebra operation is defined by a Lie bracket. The Lie bracket of \(\phi\) is:  
\[[\phi_{1},\phi_{2}]=(\phi_{1}\phi_{2}-\phi_{2}\phi_{1})^{\wedge}, \tag{16}\]  
where \(\phi^{\wedge}\) is the antisymmetric matrix of vector \(\phi\). The Lie bracket of \(\xi\) is:  
\[[\xi_{1},\xi_{2}]=({\xi_{1}}^{\wedge}{\xi_{2}}^{\wedge}-{\xi_{2}}^{\wedge}{\xi _{1}}^{\wedge})^{\wedge}, \tag{17}\] \begin{table}
\begin{tabular}{c c c} \hline \hline
**Property** & \(SO\) **(3)** & \(SE\) **(3)** \\ \hline Closure & \(R_{1}R_{2}\in SO(3)\) & \(T_{1}T_{2}\in SE(3)\) \\ Associativity & \(R_{1}(R_{2}R_{3})=(R_{1}R_{2})R_{3}=R_{1}R_{2}R_{3}\) & \(T_{1}(T_{2}T_{3})=(T_{1}T_{2})T_{3}=T_{1}T_{2}T_{3}\) \\ Identity & \(R_{1}=1R_{1}=R_{1}\in SO(3)\) & \(T_{1}1=1T_{1}=T_{1}\in SE(3)\) \\ Invertibility & \(R_{1}^{-1}\in SO(3)\) & \(T_{1}^{-1}\in SE(3)\) \\ \hline \hline \end{tabular}
\end{table} 
Table 1: Properties for Lie group. \(R_{1},R_{2},R_{3}\in SO(3)\). \(T_{1},T_{2},T_{3}\in SE(3)\). The operator \"\(\wedge\)\" has different definitions for the Lie algebra of \(SO\) (3) and \(SE\) (3). If \(\xi\) is in \(SE\) (3) space, then \(\xi^{\wedge}=\left[ \begin{array}{cc}\phi^{\wedge}&\rho\\ 0&1\end{array} \right]\). If \(\xi\) is in \(SO\) (3) space, then \(\xi^{\wedge}\) is equal to the antisymmetric matrix of vector \(\xi\). We can see from Equation (8) that a nonlinearity factor only exists in a rotation matrix. According to \(SO\) (3) and \(SE\) (3), we have two strategies to parameterize a rigid transformation matrix. The first is to use \(SO\) (3) and a translation vector to parameterize the transformation matrix. The second uses \(SE\) (3) directly to parameterize the transformation matrix. We will compare the performance of these two methods and come to a conclusion which method is more suitable for the TLS-ICP algorithm. Here, we provide two parameterization methods to calculate \(J_{\xi}\). **Method 1:** The estimated parameter is [\(\phi\),t]:  
\[\left[ \begin{array}{cc}R&t\\ 0&1\end{array} \right]=\left[ \begin{array}{cc}exp(\phi^{\wedge})&t\\ 0&1\end{array}