Open3D 点到面的ICP配准算法

一、算法原理

1、算法概述

  点到平面度量通常使用标准非线性最小二乘法来求解，例如Levenberg-Marquardt。点到平面ICP算法的每次迭代通常比点到点算法慢，但收敛速度明显更快。两个点云之间的相对旋转小于30°，在旋转矩阵中用θ替换sin θ，用1替换cos θ实现用线性逼近非线性最小二乘优化问题，加快计算速度。
  为了提高计算的数值稳定性，需要使用与旋转角度大小相当的距离。最简单的方法是重新缩放和移动两个输入点云，使它们在以原点为中心的单位球体或立方体内有界。[1]

2、点到平面ICP精配准

  传统的ICP算法采用最小化源点云与目标点云对应点间距离作为配准准则，如图(a)所示。而点到平面ICP采用最小化源点云中的点到目标点云对应点所在平面的距离作为配准准则
E ( T )