SLAM前端：ICP（Iterative Closest Point）

最新推荐文章于 2022-11-15 18:42:31 发布

LittleEmperor

最新推荐文章于 2022-11-15 18:42:31 发布

阅读量1.2k

点赞数

分类专栏： SLAM

本文链接：https://blog.csdn.net/LittleEmperor/article/details/105259932

版权

SLAM 专栏收录该内容

14 篇文章 6 订阅

订阅专栏

ICP算法是点云配准算法，给出两组点云，可以计算出两组点云的位姿关系R、t。问题数学表达为：

已知两组点云：

$X(x_1,x_2,x_3,...,x_n) ,Y(y_1,y_2,y_3,...,y_n)$

求R、t使下式最小：

$min E(R,t)= min \frac{1}{n}\sum^{n}_{i=1}\left \| x_i - (Ry_i+t) \right \|^2$ （0-1）

1 如果两组点云中的点一一对应，则求解方法很简单。

（1）求解两组点云的质心：

$\\ x_c = \frac{1}{n}\sum_{i=1}^n\left \| x_i \right \|^2\\ y_c = \frac{1}{n}\sum_{i=1}^n\left \| y_i \right \|^2$

（2）求两组点云去质心坐标：

$\\ X(x_1,x_2,x_3,...,x_n)-x_c = X^{'}(x_1^{'},x_2^{'},x_3^{'}...x_n^{'}) \\ Y(y_1,y_2,y_3,...,y_n)-y_c = Y^{'}(y_1^{'},y_2^{'},y_3^{'}...y_n^{'})$

（3）每点组成矩阵求均值、SVD求解：

$\frac{1}{n}\sum_{i=1}^n(x_i^{'}\cdot y_i^{'T})=U\cdot diag(\sigma _1,\sigma_2,\sigma_3)\cdot V^T$

（4）R、t结果如下：

$R = VU^T,t = x_c - Ry_c$

2 一般的情况是点云不相匹配，如何求解R、t。

（1）对X中的每个店，在Y中找到距离X最近的点，一一对应

（2）筛选距离最近的若干点

（3）代入公式（0-1）最优化求解得到R、t

（4）利用求解得到的R、t，重新在Y中找距离X最近的点，重复（1）-（4）直到 $\frac{1}{n}\sum^{n}_{i=1}(x_i - (Ry_i+t))$ 小于一定阈值。

ICP求解很耗时

3 公式证明

最小化方程转化：

$\\ E(R,t)= \frac{1}{n}\sum^{n}_{i=1}\left \| x_i - (Ry_i+t) \right \|^2= \frac{1}{n}\sum^{n}_{i=1}\left \| x_i - (Ry_i+t) +x_c -x_c + Ry_c-Ry_c\right \|^2\\ = \frac{1}{n}\sum^{n}_{i=1}\left \| x_i - x_c -R(y_i-y_c) +x_c-Ry_c-t\right \|^2\\$

开平方化简得到：

$E(R,t)= \frac{1}{n}\sum^{n}_{i=1}\left \| x_i - x_c -R(y_i-y_c) \right \|^2+\left \| x_c-Ry_c-t \right \|^2$

可以分为两部分，前半部分 $\frac{1}{n}\sum^{n}_{i=1}\left \| x_i - x_c -R(y_i-y_c) \right \|^2$ 仅与R有关，求解 $min E(R,t)$ 可以求解 $min\frac{1}{n}\sum^{n}_{i=1}\left \| x_i - x_c -R(y_i-y_c) \right \|^2$ ，得到R后根据后半部分 $x_c-Ry_c-t=0$ 求解得到t。因此得到：

$E^{'}(R,t)= \frac{1}{n}\sum^{n}_{i=1}\left \| x_i - x_c -R(y_i-y_c) \right \|^2$

开平方得到：

$E^{'}(R,t)= \frac{1}{n}\sum^{n}_{i=1}\left \| x_i^{'} -Ry_i^{'} \right \|^2=\frac{1}{n}\sum^{n}_{i=1} (x_i^{'T}x_i^{'}+p_i^{'T}R^TRp_i^{'}-2x_i^{'T}Rp_i^{'})$

第一项与R无关，第二项由于 $R^TR=I$ 与R无关，因此求 $minE^{'}(R,t)$ 即求 $min\frac{1}{n}\sum^{n}_{i=1} (-2x_i^{'T}Rp_i^{'}) \rightarrow max\frac{1}{n}\sum^{n}_{i=1} (x_i^{'T}Rp_i^{'})$