SLAM前端：ICP（Iterative Closest Point）

原創

共和国之辉

2020-07-07 14:29

ICP算法是點雲配准算法，給出兩組點雲，可以計算出兩組點雲的位姿關係R、t。問題數學表達爲：

已知兩組點雲：

求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結果如下：

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\frac{1}{n}\sum^{n}_{i=1}\left \| x_i - x_c -R(y_i-y_c) \right \|^2$ ，得到R後根據後半部分求解得到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無關，因此求 $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^{'})$