# Total Least-Squares Iterative Closest Point Algorithm Based on Lie Algebra
## 2 ICP Algorithm Based on Lie Algebra
### Jacobian of the GHM  
\(J_{e}=\frac{\partial\Psi}{\partial e^{T}}\) and \(J_{\xi}=\frac{\partial\Psi}{\partial\xi^{T}}\) are needed to obtain the GHM model results. First, we change the form of equation \(\Psi\) to:  
\[\Psi=(A+V_{A})R^{T}+I_{n}^{T}\otimes t^{T}-Y-V_{y}=(R\otimes I_{n})\times vec (A+V_{A})+t\otimes I_{n}-vec(Y+V_{y}), \tag{20}\]  
where \(e\) is linear in Equation (5), and \(J_{e}\) is easily obtained as:  
\[J_{e}=\left[ \begin{array}{cc}\frac{\partial\Psi}{\partial vec(V_{A})^{T}}& \frac{\partial\Psi}{\partial vec(V_{y})^{T}}\end{array} \right]=\left[R\otimes I _{n}-I_{3}\otimes I_{n}\right]_{3n\times 6n}, \tag{21}\]  
where \(I_{n}\) is an identity matrix of dimension \(n\). Then we need to calculate \(J_{\xi}=\frac{\partial\psi}{\partial\xi^{T}}\). **Method 1**: If we use parameterization method 1, then the Jacobian matrix is:  
\[J_{\xi}=\left[ \begin{array}{cc}\frac{\partial\Psi}{\partial\left[\begin{array} []{cc}\phi_{1}&\phi_{2}&\phi_{3}\end{array}\right]}&\frac{\partial\Psi}{ \partial\left[\begin{array}{cc}t_{1}&t_{2}&t_{3}\end{array}\right]}\end{array}  \right]_{3n\times 6}, \tag{22}\]  
\(\frac{\partial\psi}{\partial\left[ \begin{array}{cc}t_{1}&t_{2}&t_{3}\end{array} \right]}\) is easily obtained from Equation (20) as:  
\[\frac{\partial\psi}{\partial\left[ \begin{array}{cc}t_{1}&t_{2}&t_{3}\end{array}  \right]}=I_{3}\otimes I_{n}, \tag{23}\]\[\frac{\partial\psi}{\partial\left[ \begin{array}{cc}\phi_{1}&\phi_{2}&\phi_{3} \end{array} \right]}\] can be simplified as follows: \[\frac{\partial\psi}{\partial\left[ \begin{array}{cc}\phi_{1}&\phi_{2}&\phi_{3} \end{array} \right]}=\left[ \begin{array}{cc}\textit{vec}\left(\frac{ \partial(A+V_{A})R^{T}}{\partial\phi_{1}}\right)&\textit{vec}\left(\frac{ \partial(A+V_{A})R^{T}}{\partial\phi_{2}}\right)&\textit{vec}\left(\frac{ \partial(A+V_{A})R^{T}}{\partial\phi_{2}}\right)\end{array} \right]_{3n\times 3}, \tag{24}\]  
According to deductions from Appendix A, we know that:  
\[\frac{\partial Rp}{\partial\left[ \begin{array}{cc}\phi_{1}&\phi_{2}&\phi_{3 }\end{array} \right]}=-(Rp)^{\wedge}. \tag{25}\]  
We let \(R(X_{i}+\Delta X_{i})=\left[ \begin{array}{cc}f_{i}&g_{i}&h_{i}\end{array}  \right]^{T}\), so we can obtain the following equation:  
\[\frac{\partial R(X_{i}+\Delta X_{i})}{\partial\left[ \begin{array}{cc}\phi_ {1}&\phi_{2}&\phi_{3}\end{array} \right]}=\left[ \begin{array}{cc}\frac{ \partial f_{i}}{\partial\phi_{1}}&\frac{\partial f_{i}}{\partial\phi_{2}}& \frac{\partial f_{i}}{\partial\phi_{3}}\\ \frac{\partial g_{i}}{\partial\phi_{1}}&\frac{\partial g_{i}}{\partial\phi_{2}} &\frac{\partial g_{i}}{\partial\phi_{3}}\\ \frac{\partial h_{i}}{\partial\phi_{1}}&\frac{\partial h_{i}}{\partial\phi_{2}} &\frac{\partial h_{i}}{\partial\phi_{3}}\end{array} \right], \tag{26}\]  
The elements in Equation (26) can be obtained from Equation (25). Thus, \(\frac{\partial\psi}{\partial\left[ \begin{array}{cc}\phi_{1}&\phi_{2}&\phi_{ 3}\end{array} \right]}\) is equal to:  
\[\frac{\partial\textit{vec}(A+V_{A})R^{T}}{\partial\phi_{1}}=\left[ \begin{array} []{cc}\frac{\partial f_{1}}{\partial\phi_{1}}&\frac{\partial g_{1}}{\partial \phi_{1}}&\frac{\partial h_{1}}{\partial\phi_{1}}\\ \frac{\partial f_{2}}{\partial\phi_{1}}&\frac{\partial g_{2}}{\partial\phi_{1} }&\frac{\partial h_{2}}{\partial\phi_{1}}\\ \frac{\partial f_{n}}{\partial\phi_{1}}&\frac{\partial g_{n}}{\partial\phi_{1} }&\frac{\partial h_{n}}{\partial\phi_{1}}\\ \end{array} \right], \tag{27}\]  
\[\frac{\partial\textit{vec}(A+V_{A})R^{T}}{\partial\phi_{2}}=\left[ \begin{array} []{cc}\frac{\partial f_{1}}{\partial\phi_{2}}&\frac{\partial g_{1}}{\partial \phi_{2}}&\frac{\partial h_{1}}{\partial\phi_{2}}\\ \frac{\partial f_{2}}{\partial\phi_{2}}&\frac{\partial g_{2}}{\partial\phi_{2} }&\frac{\partial h_{2}}{\partial\phi_{2}}\\ \frac{\partial f_{n}}{\partial\phi_{2}}&\frac{\partial g_{n}}{\partial\phi_{2} }&\frac{\partial h_{n}}{\partial\phi_{2}}\\ \end{array} \right], \tag{28}\]  
\[\frac{\partial\textit{vec}(A+V_{A})R^{T}}{\partial\phi_{3}}=\left[