Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration

The Iterative Closest Point (ICP) algorithm that uses the point-toplane error metric has been shown to converge much faster than one that uses the point-to-point error metric. At each iteration of the ICP algorithm, the change of relative pose that gives the minimal point-to-plane error is usually solved using standard nonlinear least-squares methods, which are often very slow. Fortunately, when the relative orientation between the two input surfaces is small, we can approximate the nonlinear optimization problem with a linear least-squares one that can be solved more efficiently. We detail the derivation of a linear system whose least-squares solution is a good approximation to that obtained from a nonlinear optimization.