# Total Least-Squares Iterative Closest Point Algorithm Based on Lie Algebra
## 2 ICP Algorithm Based on Lie Algebra
### Variant of the Gauss-Helmert Model  
First, we transform Equation (4) to:  
\[\Phi=\underset{R,t}{min}\ e^{T}Pe+2\lambda^{T}[(A+V_{A})R^{T}+l_{n}^{T}\otimes l ^{T}-Y-V_{y}], \tag{5}\]  
where \(e=\left[\begin{array}{c}vec(V_{A})\\ vec(V_{y})\end{array}\right]_{\theta\text{tr}=1}\), and the operator \"vec\" is to stack one column of a matrix under the other from left to right. \(p\) is the Fisher information matrix and is equal to the identity matrix in this paper:  
\[A+V_{A}=\left[\begin{array}{cccc}x_{1}+\Delta x_{1}&y_{1}+\Delta y_{1}&z_{1 }+\Delta z_{1}\\ x_{2}+\Delta x_{2}&y_{2}+\Delta y_{2}&z_{2}+\Delta z_{2}\\ \ldots&\ldots&\ldots\\ x_{n}+\Delta x_{n}&y_{n}+\Delta y_{n}&z_{n}+\Delta z_{n}\end{array}\right]_{ \eta\times 3}, \tag{6}\]  
\[Y+V_{y}=\left[\begin{array}{cccc}{x_{1}}^{\prime}+\Delta{x_{1}}^{\prime}&{ y_{1}}^{\prime}+{\Delta y_{1}}^{\prime}&{z_{1}}^{\prime}+\Delta{z_{1}}^{\prime}\\ {x_{2}}^{\prime}+\Delta{x_{2}}^{\prime}&{y_{2}}^{\prime}+\Delta{y_{2}}^{\prime }&{z_{2}}^{\prime}+\Delta{z_{2}}^{\prime}\\ \ldots&\ldots&\ldots\\ {x_{n}}^{\prime}+\Delta{x_{n}}^{\prime}&{y_{n}}^{\prime}+\Delta{y_{n}}^{\prime }&{z_{n}}^{\prime}+\Delta{z_{n}}^{\prime}\end{array}\right]_{\eta\times 3}, \tag{7}\]  
\(l_{n}\) is an \(n\times 1\) vector of ones. The operator \(\otimes\) denotes the Kronecker-Zehfuss product, and \(X_{i}=\left[\begin{array}{cccc}x_{i}&y_{i}&z_{i}\end{array}\right]^{T}\), \(X_{i}^{\prime}=\left[\begin{array}{cccc}x_{i}^{\prime}&y_{i}^{\prime}&z_{i} ^{\prime}\end{array}\right]^{T}\). We find that \(\Psi=(A+V_{A})R^{T}+l_{n}\otimes l^{T}-Y-V_{y}\) is related to the estimated parameter \(\xi\) and random error e. We use a first-order Taylor expansion to \(\Psi\) and obtain a linear equation with respect to \(\xi\) and \(e\):  
\[\Psi\approx J_{e}\delta e+J_{\xi}\delta\xi+\Psi(e_{0},\xi_{0}), \tag{8}\]  
where \(J_{e}=\frac{\partial\Psi}{\partial e^{T}}\), \(J_{\xi}=\frac{\partial\Psi}{\partial\xi^{T}}\), \(\delta e=e-e_{0}\), and \(\delta\xi\) is the update step of the estimated parameter. We substitute Equation (8) into Equation (5) to obtain:  
\[\Phi\approx\underset{R,t}{min}\ e^{T}Pe+2\lambda^{T}[J_{e}\delta e+J_{\xi} \delta\xi+\Psi(e_{0},\xi_{0})], \tag{9}\]  
We take the partial derivative of Equation (9) with respect to e and the estimated parameter \(\xi\) to obtain:  
\[0.5\times\frac{\partial\Phi}{\partial e^{T}}=Pe+B_{0}^{T}\lambda=0, \tag{10}\]\[0.5\times\frac{\partial\Phi}{\partial\xi^{T}}=A_{0}^{T}\lambda=0, \tag{11}\]  
where \(B_{0}=[R\otimes I_{n}-I_{3}\otimes I_{n}]\) and \(A_{0}=J_{\xi}\). The derivations of these two parameters are given in Section 2.4. We combine Equations (8)-(11) and obtain iterative results:  
\[\delta\xi=-(J_{\xi}^{T}N_{0}^{-1}J_{\xi})^{-1}J_{\xi}^{T}N_{0}^{-1}\omega_{0}, \tag{12}\]  
\[\lambda=N_{0}^{-1}(J_{\xi}\delta\xi+\omega_{0}), \tag{13}\]  
\[e=-P^{-1}J_{e}^{T}\lambda, \tag{14}\]  
where \(\omega_{0}=-J_{e}e+\Psi(e_{0},\xi_{0})\). The initial value of the random error is \(e_{0}=0\), and \(\xi_{0}=\left[\begin{array}{cccc}0&0&0&0&0&0\end{array}\right]^{T}\). We can use Equations (12)-(14) to update \(\xi\).