ICP算法详解——我见过最清晰的解释

最新推荐文章于 2024-04-07 20:59:49 发布

负壹

最新推荐文章于 2024-04-07 20:59:49 发布

阅读量4.4w

点赞数 54

分类专栏： PCL 文章标签： pcl icp 算法 slam 计算机视觉

原文链接：https://zhuanlan.zhihu.com/p/35893884

版权

PCL 专栏收录该内容

10 篇文章 15 订阅

订阅专栏

问题引入

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

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

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

$q_{i} = Rp_{i} + t$

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

$\frac{1}{2}\sum_{i = 1}^{n}\left \| q_{i} - Rp_{i} - t \right \|^{2}$

常用的求解 $R$ 和 $t$ 的方法有：

SVD
非线性优化

非线性优化的描述比较繁琐，下面只介绍SVD方法。为了后面能使用SVD，我们需要在数学上做一点小小的变形。首先定义两组点云的质心（center of mass）为 $\mu _{p} = \frac{1}{n} \sum_{i = 1}^{n}p_{i}$ , $\mu _{q} = \frac{1}{n} \sum_{i = 1}^{n}q_{i}$ ,并作出如下处理：

MATLAB Example

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

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

preview

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

preview

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

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

输出结果

preview

负壹

关注

54
点赞
踩
493

收藏

觉得还不错? 一键收藏
21
评论
ICP算法详解——我见过最清晰的解释

问题引入迭代最近点（Iterative Closest Point, 下简称ICP）算法是一种点云匹配算法。假设我们通过RGB-D相机得到了第一组点云，相机经过位姿变换（旋转加平移）后又拍摄了第二组点云,注意这里的和的坐标分别对应移动前和移动后的坐标系（即坐标原点始终为相机光心，这里我们有移动前、移动后两个坐标系），并且我们通过相关算法筛选和调整了点云存储的顺序，使得和中的点一一对应，如在三维空间中对应同一个点。现在我们要解决的问题...
复制链接

扫一扫