# Total Least-Squares Iterative Closest Point Algorithm Based on Lie Algebra
## 1 Introduction  
An iterative closest point (ICP) algorithm [1] estimates a pose transformation matrix using coordinates from a source model and target model. This technique is frequently used in ego-motion estimation [2], geodesy, computer vision, and system identification [3]. In this paper, the ICP algorithm is only concerned with the estimation of the pose transformation matrix. This means we have known correspondences between two point clouds, and there are no outliers in the source model and target model. ICP forms a cost function using two point clouds' coordinates and searches for a rigid transformation to minimize its residual error. In [4], [PERSON] proposed an analytical solution to the ICP problem using unit-quaternion. This approach only considers target errors and assumes the error is isotropic, identical, and independent Gaussian noise. If errors from the target model are not isotropic and identical, then we can use a nonlinear optimization method [5] to obtain a rigid transformation matrix. However, errors do not only exist in the target model in practice. A more accurate model assumes that errors exist in both the source model and target model; this is called an errors-in-variables (EIV) model. [PERSON] and [PERSON] [6] proposed a total least-squares (TLS) method, especially for solving EIV problems. If the cost function of EIV is linear in the estimated parameter, we can use a Newton-Gauss TLS algorithm [7] or Lagrange TLS algorithm [8] to solve an EIV model. However, the cost function of an ICP algorithm is nonlinear because of the rotation matrix. New TLS algorithms were designed to solve nonlinear ICP problems [9; 10]. The Felus [9] uses a Procrustes approach to obtain a rotation matrix to avoid nonlinearity in the TLS cost function. Then the scale s and translation t are obtained using a linear EIV model. [PERSON] assumes that errors in the target model and source model are the same, so a rotation matrix from the Procrustes approach satisfies the EIV model. In [10], [PERSON] fixes the translation and estimates the rotation matrix using [10]. After obtainingthe rotation matrix, the translation matrix is estimated using a linear EIV model. [10] cannot guarantee the optimality and effectiveness of the algorithm because of its alternating optimization strategy. [9; 10] use approximation methods and Euler angle parameterization to solve the nonlinear TLS problem. In [11], [PERSON] constructs a cost function using elements of the rotation matrix directly. This method is convenient for obtaining results using existing methods such as [7; 12]. However, it minimizes the cost function in Euclidean space, which does not conform to reality because the rotation matrix belongs to a special orthogonal group. We cannot promise that rotation matrix constructed by elements is identity orthogonal matrix. We must minimize the Frobenius norm to project the rotation matrix of the TLS algorithm to a special orthogonal group. This does not guarantee that the final rotation matrix is the global optimal solution. Previous papers focus on how to construct a robust cost function and how to deal with the weight matrix. The Euler angle is a commonly used variable to parameterize the rotation matrix [13; 14; 15]. However, the Euler angle loses its freedom in the case of a gimbal lock and is not suitable for interpolating in nonlinear optimization. In practical situations, measurement data cannot promise that a gimbal lock does not occur. To solve this problem, we introduce Lie algebra to the TLS problem. We use a variant of the Gauss-Helmert [16] model, which is a general method to solve nonlinear TLS problems without assumptions, and Lie algebra is used to parameterize the rotation matrix without the gimbal lock problem. The rest of this paper is organized as follows. In Section 2, we introduce definitions and properties of Lie algebra and Lie groups. Then we introduce a variant of the Gauss-Helmert model that is used in the ICP model. We specify the Jacobian matrix used in the Gauss-Helmert model.