ICP的全称是 iterative closest point —— 迭代最近点。它是一种点云匹配的算法。
在三维重建或者视觉Slam场景中，经常需要确定某一时刻的相机位姿。
相机在运动过程中，不同时刻对同一物体获取的三维点云信息是不同的。
此时我们可以通过点云的对应点在两个时刻间差异来计算相机位姿的改变。
对位姿、相机模型等不了解的可以去看我的VSLAM笔记-相机模型。
简而言之，ICP目的就是为了使用某种方法求出两组点云之间的三维旋转与平移矩阵 T \boldsymbol T T
通常，我们可以使用一组空间点云数据来表示某一物体，记为
{
p
i
}
\{\boldsymbol p_i\}
{pi}
在某一时刻，该物体发生了移动与旋转，此时他的空间坐标发生了变化，也就是点云数据
{
p
i
}
\{\boldsymbol p_i\}
{pi} 的坐标全都发生了变化，新点云坐标记为
{
q
i
}
\{\boldsymbol q_i\}
{qi}
那么通过学习刚性物体的旋转平移我们可以知道，
p
i
\boldsymbol p_i
pi 和
q
i
\boldsymbol q_i
qi 之间存在一定关系：
q
i
=
R
p
i
+
t
,
i
=
1
,
2
,
…
,
n
或
q
ˉ
i
=
T
p
ˉ
i
,
i
=
1
,
2
,
…
,
n
\boldsymbol q_i = \boldsymbol R\boldsymbol p_i + \boldsymbol t~,~~~~~~~~i=1,2,\dots,n \\ 或 \\ \bar {\boldsymbol q}_i = \boldsymbol T \bar{\boldsymbol p}_i~,~~~~~~~~i=1,2,\dots,n
qi=Rpi+t , i=1,2,…,n或qˉi=Tpˉi , i=1,2,…,n
ICP算法的大致分为两步：
那么此时需要解决的两个问题就是：
从算法名 “迭代最近点” 我们不难看出，此算法解决第一个问题的基本思想是使用距离最近的点作为匹配点。
也就是遍历点云
P
\boldsymbol P
P 中的所有点，计算它们与点云
Q
\boldsymbol Q
Q 中每个点的距离，将距离最少的点作为其对应点。
例如查找
p
5
\boldsymbol p_5
p5 的对应点就是计算它与所有
q
\boldsymbol q
q 的距离，找出距离最近的那个
q
\boldsymbol q
q：
min
q
i
{
d
i
s
t
(
p
5
,
q
i
)
}
\min_{\boldsymbol q_i} \Big\{dist(\boldsymbol p_5, \boldsymbol q_i)\Big\}
qimin{dist(p5,qi)}
时间复杂度为 O ( n 2 ) O(n^2) O(n2)
找到对应点后，就可以使用一定方法来找出最优的 R , t \boldsymbol R,\ \boldsymbol t R, t
注意，这里之所以说是 “最优的”
R
,
t
\boldsymbol R,\ \boldsymbol t
R, t 值，是因为使用最近点匹配方法找出的对应点往往不是真实对应的点：
求出来的 R , t \boldsymbol R,\ \boldsymbol t R, t 自然也不会是最终真正的值。
所以这个时候我们就需要“迭代”，第一次计算得出了一组当前最优的 R , t \boldsymbol R,\ \boldsymbol t R, t 值以后，可以将点云 P \boldsymbol P P 经过 R p + t \boldsymbol R\boldsymbol p + \boldsymbol t Rp+t移动到 P ′ \boldsymbol P' P′ 的位置（就离目标点云 Q \boldsymbol Q Q 近了一些），此时，再对 新的点云 P ′ \boldsymbol P' P′ 和 点云 Q \boldsymbol Q Q 重复上面的两个步骤即可。
每次迭代都可以式 P \boldsymbol P P 更加靠近 Q \boldsymbol Q Q 直到收敛为止。
通过刚刚分析我们知道：
q
i
≠
R
p
i
+
t
\boldsymbol q_i \neq \boldsymbol R\boldsymbol p_i + \boldsymbol t
qi=Rpi+t
实际上：
q
i
=
R
p
i
+
t
+
e
i
\boldsymbol q_i = \boldsymbol R\boldsymbol p_i + \boldsymbol t + \boldsymbol e_i
qi=Rpi+t+ei
（此处是以 point-to-point 方法为例构造误差函数，其中
e
i
\boldsymbol e_i
ei 表示误差）
那么现在的任务其实就是使误差最小，即
min
R
,
t
∑
i
=
1
n
∣
q
i
−
R
p
i
−
t
∣
或者
min
R
,
t
1
2
∑
i
=
1
n
∥
q
i
−
R
p
i
−
t
∥
2
\min_{\boldsymbol R,\boldsymbol t}\sum^n_{i=1}|\boldsymbol q_i - \boldsymbol R\boldsymbol p_i - \boldsymbol t| \\ 或者 \\ \min_{\boldsymbol R,\boldsymbol t}\frac{1}{2}\sum^n_{i=1}\|\boldsymbol q_i - \boldsymbol R\boldsymbol p_i - \boldsymbol t\|^2
R,tmini=1∑n∣qi−Rpi−t∣或者R,tmin21i=1∑n∥qi−Rpi−t∥2
再使用最小二乘等方式求解
R
,
t
\boldsymbol R,\boldsymbol t
R,t 即可
这里讲的是最基本的ICP算法的操作方法
其实对于上面两个问题的处理方法还有很多。
例如
对于问题①，除了使用最近点匹配，还可以使用投影坐标作为对应点匹配，或者融合RGB图像使用特征提取的方法来匹配等等；
对于问题②，常见的ICP构造误差函数的方法有Point-to-Point, Point-to-Plane, Plane-to-Plane等。
另外还有：
等等许多问题也是很值得思考的。
ICP感觉是一个极其耗时的过程，所以按照使用场景选对每一步的方法很重要~
前几天刚看的 Kinect Fusion 使用的就是 投影坐标对应点匹配 + Point-to-Plane 方法来求解ICP的
常用的求解方法有两种：
下面以 Point-to-Point ICP为例推导求解过程
Point-to-Point 优化误差目标为：
arg
min
R
,
t
1
2
∑
i
=
1
n
∥
q
i
−
R
p
i
−
t
∥
2
\arg\min_{\boldsymbol R,\boldsymbol t}\frac{1}{2}\sum^n_{i=1}\|\boldsymbol q_i - \boldsymbol R\boldsymbol p_i - \boldsymbol t\|^2
argR,tmin21i=1∑n∥qi−Rpi−t∥2
提一句：Point-to-Plane的目标函数为 arg min R , t 1 2 ∑ i = 1 n ∥ ( q i − R p i − t ) ⋅ n i ∥ 2 \arg\displaystyle\min_{\boldsymbol R,\boldsymbol t}\frac{1}{2}\sum^n_{i=1}\|(\boldsymbol q_i - \boldsymbol R\boldsymbol p_i - \boldsymbol t)\cdot\boldsymbol n_i\|^2 argR,tmin21i=1∑n∥(qi−Rpi−t)⋅ni∥2 ，其中 n i \boldsymbol n_i ni 为法向量。有兴趣的可以自己查一下资料看看推导过程。
先求 t \boldsymbol t t
记：
e
=
1
2
∑
i
=
1
n
∥
q
i
−
R
p
i
−
t
∥
2
\boldsymbol e=\frac{1}{2}\sum^n_{i=1}\|\boldsymbol q_i - \boldsymbol R\boldsymbol p_i - \boldsymbol t\|^2
e=21i=1∑n∥qi−Rpi−t∥2
对
t
\boldsymbol t
t求导：
∂
e
∂
t
=
−
(
∑
i
=
1
n
q
i
−
∑
i
=
1
n
R
p
i
−
n
t
)
\frac{\partial\boldsymbol e}{\partial\boldsymbol t}=-\Big(\sum^n_{i=1}\boldsymbol q_i - \sum^n_{i=1}\boldsymbol R\boldsymbol p_i - n\boldsymbol t\Big)
∂t∂e=−(i=1∑nqi−i=1∑nRpi−nt)
令
∂
e
∂
t
=
0
\frac{\partial\boldsymbol e}{\partial\boldsymbol t}=0
∂t∂e=0得到：
t
^
=
1
n
∑
i
=
1
n
q
i
−
R
⋅
1
n
∑
i
=
1
n
p
i
\hat{\boldsymbol t}=\frac{1}{n}\sum^n_{i=1}\boldsymbol q_i - \boldsymbol R\cdot\frac{1}{n}\sum^n_{i=1}\boldsymbol p_i
t^=n1i=1∑nqi−R⋅n1i=1∑npi
分别记
μ
q
=
1
n
∑
i
=
1
n
q
i
,
μ
p
=
1
n
∑
i
=
1
n
p
i
\boldsymbol\mu_q=\displaystyle\frac{1}{n}\sum^n_{i=1}\boldsymbol q_i~, ~\boldsymbol\mu_p=\displaystyle\frac{1}{n}\sum^n_{i=1}\boldsymbol p_i
μq=n1i=1∑nqi , μp=n1i=1∑npi（很多地方也把它们称作点云的质心），那么上式就变为：
t
^
=
μ
q
−
R
μ
p
\hat{\boldsymbol t}=\boldsymbol\mu_q - \boldsymbol R\boldsymbol\mu_p
t^=μq−Rμp
再求 R \boldsymbol R R
将
t
^
\hat{\boldsymbol t}
t^ 带入原误差函数中得：
e
=
1
2
∑
i
=
1
n
∥
q
i
−
R
p
i
−
(
μ
q
−
R
μ
p
)
∥
2
=
1
2
∑
i
=
1
n
∥
q
i
−
μ
q
−
R
(
p
i
−
μ
p
)
∥
2
\begin{aligned} \boldsymbol e&=\frac{1}{2}\sum^n_{i=1}\|\boldsymbol q_i - \boldsymbol R\boldsymbol p_i - (\boldsymbol\mu_q - \boldsymbol R\boldsymbol\mu_p)\|^2 \\ &=\frac{1}{2}\sum^n_{i=1}\| \textcolor{#0033CC}{\boldsymbol q_i - \boldsymbol\mu_q} - \boldsymbol R( \textcolor{#BB0000}{\boldsymbol p_i - \boldsymbol\mu_p})\|^2 \end{aligned}
e=21i=1∑n∥qi−Rpi−(μq−Rμp)∥2=21i=1∑n∥qi−μq−R(pi−μp)∥2
再分别记
x
i
=
q
i
−
μ
q
,
y
i
=
p
i
−
μ
p
\boldsymbol x_i=\textcolor{#0033CC}{\boldsymbol q_i - \boldsymbol\mu_q} ~, ~\boldsymbol y_i=\textcolor{#BB0000}{\boldsymbol p_i - \boldsymbol\mu_p}
xi=qi−μq , yi=pi−μp （相当于是每个点与其质心的距离，去中心化的操作） 得：
e
=
1
2
∑
i
=
1
n
∥
x
i
−
R
y
i
∥
2
\boldsymbol e=\frac{1}{2}\sum^n_{i=1}\| \boldsymbol x_i - \boldsymbol R\boldsymbol y_i\|^2
e=21i=1∑n∥xi−Ryi∥2
所以：
R
^
=
arg
min
R
∑
i
=
1
n
∥
x
i
−
R
y
i
∥
2
=
arg
min
R
∑
i
=
1
n
(
x
i
−
R
y
i
)
T
(
x
i
−
R
y
i
)
=
arg
min
R
∑
i
=
1
n
(
x
i
T
x
i
−
x
i
T
R
y
i
−
y
i
T
R
T
x
i
−
y
i
T
R
T
R
y
i
)
=
arg
min
R
∑
i
=
1
n
(
x
i
T
x
i
−
2
x
i
T
R
y
i
−
y
i
T
y
i
)
=
arg
max
R
∑
i
=
1
n
x
i
T
R
y
i
=
arg
max
R
t
r
(
∑
i
=
1
n
x
i
T
R
y
i
)
=
arg
max
R
t
r
(
∑
i
=
1
n
R
y
i
x
i
T
)
=
arg
max
R
t
r
(
R
∑
i
=
1
n
y
i
x
i
T
)
\begin{aligned} \hat{\boldsymbol R}&=\arg\min_{\boldsymbol R}\sum^n_{i=1}\| \boldsymbol x_i - \boldsymbol R\boldsymbol y_i\|^2 \\ &=\arg\min_{\boldsymbol R}\sum^n_{i=1} ( \boldsymbol x_i - \boldsymbol R\boldsymbol y_i) ^\mathrm T( \boldsymbol x_i - \boldsymbol R\boldsymbol y_i) \\ &=\arg\min_{\boldsymbol R}\sum^n_{i=1} ( \boldsymbol x_i^\mathrm T\boldsymbol x_i - \boldsymbol x_i^\mathrm T\boldsymbol R\boldsymbol y_i - \boldsymbol y_i^\mathrm T\boldsymbol R^\mathrm T\boldsymbol x_i - \boldsymbol y_i^\mathrm T\boldsymbol R^\mathrm T\boldsymbol R\boldsymbol y_i) \\ &=\arg\min_{\boldsymbol R}\sum^n_{i=1} ( \boldsymbol x_i^\mathrm T\boldsymbol x_i - 2\boldsymbol x_i^\mathrm T\boldsymbol R\boldsymbol y_i - \boldsymbol y_i^\mathrm T\boldsymbol y_i) \\ &=\arg\max_{\boldsymbol R}\sum^n_{i=1}\boldsymbol x_i^\mathrm T\boldsymbol R\boldsymbol y_i \\ &=\arg\max_{\boldsymbol R}tr\Big(\sum^n_{i=1}\boldsymbol x_i^\mathrm T\boldsymbol R\boldsymbol y_i\Big) \\ &=\arg\max_{\boldsymbol R}tr\Big(\sum^n_{i=1}\boldsymbol R\boldsymbol y_i\boldsymbol x_i^\mathrm T\Big) \\ &=\arg\max_{\boldsymbol R}tr\Big(\boldsymbol R\sum^n_{i=1}\boldsymbol y_i\boldsymbol x_i^\mathrm T\Big) \end{aligned}
R^=argRmini=1∑n∥xi−Ryi∥2=argRmini=1∑n(xi−Ryi)T(xi−Ryi)=argRmini=1∑n(xiTxi−xiTRyi−yiTRTxi−yiTRTRyi)=argRmini=1∑n(xiTxi−2xiTRyi−yiTyi)=argRmaxi=1∑nxiTRyi=argRmaxtr(i=1∑nxiTRyi)=argRmaxtr(i=1∑nRyixiT)=argRmaxtr(Ri=1∑nyixiT)
上面的
t
r
tr
tr 表示矩阵的迹
最后这个
y
i
x
i
T
\boldsymbol y_i\boldsymbol x_i^\mathrm T
yixiT实际上是个协方差矩阵
记
H
=
∑
i
=
1
n
y
i
x
i
T
\boldsymbol H=\displaystyle\sum^n_{i=1}\boldsymbol y_i\boldsymbol x_i^\mathrm T
H=i=1∑nyixiT ，有：
R
^
=
arg
max
R
t
r
(
R
H
)
\hat{\boldsymbol R}=\arg\max_{\boldsymbol R}tr\Big(\boldsymbol R\boldsymbol H\Big)
R^=argRmaxtr(RH)
现在对
H
\boldsymbol H
H进行SVD分解可得：
H
=
U
Σ
V
T
\boldsymbol H=\boldsymbol U\boldsymbol\Sigma\boldsymbol V^\mathrm T
H=UΣVT
则：
R
^
=
arg
max
R
t
r
(
R
U
Σ
V
T
)
=
arg
max
R
t
r
(
Σ
V
T
R
U
)
\begin{aligned} \hat{\boldsymbol R}&=\arg\max_{\boldsymbol R}tr\Big(\boldsymbol R\boldsymbol U\boldsymbol\Sigma\boldsymbol V^\mathrm T\Big) \\ &=\arg\max_{\boldsymbol R}tr\Big(\boldsymbol\Sigma\boldsymbol V^\mathrm T\boldsymbol R\boldsymbol U\Big) \end{aligned}
R^=argRmaxtr(RUΣVT)=argRmaxtr(ΣVTRU)
记
M
=
V
T
R
U
\boldsymbol M=\boldsymbol V^\mathrm T\boldsymbol R\boldsymbol U
M=VTRU，又
Σ
\boldsymbol\Sigma
Σ 是对角阵
那么：
R
^
=
arg
max
R
t
r
(
Σ
M
)
=
arg
max
R
t
r
(
∑
i
=
1
n
σ
i
m
i
i
)
\begin{aligned} \hat{\boldsymbol R}&=\arg\max_{\boldsymbol R}tr\Big(\boldsymbol\Sigma\boldsymbol M\Big) \\ &=\arg\max_{\boldsymbol R}tr\Big(\sum^n_{i=1}\sigma_i m_{ii}\Big) \end{aligned}
R^=argRmaxtr(ΣM)=argRmaxtr(i=1∑nσimii)
因为
V
T
,
R
,
U
\boldsymbol V^\mathrm T,\boldsymbol R,\boldsymbol U
VT,R,U 均为正交矩阵，所以
M
\boldsymbol M
M是正交矩阵，则
m
i
j
≤
1
m_{ij}\le1
mij≤1
故当
m
i
i
=
1
m_{ii}=1
mii=1 时，
∑
i
=
1
n
σ
i
m
i
i
\displaystyle\sum^n_{i=1}\sigma_i m_{ii}
i=1∑nσimii取到最大值。
而
M
\boldsymbol M
M 是正交矩阵，所以
M
=
I
\boldsymbol M=\boldsymbol I
M=I
即
V
T
R
U
=
I
\boldsymbol V^\mathrm T\boldsymbol R\boldsymbol U=\boldsymbol I
VTRU=I
所以
R
^
=
V
U
T
\hat{\boldsymbol R}=\boldsymbol V\boldsymbol U^\mathrm T
R^=VUT
至此，求得最优的 R \boldsymbol R R 与 t \boldsymbol t t
此方法比较复杂，可以看我的VSLAM笔记-BundleAdjustment。
懒得写参考资料了，好麻烦 >.>
