这一章的数学概念涉及很多,梳理清楚很重要。
注:
1)学习视频:【高翔】视觉SLAM十四讲。
第4讲
4.1 理论部分
概念的东西很多,但主要还是围绕李群和李代数展开。
一个最本质的问题,为什么要采用李群和李代数?
这是因为旋转矩阵含有约束,优化困难;而李代数无约束,可以把位姿估计变成无约束的优化问题。
1)李群&李代数基础
- 一种集合加上一种运算的代数结构称为群,满足“封结幺逆”性质。之前所提及的特殊正交群 S O ( 3 ) SO(3) SO(3)、特殊欧式群 S E ( 3 ) SE(3) SE(3),均是一种集合加上一种运算(矩阵乘法,这2个群均对加法不封闭);
- 由李群引入李代数:首先我们有一个旋转矩阵 R ( t ) \pmb{R}(t) RRR(t),由其正交且行列式为1的性质,可得到 R ( t ) R ( t ) T = I \pmb{R}(t)\pmb{R}(t)^T=\pmb{I} RRR(t)RRR(t)T=III,两边同时求导再移相,得知 R ( ˙ t ) R ( t ) T = ϕ ( t ) ^ \pmb{R}\dot(t)\pmb{R}(t)^T=\phi(t)\hat{} RRR(˙t)RRR(t)T=ϕ(t)^ 为反对称矩阵,变换得 R ( ˙ t ) = ϕ ( t ) ^ R ( t ) \pmb{R}\dot(t)=\phi(t)\hat{}\ \pmb{R}(t) RRR(˙t)=ϕ(t)^ RRR(t),即旋转矩阵的导数为一个反对称矩阵与它本身的乘法。随后对 R ( t ) \pmb{R}(t) RRR(t)泰勒展开,引入 t 0 = 0 , R ( t 0 ) = R 0 = I t_0=0,R(t_0)=R_0=I t0=0,R(t0)=R0=I,得 R ( t ) ≈ I + ϕ ( t 0 ) ^ ( t ) \pmb{R}(t)\approx{I}+\phi(t_0)\hat{}\ (t) RRR(t)≈I+ϕ(t0)^ (t),令 ϕ ( t 0 ) = ϕ 0 \phi(t_0)=\phi_0 ϕ(t0)=ϕ0,得到 R ( ˙ t ) = ϕ 0 ^ R ( t ) \pmb{R}\dot(t)=\phi_0\hat{}\ \pmb{R}(t) RRR(˙t)=ϕ0^ RRR(t),则 R ( t ) = e x p ( ϕ 0 ^ t ) \pmb{R}(t)=exp(\phi_0\hat{}\ t) RRR(t)=exp(ϕ0^ t)(仅说明 t = 0 t=0 t=0附近)。 ϕ \phi ϕ就是对应 S O ( 3 ) SO(3) SO(3)的李代数 s o ( 3 ) so(3) so(3);
- 李代数由一个集合、一个数域和一个二元运算组成。二元运算称为李括号;
- 李代数 s o ( 3 ) so(3) so(3)的元素是3维向量 ϕ \phi ϕ(或者3维反对称矩阵 ϕ = ϕ ^ \pmb\phi=\phi\ \hat{} ϕϕϕ=ϕ ^,两者一一对应,不严格要求可以这么表述);
- 李代数 s e ( 3 ) se(3) se(3)的元素是6维向量 ξ \xi ξ(3维平移加3维旋转)(或者4维矩阵 ξ ^ \xi\ \hat{} ξ ^,同样矩阵与向量一一对应,但这里的反对称符号仅是扩充,不表示反对称);
- 李群是具有连续性质的群(与离散相对应),李代数是一个单位元附近的正切空间,每个李群对应一个李代数。简单笼统地说,李群是一个矩阵集合,李代数是相对应的一个向量集合。
2)指数与对数映射
- 由前面的推导我们知道,李代数求李群可以通过指数映射,相对的,李群求李代数可以通过对数映射。但简便运算中,可以采用迹求李代数,在第3讲中有;
- 李代数 s o ( 3 ) so(3) so(3)的物理意义是旋转向量;
- 李代数 s e ( 3 ) se(3) se(3)的平移部分 J ρ \pmb{J\rho} JρJρJρ和李群 S E ( 3 ) SE(3) SE(3)的平移部分 ρ \pmb{\rho} ρρρ相差一个雅可比矩阵 J \pmb{J} JJJ;
- 李群与李代数的对应关系,整理在书中图4-1,非常详细。
注:
图片来源:从零开始一起学习SLAM | 为啥需要李群与李代数?。
3)李代数求导
- 上一个问题中,我们知道了李代数和李群之间存在指数、对数映射关系,但是李群乘法和李代数加法之间具体是怎样的关系,在这里进一步推导。
- 结合BCH公式和线性近似表达,可以得出李群乘法与李代数加法的关系: ln ( e x p ( ϕ 1 ^ ) e x p ( ϕ 2 ^ ) ) v ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 当 ϕ 1 为小量, J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 当 ϕ 2 为小量. \ln(exp(\phi_1\hat{}\ )exp(\phi_2\hat{}\ ))^\textsf{v}\approx\begin{cases} \pmb{J}_l(\phi_2)^{-1}\phi_1+\phi_2& \texttt{当}\phi_1\texttt{为小量,}\\ \pmb{J}_r(\phi_1)^{-1}\phi_2+\phi_1& \texttt{当}\phi_2\texttt{为小量.} \end{cases} ln(exp(ϕ1^ )exp(ϕ2^ ))v≈{JJJl(ϕ2)−1ϕ1+ϕ2JJJr(ϕ1)−1ϕ2+ϕ1当ϕ1为小量,当ϕ2为小量.(式子左边可以理解为,先对两个李群 R 1 R_1 R1和 R 2 R_2 R2相乘,然后求 ln \ln ln得到李群乘积对应的反对称矩阵,然后求 v \textsf{v} v得到对应的李代数);
- 上一步的式子中存在左乘小量还是右乘小量的问题,对应左乘模型和右乘模型,看个人习惯。
- 总体而言(左乘模型为例),李群左乘一个小量 e x p ( Δ ϕ ^ ) exp(\Delta\phi\ \hat{}\ ) exp(Δϕ ^ )等于李代数加一个 J l − 1 ( ϕ ) Δ ϕ \pmb{J}_l^{-1}(\phi)\Delta\phi JJJl−1(ϕ)Δϕ【对应公式 e x p ( Δ ϕ ^ ) e x p ( ϕ ^ ) = e x p ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) ^ ) exp(\Delta\phi\ \hat{}\ )exp(\phi\hat\ )=exp((\phi+\pmb{J}_l^{-1}(\phi)\Delta\phi)\hat\ ) exp(Δϕ ^ )exp(ϕ ^)=exp((ϕ+JJJl−1(ϕ)Δϕ) ^)】。反之,李代数加一个 Δ ϕ \Delta\phi Δϕ等于李群左乘一个 e x p ( ( J l Δ ϕ ) ^ ) exp((\pmb{J}_l\Delta\phi)\ \hat{}\ ) exp((JJJlΔϕ) ^ )【对应公式 e x p ( ( ϕ + Δ ϕ ) ^ ) = e x p ( ( J l Δ ϕ ) ^ ) e x p ( ϕ ^ ) exp((\phi+\Delta\phi)\hat\ )=exp((\pmb{J}_l\Delta\phi)\ \hat{}\ )exp(\phi\hat\ ) exp((ϕ+Δϕ) ^)=exp((JJJlΔϕ) ^ )exp(ϕ ^)】;
- 李代数求导的实际意义是构建位姿相关函数时会利用关于位姿的导数优化估计值。但由于李群对加法不封闭,因此需要借助李代数加法;
- 李代数解决求导问题有2种思路。a.导数模型:求导时给 R \pmb{R} RRR对应的李代数加一个小量,求相对于小量的变化率;b.扰动模型:对李群左乘(或右乘)微小扰动,求相对于小量的李代数的变化率。实际常用扰动模型,因为导数模型会多一个雅可比矩阵,运算复杂;
- S O ( 3 ) SO(3) SO(3)上的李代数求导: ∂ ( Rp ) ∂ φ = − ( R p ) ^ \frac{\partial ( \textbf {Rp})}{\partial \varphi}=-(\pmb{Rp})\hat{} ∂φ∂(Rp)=−(RpRpRp)^
- S E ( 3 ) SE(3) SE(3)上的李代数求导: ∂ ( Tp ) ∂ δ ξ = [ I − ( R p + t ) ^ 0 T 0 T ] \frac{\partial ( \textbf {Tp})}{\partial \delta\xi}=\begin{bmatrix} \pmb{I} & -(\pmb{Rp+t})\hat{} \\ \pmb{0}^T & \pmb{0}^T \end{bmatrix}\quad ∂δξ∂(Tp)=[III000T−(Rp+tRp+tRp+t)^000T]
4)相似变换群
- 相似变换群 S i m ( 3 ) Sim(3) Sim(3)只用于单目视觉;
- 单目视觉中,世界系到相机系的变换不能用欧式变换(仅含平移、旋转)描述,需要用含有尺度因子的相似变换描述;
- 李代数 s i m ( 3 ) sim(3) sim(3)的元素为7维向量 ζ \zeta ζ,前6维与 s e ( 3 ) se(3) se(3)相同,仅多一项 σ \sigma σ。更多内容见书 P 92 − 93 P_{92-93} P92−93。
4.2 实践部分
待续ing