问题引入

        迭代最近点（Iterative Closest Point, 下简称ICP）算法是一种点云匹配算法。

        假设我们通过RGB-D相机得到了第一组点云 ，相机经过位姿变换（旋转加平移）后又拍摄了第二组点云 ,注意这里的  和  的坐标分别对应移动前和移动后的坐标系（即坐标原点始终为相机光心，这里我们有移动前、移动后两个坐标系），并且我们通过相关算法筛选和调整了点云存储的顺序，使得 和 中的点一一对应，如在三维空间中对应同一个点。

        现在我们要解决的问题是：计算相机的旋转   和平移  ，在没有误差的情况下，从 坐标系转换到 的公式为：

        但由于噪声及错误匹配（如  其实并不对应空间中同一点，但特征匹配算法错误地认为二者是同一点）的存在， 上式不总是成立，所以我们要最小化的目标函数为

        常用的求解  和  的方法有：

1. SVD
2. 非线性优化

        非线性优化的描述比较繁琐，下面只介绍SVD方法。为了后面能使用SVD，我们需要在数学上做一点小小的变形。首先定义两组点云的质心（center of mass）为 ,,并作出如下处理：

MATLAB Example

MATLAB R2018a提供了ICP例程，需要安装Computer Vision System Toolbox，我们来简单看一下这个例子。

首先，加载了原始的点云图

对点云进行旋转30度、向xyz分别平移5、5、10后得到变换后的点云图

由于每个点在三位矩阵中的下标并没有发生变化，只是每个点的坐标发生了变化，满足我们之前所要求的点云已经配准前提。调用ICP函数

tform = pcregistericp(ptCloudTformed,ptCloud,'Extrapolate',true);复制

输出结果