Home

Iterative Closest Point（ICP）匹配算法

算法功能简介

• ICP（Iterative Closest Point，迭代最近点）算法是一种点集-点集的匹配方法，基于最小二乘法，计算一个匹配变换以使得点集与点集尽量重叠。

• ICP算法的输入为两帧待匹配的点云，数据格式为[x, y, z]，即点云的三维坐标。

• 重要的接口函数：

  • 【输入】void pcl::IterativeClosestPoint::setInputCloud(const PointCloudSourceConstPtr& cloud)

    设置源点云（被变换以进行匹配的点云），在匹配过程中不断迭代变换位置。需要的数据格式为[x, y, z]，即点云的三维坐标。

  • 【输入】void pcl::IterativeClosestPoint::setInputTarget(const PointCloudTargetConstPtr& cloud)

    设置目标点云（通常是地图点云），在匹配过程中保持不变。需要的数据格式为[x, y, z]，即点云的三维坐标。

  • 【输出】void pcl::IterativeClosestPoint::align(PointCloudSource& output)

    进行ICP匹配，参数返回匹配后的源点云。产生的数据格式为[x, y, z]，即点云的三维坐标。

    

  • 【可选输入】void setMaximumIterations(int nr_iterations)

    设置最大迭代次数。

  • 【可选输入】void setMaxCorrespondenceDistance(double distance_threshold)

    设置搜寻最近点时的搜寻范围半径。

  • 【可选输入】void setTransformationEpsilon(double epsilon)

    设置判断收敛的迭代步长阈值。

  • 【可选输入】void setEuclideanFitnessEpsilon(double epsilon)

    设置判断收敛的误差精度阈值。

  • 【可选输出】double getFitnessScore(double max_range = std::numeric_limits<double>::max());

    函数返回两帧点云匹配的均方误差值。

  • 【可选输出】Matrix4 getFinalTransformation()

    函数返回两帧点云匹配使用的4*4变换矩阵。

算法计算流程

todo：KD树的建立算法流程。

其中，计算变换矩阵的具体数学推导过程如下。

关于矩阵求解数学推导：https://zhuanlan.zhihu.com/p/107218828

两组对应的点集:

求欧式变换 使得：

ICP 算法基于最小二乘法进行迭代计算，使得误差平方和达到极小值：

（1）求解 translation

对 求偏导，可得：

令 ，求得：

定义质心：

因而，

将 代入 可得：

定义去质心坐标：

因此，目标函数变为：

（2）求解 rotation

是标量，对于任意标量 ，都满足 ，因此，

将其代入目标函数，可得：

因而，

由上图可知，

其中， 和 为 维矩阵。

定义协方差矩阵 ，对 做 SVD 分解：

因而，求解问题变为使下式最大化：

令 ，由于满足MM^T = E，故M是正交阵，其列向量 是 orthonormal vectors，即 。因此， 对 的所有元素都有 。

当 时， 最大； 又是正交阵，因此 必为单位阵。

（3）SVD计算方法

1. 计算 SS^T 和 S^TS；
2. 分别计算 SS^T 和 S^TS 的特征向量及其特征值；
3. SS^T 的特征向量组成 U；而 S^TS 的特征向量组成 V；
4. 对 SS^T 和 S^TS 的非零特征值求平方根，对应上述特征向量的位置，填入 Σ 的对角元。

样例效果展示

白色代表目标点云（通常为地图点云）。

红色代表本次迭代前的源点云，绿线代表源点云及其在目标点云中找到的对应点。

蓝色代表本次迭代后的源点云，作为下次迭代前的输入。