当我们知道空间点在两个坐标系下的位置时,可以通过ICP来求解其相对位姿。ICP最初常用在激光SLAM中,因为激光可以获得3D位置,而不像视觉还需要经过相机的投影变换,但是从激光雷达获得的三维点云中,我们很难对两帧点云进行匹配,通常是寻找距离最近的点作为匹配点,所以称为迭代最近法(ICP)。当我们已知相机的位姿后,可以把相机坐标系下的特征点
P
i
c
P_i^c
Pic转换到世界坐标系下,这与世界坐标系下对应的特征点
P
i
w
P_i^w
Piw存在误差,即:
e
i
=
P
i
w
−
(
R
w
c
P
i
c
+
t
w
c
)
e_i=P_i^w-(R_{wc}P_i^c+t_{wc})
ei=Piw−(RwcPic+twc)
考虑所有特征点的误差,我们定义总误差为:
J
(
R
,
t
)
=
1
2
∑
i
=
1
n
∥
e
i
∥
2
2
J(R,t) =\frac 1 2\sum_{i=1}^n\|e_i\|_2^2
J(R,t)=21i=1∑n∥ei∥22
我们的目标是寻找一个最优的
R
R
R、
t
t
t,使得总误差
J
J
J最小,即:
T
∗
=
arg min
R
.
t
J
(
R
,
t
)
T^*=\argmin_{R.t}J(R,t)
T∗=R.targminJ(R,t)
这是一个非线性最小二乘问题,通常可以用非线性优化的方法进行求解,但对于ICP来说,我们还可以使用SVD分解的方式求出解析解。
SVD方法
首先我们先定义相机坐标系下和世界坐标系下所有3D特征点的质心分别为(注意质心是没有下标的):
P
c
=
1
n
∑
i
=
1
n
P
i
c
P^c=\frac 1 n \sum_{i=1}^nP_i^c
Pc=n1i=1∑nPic
P
w
=
1
n
∑
i
=
1
n
P
i
w
P^w=\frac 1 n \sum_{i=1}^nP_i^w
Pw=n1i=1∑nPiw
考虑第
i
i
i个误差项:
e
i
=
P
i
w
−
(
R
w
c
P
i
c
+
t
w
c
)
=
P
i
w
−
P
w
+
P
w
−
R
w
c
(
P
i
c
−
P
c
+
P
c
)
−
t
w
c
=
P
i
w
−
P
w
−
R
w
c
(
P
i
c
−
P
c
)
+
P
w
−
(
R
w
c
P
c
+
t
w
c
)
=
P
′
i
w
−
R
w
c
P
′
i
c
⏟
e
i
a
+
P
w
−
(
R
w
c
P
c
+
t
w
c
)
⏟
e
i
b
\begin{aligned}e_i &=P_i^w-(R_{wc}P_i^c+t_{wc}) \\ &=P_i^w-P^w+P^w-R_{wc}(P_i^c-P^c+P^c)-t_{wc} \\ &=P_i^w-P^w-R_{wc}(P_i^c-P^c)+P^w-(R_{wc}P^c+t_{wc}) \\&=\underbrace{{P'}_i^w-R_{wc}{P'}_i^c}_{e_{ia}}+\underbrace{P^w-(R_{wc}P^c+t_{wc})}_{e_{ib}} \end{aligned}
ei=Piw−(RwcPic+twc)=Piw−Pw+Pw−Rwc(Pic−Pc+Pc)−twc=Piw−Pw−Rwc(Pic−Pc)+Pw−(RwcPc+twc)=eia
P′iw−RwcP′ic+eib
Pw−(RwcPc+twc)
其中
P
′
i
w
=
P
i
w
−
P
w
{P'}_i^w=P_i^w-P^w
P′iw=Piw−Pw,
P
′
i
c
=
P
i
c
−
P
c
{P'}_i^c=P_i^c-P^c
P′ic=Pic−Pc,分别表示特征点在世界和相机坐标系下的去质心坐标。由于:
∑
i
=
1
n
e
i
a
=
∑
i
=
1
n
(
P
′
i
w
−
R
w
c
P
′
i
c
)
=
0
\sum_{i=1}^ne_{ia} =\sum_{i=1}^n({{P'}_i^w-R_{wc}{P'}_i^c})=0
i=1∑neia=i=1∑n(P′iw−RwcP′ic)=0
那么总误差可以化简为:
J
(
R
,
t
)
=
1
2
∑
i
=
1
n
∥
e
i
∥
2
2
=
1
2
∑
i
=
1
n
∥
e
i
a
∥
2
2
+
1
2
∑
i
=
1
n
∥
e
i
b
∥
2
2
=
J
a
(
R
)
+
J
b
(
R
,
t
)
J(R,t) =\frac 1 2\sum_{i=1}^n\|e_i\|_2^2=\frac 1 2\sum_{i=1}^n\|e_{ia}\|_2^2+\frac 1 2\sum_{i=1}^n\|e_{ib}\|_2^2=J_a(R)+J_b(R,t)
J(R,t)=21i=1∑n∥ei∥22=21i=1∑n∥eia∥22+21i=1∑n∥eib∥22=Ja(R)+Jb(R,t)
由于
J
a
J_a
Ja只与
R
R
R有关,与
t
t
t无关,我们可以先通过优化
J
a
J_a
Ja求得
R
R
R,再带入
J
b
(
R
,
t
)
=
0
J_b(R,t)=0
Jb(R,t)=0,求得
t
t
t。
J
a
J_a
Ja可进一步展开为:
J
a
(
R
)
=
1
2
∑
i
=
1
n
∥
P
′
i
w
−
R
w
c
P
′
i
c
∥
2
2
=
1
2
∑
i
=
1
n
(
P
′
i
w
T
P
′
i
w
+
P
′
i
c
T
R
w
c
T
R
w
c
P
′
i
c
−
2
P
′
i
w
T
R
w
c
P
′
i
c
)
J_a(R)=\frac 1 2\sum_{i=1}^n\|{P'}_i^w-R_{wc}{P'}_i^c\|_2^2=\frac 1 2\sum_{i=1}^n({{P'}_i^w}^T{P'}_i^w+{{P'}_i^c}^TR_{wc}^TR_{wc}{P'}_i^c-2{{P'}_i^w}^TR_{wc}{P'}_i^c)
Ja(R)=21i=1∑n∥P′iw−RwcP′ic∥22=21i=1∑n(P′iwTP′iw+P′icTRwcTRwcP′ic−2P′iwTRwcP′ic)
由于前两项与
R
R
R无关,所以优化目标可以简化为:
J
′
a
(
R
)
=
−
∑
i
=
1
n
P
′
i
w
T
R
w
c
P
′
i
c
=
−
∑
i
=
1
n
t
r
(
R
w
c
P
′
i
c
P
′
i
w
T
)
=
−
t
r
(
R
w
c
∑
i
=
1
n
P
′
i
c
P
′
i
w
T
)
{J'}_a(R) =-\sum_{i=1}^n {{P'}_i^w}^TR_{wc}{P'}_i^c=-\sum_{i=1}^n tr(R_{wc}{P'}_i^c{{P'}_i^w}^T)=-tr( R_{wc} \sum_{i=1}^n{P'}_i^c{{P'}_i^w}^T)
J′a(R)=−i=1∑nP′iwTRwcP′ic=−i=1∑ntr(RwcP′icP′iwT)=−tr(Rwci=1∑nP′icP′iwT)
令
W
=
∑
i
=
1
n
P
′
i
c
P
′
i
w
T
W=\sum_{i=1}^n{P'}_i^c{{P'}_i^w}^T
W=∑i=1nP′icP′iwT,对
W
W
W进行SVD分解,即
W
=
U
Σ
V
T
W=U\Sigma V^T
W=UΣVT,
Σ
\Sigma
Σ对应的奇异值从大到小排列。 根据最优性证明,当
W
W
W满秩时,最优的
R
=
U
V
T
R=UV^T
R=UVT,如果
R
R
R行列式小于0,取负作为最优值。
非线性优化方法
由于ICP存在无穷多解或者唯一解,如果我们使用迭代的方法获取局部最优解,即为全局最优解,因此对于ICP问题,我们可以任意选定初始值并进行迭代。误差的一阶导数即雅克比矩阵为:
∂
e
i
∂
R
=
−
(
R
w
c
P
i
c
)
∧
=
(
−
P
i
w
)
∧
,
∂
e
i
∂
t
=
I
\frac {\partial e_i} {\partial R}=-(R_{wc}P_i^c)^{\land}=(-P_i^w)^{\land},\frac {\partial e_i} {\partial t}=I
∂R∂ei=−(RwcPic)∧=(−Piw)∧,∂t∂ei=I