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

快来打我* 2023-02-23 05:11 32阅读 0赞

## **问题引入** ##

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

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

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

![q\_\{i\} = Rp\_\{i\} + t][q_i_ _ Rp_i_ _ t]

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

![\\frac\{1\}\{2\}\\sum\_\{i = 1\}^\{n\}\\left \\| q\_\{i\} - Rp\_\{i\} - t \\right \\|^\{2\}][frac_1_2_sum_i _ 1_n_left _ q_i_ - Rp_i_ - t _right _2]

常用的求解 ![R][] 和 ![t][] 的方法有：

1.  SVD
2.  非线性优化

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxNjg1MjY1_size_16_color_FFFFFF_t_70][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxNjg1MjY1_size_16_color_FFFFFF_t_70 1][]

## **MATLAB Example** ##

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

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

![preview][]

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

![preview][preview 1]

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

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

输出结果

![preview][preview 2]

[P _ _left _p _1_ _p _2_ _p _3_ _cdots _p _n_ _right]: https://private.codecogs.com/gif.latex?P%20%3D%20%5Cleft%20%5C%7Bp%20_%7B1%7D%20%2Cp%20_%7B2%7D%20%2Cp%20_%7B3%7D%20%2C%5Ccdots%20%2Cp%20_%7Bn%7D%20%5Cright%20%5C%7D
[Q _ _left _q _1_ _q _2_ _q _3_ _cdots _q _n_ _right]: https://private.codecogs.com/gif.latex?Q%20%3D%20%5Cleft%20%5C%7Bq%20_%7B1%7D%20%2Cq%20_%7B2%7D%20%2Cq%20_%7B3%7D%20%2C%5Ccdots%20%2Cq%20_%7Bn%7D%20%5Cright%20%5C%7D
[P]: https://private.codecogs.com/gif.latex?P
[Q]: https://private.codecogs.com/gif.latex?Q
[left _P _99_Q _99_ _right]: https://private.codecogs.com/gif.latex?%5Cleft%20%28P%20_%7B99%7D%2CQ%20_%7B99%7D%20%5Cright%20%29
[R]: https://private.codecogs.com/gif.latex?R
[t]: https://private.codecogs.com/gif.latex?t
[q_i_ _ Rp_i_ _ t]: https://private.codecogs.com/gif.latex?q_%7Bi%7D%20%3D%20Rp_%7Bi%7D%20&plus;%20t
[frac_1_2_sum_i _ 1_n_left _ q_i_ - Rp_i_ - t _right _2]: https://private.codecogs.com/gif.latex?%5Cfrac%7B1%7D%7B2%7D%5Csum_%7Bi%20%3D%201%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20q_%7Bi%7D%20-%20Rp_%7Bi%7D%20-%20t%20%5Cright%20%5C%7C%5E%7B2%7D
[mu _p_ _ _frac_1_n_ _sum_i _ 1_n_p_i]: https://private.codecogs.com/gif.latex?%5Cmu%20_%7Bp%7D%20%3D%20%5Cfrac%7B1%7D%7Bn%7D%20%5Csum_%7Bi%20%3D%201%7D%5E%7Bn%7Dp_%7Bi%7D
[mu _q_ _ _frac_1_n_ _sum_i _ 1_n_q_i]: https://private.codecogs.com/gif.latex?%5Cmu%20_%7Bq%7D%20%3D%20%5Cfrac%7B1%7D%7Bn%7D%20%5Csum_%7Bi%20%3D%201%7D%5E%7Bn%7Dq_%7Bi%7D
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxNjg1MjY1_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200705163400916.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxNjg1MjY1,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxNjg1MjY1_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200705163441973.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQxNjg1MjY1,size_16,color_FFFFFF,t_70
[ICP]: https://link.zhihu.com/?target=https%3A//www.mathworks.com/examples/matlab-computer-vision/mw/vision-ex19773063-align-two-point-clouds-using-icp-algorithm
[Computer Vision System Toolbox]: https://link.zhihu.com/?target=https%3A//ww2.mathworks.cn/products/computer-vision.html%3Fs_cid%3DME_prod_MW
[preview]: https://imgconvert.csdnimg.cn/aHR0cHM6Ly9waWMxLnpoaW1nLmNvbS92Mi1hY2ZhMzBkMmQyYThhODFjMDcxN2I2YjViY2NiYzY3Y19yLmpwZw?x-oss-process=image/format,png
[preview 1]: https://imgconvert.csdnimg.cn/aHR0cHM6Ly9waWMzLnpoaW1nLmNvbS92Mi05MTY2ZWVlMGY1NzQyM2JlYTVkNmJmYmZiYTBhZmJmYV9yLmpwZw?x-oss-process=image/format,png
[preview 2]: https://imgconvert.csdnimg.cn/aHR0cHM6Ly9waWMzLnpoaW1nLmNvbS92Mi0wZjE1MmI1NWI5OTEwZGM3ZWEwNGIyNmU3ODBjN2RhNl9yLmpwZw?x-oss-process=image/format,png