optimal solution for 3D point registration without correspondence. The ICP framework [@besl1992method] gives a general way to find correspondences and pose at the same time. Although this framework was originally designed for points, other 3D models can also be introduced into this framework [@segal2009generalized; @censi2008icp; @serafin2015nicp]. The ICP framework iteratively finds the nearest elements as correspondences then calculates the pose until it converges. Efficiently and accurately estimating the pose from current nearest 3D correspondences is critical for the ICP framework.

Problem Formulation {#sec:Problem}
===================

In this paper, we use italic, boldfaced lowercase and boldfaced uppercase letters to represent scalars, vectors and matrices, respectively. The paper focuses on the problem of pose estimation from point-to-plane, point-to-line and point-to-point correspondences. We first consider the point-to-plane correspondence. Suppose we have a point ${{\mathbf{x}}_{l}}$ in one coordinate system and the corresponding plane in another coordinate system represented by the plane’s norm-one normal $\mathbf{n}$ and a point ${{\mathbf{y}}^{\pi }}$ lying on it. Then the scalar residual between a point and a plane can be written as $$\label{equ:1}
{{r}_{\pi }}={{\mathbf{n}}^{T}}\left( \mathbf{R}{{\mathbf{x}}_{\pi }}+\mathbf{t}-{{\mathbf{y}}_{\pi }} \right)$$ For the point-to-line correspondence, we can calculate the 3-dimensional residual vector between a point ${{\mathbf{x}}_{l}}$ and the corresponding line represented by the 