介绍
本文介绍了相同尺度的点云ICP算法,若点云之间尺度不同,则需要估计尺度,可以看一下另一篇博文《通过ICP配准两组尺度不同点云, 统一坐标系(附代码)》
本文介绍的ICP问题,是在3D点已配对情况下的ICP,并且,3D点坐标的尺度相同。如果两个点云的尺度不同,需要另外考虑。可以搜索关键字"scale of ICP"。
视觉中的ICP问题描述如下,假设我们有一组配对好的3D点:
P
=
{
p
1
,
.
.
.
,
p
n
}
P
′
=
{
p
1
′
,
.
.
.
,
p
n
′
}
P = \{p_1, ... , p_n\} \\ P' = \{p'_1, ... , p'_n\}
P={p1,...,pn}P′={p1′,...,pn′}
现在,我们想找一个欧式变换 R, t,使得:
p
i
′
=
R
p
i
+
t
,
对
任
意
i
成
立
p'_i = Rp_i+t, 对任意i成立
pi′=Rpi+t,对任意i成立
这个问题的一种解法(SVD分解的方法):解耦R和t,先求出R,再代入求得t。
基于SVD的ICP算法
参考论文:K. S. Arun, T. S. Huang and S. D. Blostein, “Least-Squares Fitting of Two 3-D Point Sets,” in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. PAMI-9, no. 5, pp. 698-700, Sept. 1987, doi: 10.1109/TPAMI.1987.4767965.
这是1987年提出的方法。
误差函数的构建
首先,假设有 N 组 3D-3D 匹配点对,存在误差
e
i
e_i
ei ,则有
e
i
=
p
i
′
−
(
R
p
i
+
t
)
e_i = p'_i - (Rp_i+t)
ei=pi′−(Rpi+t)
构建最小二乘问题,求使误差平方和
J
J
J 达到极小的R和t
m
i
n
R
,
t
J
=
1
2
∑
i
=
1
N
∣
∣
e
i
∣
∣
2
2
=
1
2
∑
i
=
1
N
∣
∣
p
i
′
−
(
R
p
i
+
t
)
∣
∣
2
2
\underset{R,t}{min}J = \frac{1}{2}\sum_{i=1}^{N}||e_i||_2^2 = \frac{1}{2}\sum_{i=1}^{N}||p'_i - (Rp_i+t)||_2^2
R,tminJ=21i=1∑N∣∣ei∣∣22=21i=1∑N∣∣pi′−(Rpi+t)∣∣22
**注意:**与平常的单应矩阵、基本矩阵的配对点不同,在3D-3D配对点中,匹配的那一对3D点,其实是空间中同一个点在不同坐标系下的不同表示而已,如下图:
解耦R和t
首先,引入质心的定义:假设有N个点,每个点的坐标为
p
i
p_i
pi ,则质心为
p
=
1
N
∑
i
=
1
N
p
i
\boldsymbol{p} = \frac{1}{N}\sum_{i=1}^{N}\boldsymbol{p_i}
p=N1i=1∑Npi
角度一:从几何的角度理解
首先要明确一点:在3D-3D配对点中,匹配的那一对3D点,其实是空间中 同一个点 在不同坐标系下的不同表示而已。
我们可以进行一个坐标变换,移动两个点集的坐标轴,使得两个坐标系的原点重合于各自的质心:
q
i
=
p
i
−
p
q
i
′
=
p
i
′
−
p
′
q_i = p_i - p \\ q'_i = p'_i - p'
qi=pi−pqi′=pi′−p′
其中,
p
p
p 和
p
′
p'
p′ 是各自坐标系下的质心:
p
=
1
N
∑
i
=
1
N
p
i
p
′
=
1
N
∑
i
=
1
N
p
i
′
\boldsymbol{p} = \frac{1}{N}\sum_{i=1}^{N}\boldsymbol{p_i} \\ \boldsymbol{p'} = \frac{1}{N}\sum_{i=1}^{N}\boldsymbol{p'_i}
p=N1i=1∑Npip′=N1i=1∑Npi′
注意到这两个质心在空间中其实是同一个点。所以坐标变换后的两个点集坐标系原点重合,平移分量t为0,只存在旋转分量。因此误差平方和
J
J
J 变为
m
i
n
R
J
=
1
2
∑
i
=
1
N
∣
∣
q
i
′
−
R
q
i
∣
∣
2
2
\underset{R}{min}J = \frac{1}{2}\sum_{i=1}^{N}||q'_i - Rq_i||_2^2
RminJ=21i=1∑N∣∣qi′−Rqi∣∣22
这样就实现了R和t的解耦,可以单独对R进行求解(求解R的方法后面再说,先假设已经求得到)。
那么求出R后,怎么求t呢?假设我们所要求得最小二乘解为
R
^
,
t
^
\hat{R}, \hat{t}
R^,t^ ,记
p
i
′
′
=
R
^
p
i
+
t
^
p''_i = \hat{R}p_i+\hat{t}
pi′′=R^pi+t^ ,那么
p
i
′
p'_i
pi′ 和
p
i
′
′
p''_i
pi′′ 将有相同的质心,即
p
′
=
p
′
′
p'=p''
p′=p′′
其中,
p
′
′
=
1
N
∑
i
=
1
N
p
i
′
′
=
R
^
p
+
t
^
p''= \frac{1}{N}\sum_{i=1}^{N}\boldsymbol{p''_i} = \hat{R}p+\hat{t} \\
p′′=N1i=1∑Npi′′=R^p+t^
所以可以求出t
t
^
=
p
′
′
−
R
^
p
=
p
′
−
R
^
p
\hat{t} = p'' - \hat{R}p = p' - \hat{R}p
t^=p′′−R^p=p′−R^p
角度二:从数学的角度理解
根据质心的定义,对误差函数进行变换:
m
i
n
R
,
t
J
=
1
2
∑
i
=
1
N
∣
∣
p
i
′
−
(
R
p
i
+
t
)
∣
∣
2
2
=
1
2
∑
i
=
1
N
∣
∣
(
p
i
′
−
p
′
−
R
(
p
i
−
p
)
)
+
(
p
′
−
R
p
−
t
)
∣
∣
2
2
=
1
2
∑
i
=
1
N
(
∣
∣
p
i
′
−
p
′
−
R
(
p
i
−
p
)
∣
∣
2
2
+
∣
∣
p
′
−
R
p
−
t
∣
∣
2
2
+
2
(
p
i
′
−
p
′
−
R
(
p
i
−
p
)
)
T
(
p
′
−
R
p
−
t
)
)
\begin{aligned} \underset{R,t}{min}J &= \frac{1}{2}\sum_{i=1}^{N}||p'_i - (Rp_i+t)||_2^2 \\ &= \frac{1}{2}\sum_{i=1}^{N}||(p'_i - p' - R(p_i - p)) + (p'-Rp-t)||_2^2 \\ &= \frac{1}{2}\sum_{i=1}^{N}(||p'_i - p' - R(p_i - p)||_2^2 + ||p'-Rp-t||_2^2 + 2(p'_i - p' - R(p_i - p))^T(p'-Rp-t)) \\ \end{aligned}
R,tminJ=21i=1∑N∣∣pi′−(Rpi+t)∣∣22=21i=1∑N∣∣(pi′−p′−R(pi−p))+(p′−Rp−t)∣∣22=21i=1∑N(∣∣pi′−p′−R(pi−p)∣∣22+∣∣p′−Rp−t∣∣22+2(pi′−p′−R(pi−p))T(p′−Rp−t))
注意到交叉相部分中
(
p
i
′
−
p
′
−
R
(
p
i
−
p
)
)
(p'_i - p' - R(p_i - p))
(pi′−p′−R(pi−p)) 在求和之后为零,因此优化目标函数可以简化为
m
i
n
R
,
t
J
=
1
2
∑
i
=
1
N
(
∣
∣
p
i
′
−
p
′
−
R
(
p
i
−
p
)
∣
∣
2
2
+
∣
∣
p
′
−
R
p
−
t
∣
∣
2
2
)
=
1
2
∑
i
=
1
N
(
∣
∣
q
i
′
−
R
q
i
∣
∣
2
2
+
∣
∣
p
′
−
R
p
−
t
∣
∣
2
2
)
\begin{aligned} \underset{R,t}{min}J &= \frac{1}{2}\sum_{i=1}^{N}(||p'_i - p' - R(p_i - p)||_2^2 + ||p'-Rp-t||_2^2) \\ &= \frac{1}{2}\sum_{i=1}^{N}(||q'_i - Rq_i||_2^2 + ||p'-Rp-t||_2^2) \\ \end{aligned}
R,tminJ=21i=1∑N(∣∣pi′−p′−R(pi−p)∣∣22+∣∣p′−Rp−t∣∣22)=21i=1∑N(∣∣qi′−Rqi∣∣22+∣∣p′−Rp−t∣∣22)
观察等式右边的左右两项,左边只和旋转矩阵R有关(解耦了t),而右边既有R也有t。我们先通过左边求解R,再代入右边的部分,另右边部分为零就能得到t。
求解R
前面介绍了怎么解耦R和t,解耦后,只要求得R,就能代入式子直接求得t。下面叙述一下R如何求解,以及为什么可以这么求。
思路是,通过只有R的误差项求解出R。首先展开只有R的误差项:
1
2
∑
i
=
1
N
∣
∣
q
i
′
−
R
q
i
∣
∣
2
2
=
1
2
∑
i
=
1
N
(
q
i
′
T
q
i
+
q
i
T
R
T
R
q
i
−
2
q
i
′
T
R
q
i
)
=
1
2
∑
i
=
1
N
(
q
i
′
T
q
i
+
q
i
T
q
i
−
2
q
i
′
T
R
q
i
)
\begin{aligned} \frac{1}{2}\sum_{i=1}^{N}||q'_i - Rq_i||_2^2 &= \frac{1}{2}\sum_{i=1}^{N}(q'^T_iq_i + q^T_iR^TRq_i - 2q'^T_iRq_i) \\ &= \frac{1}{2}\sum_{i=1}^{N}(q'^T_iq_i + q^T_iq_i - 2q'^T_iRq_i) \end{aligned}
21i=1∑N∣∣qi′−Rqi∣∣22=21i=1∑N(qi′Tqi+qiTRTRqi−2qi′TRqi)=21i=1∑N(qi′Tqi+qiTqi−2qi′TRqi)
注意到前两项都与R无关,因此实际上优化函数变为:
∑
i
=
1
N
−
q
i
′
T
R
q
i
\sum_{i=1}^{N}-q'^T_iRq_i
i=1∑N−qi′TRqi
注意到上面式子结果是个1x1的标量的和,所以标量再套个迹的运算,也不影响结果:
∑
i
=
1
N
−
q
i
′
T
R
q
i
=
∑
i
=
1
N
t
r
(
−
q
i
′
T
R
q
i
)
=
−
∑
i
=
1
N
t
r
(
q
i
′
T
(
R
q
i
)
)
=
−
∑
i
=
1
N
t
r
(
(
R
q
i
)
q
i
′
T
)
=
−
∑
i
=
1
N
t
r
(
R
(
q
i
q
i
′
T
)
)
=
−
t
r
(
R
∑
i
=
1
N
q
i
q
i
′
T
)
\begin{aligned} \sum_{i=1}^{N}-q'^T_iRq_i &= \sum_{i=1}^{N}tr(-q'^T_iRq_i) \\ &= -\sum_{i=1}^{N}tr(q'^T_i(Rq_i)) \\ &= -\sum_{i=1}^{N}tr((Rq_i)q'^T_i) \\ &= -\sum_{i=1}^{N}tr(R(q_iq'^T_i)) \\ &= -tr(R\sum_{i=1}^{N}q_iq'^T_i) \\ \end{aligned}
i=1∑N−qi′TRqi=i=1∑Ntr(−qi′TRqi)=−i=1∑Ntr(qi′T(Rqi))=−i=1∑Ntr((Rqi)qi′T)=−i=1∑Ntr(R(qiqi′T))=−tr(Ri=1∑Nqiqi′T)
上面的推导用到了迹的性质,详情看附录。
为了求解R,定义矩阵W:
W
=
∑
i
=
1
N
q
i
q
i
′
T
W = \sum_{i=1}^{N}q_iq'^T_i
W=i=1∑Nqiqi′T
W是一个3x3的矩阵,对其进行SVD分解,有
W
=
U
D
V
T
W = UDV^T
W=UDVT
令
X
=
V
U
T
X = VU^T
X=VUT ,这里不考虑它是怎么想到的,但可以证明它是使得误差函数最小的最小二乘解。
证明 X = V U T X = VU^T X=VUT 是最小二乘解
将
R
=
X
=
V
U
T
R = X = VU^T
R=X=VUT 代入误差函数,取代R那一项,有
−
t
r
(
R
∑
i
=
1
N
q
i
q
i
′
T
)
=
−
t
r
(
R
W
)
=
−
t
r
(
X
W
)
=
−
t
r
(
V
U
T
U
D
V
T
)
\begin{aligned} -tr(R\sum_{i=1}^{N}q_iq'^T_i) &= -tr(RW) \\ &= -tr(XW) \\ &= -tr(VU^TUDV^T) \\ \end{aligned}
−tr(Ri=1∑Nqiqi′T)=−tr(RW)=−tr(XW)=−tr(VUTUDVT)
由于V和U都是正交矩阵,所以
U
T
U
=
E
U^TU = E
UTU=E ,则上式变为
−
t
r
(
V
D
V
T
)
-tr(VDV^T)
−tr(VDVT)
因为
V
D
V
T
VDV^T
VDVT 是一个对称正定矩阵,所以可以推断
X
W
XW
XW 是一个对称正定矩阵。
由附录中的辅助定理1,对于任意正交矩阵B,和对称正定矩阵
X
W
XW
XW ,我们有
t
r
(
X
W
)
≥
t
r
(
B
X
W
)
tr(XW) \ge tr(BXW)
tr(XW)≥tr(BXW)
所以取
X
=
V
U
T
X = VU^T
X=VUT 时,
t
r
(
B
X
W
)
tr(BXW)
tr(BXW) 取得最大值,也就是
−
t
r
(
B
X
W
)
-tr(BXW)
−tr(BXW) 取得最小值。但是,
−
t
r
(
B
X
W
)
-tr(BXW)
−tr(BXW) 取得最小值,与我们要求的误差函数
−
t
r
(
R
W
)
-tr(RW)
−tr(RW) 取最小值有什么关系呢?
可以看一下,B和X都是正交矩阵,而B是一个任意的正交矩阵,所以BX可以表示任意的正交矩阵。这表明了我们在所有正交矩阵中,找到了一个解 V U T VU^T VUT 使得误差函数最小,那这个解也能使得 − t r ( R W ) -tr(RW) −tr(RW) 取最小值。我们只需要进一步判断这个解是不是旋转矩阵即可。
判断X是否为所需的解
上面已经说了, X 是最小二乘解,它使得误差函数最小。但是由于噪声和匹配点共线的原因,X 不一定是一个旋转矩阵,也就是说不一定是我们需要的解。
接下来分析其什么时候 X 是我们所需的解,什么时候不是,如果不是的时候,能不能通过它找到我们所需的解(我们需要的解必须要是一个旋转矩阵)。简要的说明如下,如果需要详细说明请看论文:
-
q
i
q_i
qi 不共面。 此时另误差函数取最小值的X,必定是旋转矩阵。
- **特殊情况: q i q_i qi 不共面,但 d e t ( x ) = − 1 det(x)=-1 det(x)=−1 ,即得到的解是个反射矩阵。**这种情况发生在噪声特别大的情况下,此时没有旋转矩阵能比反射矩阵使得误差更小。这种情况下不能用最小二乘法求解该问题,考虑使用RANSAC方法。
-
q
i
q_i
qi 共面,但不共线。 当W的奇异值有且仅有一个为0时,
q
i
q_i
qi 共面。此时有两个解都能使得误差函数取最小,分别是一个旋转矩阵和一个反射矩阵。
- 此时改变 v 3 v_3 v3 (W奇异值分解后V矩阵的第3列向量) 的正负号不会改变W的值,但是却能使得X从反射矩阵变为旋转矩阵,或者从旋转矩阵变为反射矩阵.
- 所以当 d e t ( x ) = − 1 det(x)=-1 det(x)=−1 时(反射矩阵的行列式为-1),通过改变 v 3 v_3 v3 的正负号,使得X变为我们所需要的旋转矩阵。
- q i q_i qi 共线。 当且仅当有两个奇异值相等时, q i q_i qi 共线。此时有无穷多的旋转矩阵和反射矩阵X,都能使误差函数最小,这种情况下用最小二乘法可能不能求解该问题,考虑使用RANSAC方法。
总的来说就是,当 d e t ( x ) = + 1 det(x)=+1 det(x)=+1 时,得到的就是所需的解; 反之当 d e t ( x ) = − 1 det(x)=-1 det(x)=−1 时,只有点共面的情况下,我们能通过修改结果得到解,其他情况下都不行,考虑用RANSAC方法。
在大部分情况下,这就是ICP的解。具体证明见下一小节,这里先分三种情况简要说明:
- q i q_i qi 不共面。只有旋转矩阵X能使得误差为0
其中D是奇异值组成的对角阵,并且奇异值按照从上到下,按照从大到小排列。
下面求解R,分三种情况:
- 第一种:无噪声情况
- q i q_i qi 不共面。只有旋转矩阵X能使得误差为0
- qi共面,但不共线。有一个旋转矩阵X和一个反射矩阵X能使得误差为0,但是很容易验证。 具体咋弄?
- 对H进行SVD分解 H = UDV^T ,当且仅当只有一个奇异值为0时,这些点共面。
- 此时改变v_3的正负号不会改变H的值,但是却能使得X从反射矩阵变为旋转矩阵,或者从旋转矩阵变为反射矩阵
- 所以当det(x)=-1时(反射矩阵),通过改变v_3的正负号,使得X变为我们需要的旋转矩阵。
- qi共线。有无穷多的旋转矩阵和反射矩阵X,能使得误差为0. 这种情况时,该方法解不了。
- 当且仅当有两个奇异值相等时,这些点共线。
- 第二种:有噪声情况
- s
- qi共面,结论和无噪声的一样,只是误差不能为0了,只是取最小。特殊的情况是只有3对匹配点的情况
- 不能处理的情况是:解X为反射矩阵(det(X)=-1) ,且H没有奇异值为0的情况(即点qi不共面)。此时没有旋转矩阵能比反射矩阵使得误差更小,这种情况一般只发生在噪声特别大的情况下,此时用最小二乘解可能始终得不到正确答案。这时候用RANSAC之类的方法剔除外点可能更好(一次只用3个点计算ICP)。
R = U V T R=UV^T R=UVT 还是 V U T VU^T VUT ?
如果同时看过《视觉slam十四讲》和论文 “Least-Squares Fitting of Two 3-D Point Sets” ,你可能会产生一个困惑:在论文中, R = V U T R=VU^T R=VUT , 但在书《视觉slam十四讲》中, R = U V T R=UV^T R=UVT ,他们的公式并不相同,这是因为在推导过程中对W的定义不同导致的。
在论文中,推导用的相关公式如下:
p
i
′
=
R
p
i
+
T
+
N
i
W
=
∑
i
=
1
N
q
i
q
i
′
T
R
=
V
U
T
\begin{aligned} p'_i &= Rp_i + T + N_i \\ W &= \sum_{i=1}^{N}q_iq'^T_{i} \\ R &= VU^T \\ \end{aligned}
pi′WR=Rpi+T+Ni=i=1∑Nqiqi′T=VUT
在书《视觉slam十四讲》中,推导用的相关公式如下:
p
i
=
R
p
i
′
+
t
+
e
i
W
=
∑
i
=
1
N
q
i
q
i
′
T
R
=
U
V
T
\begin{aligned} p_i &= Rp'_i + t + e_i \\ W &= \sum_{i=1}^{N}q_iq'^T_{i} \\ R &= UV^T \\ \end{aligned}
piWR=Rpi′+t+ei=i=1∑Nqiqi′T=UVT
可以看到,它们的旋转矩阵R,一个是从
p
i
p_i
pi 到
p
i
′
p'_i
pi′ 的,另一个正好相反,但是W的矩阵定义却是一样的,这就是差异所在。实际上,书《视觉slam十四讲》中的定义不够直观,它的W应该定义为
∑
i
=
1
N
q
i
q
i
′
T
\sum_{i=1}^{N}q_iq'^T_{i}
∑i=1Nqiqi′T 的转置才更合适,正是由于它们对W的定义差了一个转置,所以导致得到的解R,也差一个转置(
U
V
T
=
(
V
U
T
)
T
UV^T=(VU^T)^T
UVT=(VUT)T)。
算法流程总结
- 求两组点的质心 p , p ′ p,p' p,p′
p = 1 N ∑ i = 1 N p i p ′ = 1 N ∑ i = 1 N p i ′ \boldsymbol{p} = \frac{1}{N}\sum_{i=1}^{N}\boldsymbol{p_i} \\ \boldsymbol{p'} = \frac{1}{N}\sum_{i=1}^{N}\boldsymbol{p'_i} p=N1i=1∑Npip′=N1i=1∑Npi′
- 得到两组点的去质心坐标
q i = p i − p q i ′ = p i ′ − p ′ q_i = p_i - p \\ q'_i = p'_i - p' qi=pi−pqi′=pi′−p′
- 计算矩阵W
W = ∑ i = 1 N q i q i ′ T W = \sum_{i=1}^{N}q_iq'^T_i W=i=1∑Nqiqi′T
- 对W进行SVD分解,得到可能的解 X = V U T X = VU^T X=VUT
W = U D V T X = V U T W = UDV^T \\ X = VU^T W=UDVTX=VUT
判断X是否为所需解
如果 d e t ( X ) = + 1 det(X) = +1 det(X)=+1 , X是一个旋转矩阵,即为所需的解。
如果 d e t ( X ) = − 1 det(X) = -1 det(X)=−1 , X是一个反射矩阵。
如果W有且仅有一个奇异值为0,那么所需的旋转矩阵解为
X ′ = V ′ U T X' = V'U^T X′=V′UT
其中, V ′ = [ v 1 , v 2 , − v 3 ] V' = [v1, v2, -v3] V′=[v1,v2,−v3] ,即把V的最后一列向量符号取反。如果W没有奇异值为0,那么用最小二乘的方法解不出来,改用RANSAC方法求解ICP问题。
若计算得到了所需的解R = X,代入公式求得t
t = p ′ − R p t = p'-Rp t=p′−Rp
至此,R和t都已经求出来了。
代码实现
下面摘自《视觉SLAM十四讲》第七章。(这应该是本书第二版的代码,因为第一版代码中并没有判断解X是否为反射矩阵)
void pose_estimation_3d3d (
const vector<Point3f>& pts1,
const vector<Point3f>& pts2,
Mat& R, Mat& t
)
{
Point3f p1, p2; // center of mass
int N = pts1.size();
for ( int i=0; i<N; i++ )
{
p1 += pts1[i];
p2 += pts2[i];
}
p1 = Point3f( Vec3f(p1) / N);
p2 = Point3f( Vec3f(p2) / N);
vector<Point3f> q1 ( N ), q2 ( N ); // remove the center
for ( int i=0; i<N; i++ )
{
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for ( int i=0; i<N; i++ )
{
W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
}
cout<<"W="<<W<<endl;
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
if (U.determinant() * V.determinant() < 0)
{
for (int x = 0; x < 3; ++x)
{
U(x, 2) *= -1;
}
}
cout<<"U="<<U<<endl;
cout<<"V="<<V<<endl;
Eigen::Matrix3d R_ = U* ( V.transpose() );
Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );
// convert to cv::Mat
R = ( Mat_<double> ( 3,3 ) <<
R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
);
t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}
附录
矩阵的迹
参考:百度百科-矩阵的迹
在线性代数中,一个n×n矩阵A的主对角线(从左上方至右下方的对角线)上各个元素的总和被称为矩阵A的迹(或迹数),一般记作 tr(A)。
性质:
- 迹是所有对角元素的和
- 迹是所有特征值的和
- tr(AB)=tr(BA)
- t r ( m A + n B ) = m ∗ t r ( A ) + n ∗ t r ( B ) tr(mA+nB)=m*tr(A)+n*tr(B) tr(mA+nB)=m∗tr(A)+n∗tr(B)
Lemma 1(辅助定理 1)
《Least-Squares Fitting of Two 3-D Point Sets》中使用的一个辅助定理。
对于任何正定矩阵
A
A
T
AA^T
AAT 和任何正交矩阵B(orthonormal),有
t
r
(
A
A
T
)
≥
t
r
(
B
A
A
T
)
tr(AA^T) \ge tr(BAA^T)
tr(AAT)≥tr(BAAT)
证明:
A A T AA^T AAT 和 A T A A^TA ATA 的性质
参考:https://blog.csdn.net/Europe233/article/details/86720864
对于任意一个矩阵 A ∈ R m × n A\in R^{m×n} A∈Rm×n,则 A A T AA^T AAT 和 A T A A^TA ATA 都是对称半正定矩阵。
证明:
A A T AA^T AAT 显然对称,同时有: ∀ x ∈ R n , x T A T A x = ( A x ) T ( A x ) ≥ 0 ∀x∈R^n,x^TA^TAx=(Ax)^T(Ax)≥0 ∀x∈Rn,xTATAx=(Ax)T(Ax)≥0
同理可证 A T A A^TA ATA 对称半正定.
正交矩阵的性质
- 两个正交矩阵的积也是正交矩阵
- 任意正交矩阵的行列式只有两种取值:+1和-1
参考文献
-
《视觉SLAM十四讲》第一版 - 高翔
-
K. S. Arun, T. S. Huang and S. D. Blostein, “Least-Squares Fitting of Two 3-D Point Sets,” in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. PAMI-9, no. 5, pp. 698-700, Sept. 1987, doi: 10.1109/TPAMI.1987.4767965.
利用SVD分解求ICP问题的方法,看了十四讲后再看这个就容易很多。如果还看不懂可以先看看下面的参考链接“多视图几何之SVD”。
-
多视图几何之SVD | 证明了多个问题下为何能用SVD求解 | ICP中为何SVD可行 | 8点法求基本矩阵求解,为何是用SVD的最后一列向量 | 本质矩阵恢复R和t为什么可以用SVD | SVD为什么能求最小二乘解