标签：wedge phi mathbf 矩阵 李群 exp lim Sophus 代数

开篇介绍

在上次博客中，我们介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角、四元数等若干种方式。其中重点介绍了旋转的表示，但是在SLAM中，除了表示之外，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。于是我们把他归结为一种典型的优化问题，求解最优的 R \mathbf{R} R， t \mathbf{t} t，使得误差最小化。

一、群和李群

在上次谈到的三维旋转矩阵和变换矩阵中，我们知道三维旋转矩阵构成了特殊正交群 S O ( 3 ) SO(3) SO(3)，而变换矩阵构成了特殊欧氏群 S E ( 3 ) SE(3) SE(3)： S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3)=\left \{\mathbf{R}\in\mathbb{R}^{3\times3}\mid\mathbf{R}\mathbf{R}^T=\mathbf{I},det(\mathbf{R})=1 \right \} SO(3)={R∈R3×3∣RRT=I,det(R)=1} S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3)=\left \{ \mathbf{T}=\begin{bmatrix} \mathbf{R}&\mathbf{t} \\ \mathbf{0}^T&1 \end{bmatrix}\in\mathbb{R}^{4\times 4}\mid\mathbf{R}\in SO(3),\mathbf{t}\in\mathbb{R}^3\right \} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
但是，旋转矩阵和变换矩阵对加法来说都是不封闭的。也就是说，对任意两个旋转矩阵 R 1 , R 2 R_1,R_2 R1​,R2​，按照矩阵加法的定义，和不再是一个旋转矩阵： R 1 + R 2 ∉ S O ( 3 ) R_1+R_2\notin SO(3) R1​+R2​∈/​SO(3)然而，它们对乘法有着封闭性： R 1 R 2 ∈ S O ( 3 ) , T 1 T 2 ∈ S E ( 3 ) R_1R_2\in SO(3),T_1T_2\in SE(3) R1​R2​∈SO(3),T1​T2​∈SE(3)
群的概念：群是一种集合加上一种运算的代数结构。集合记作 A \mathbf{A} A，运算记作 ⋅ \cdot ⋅，那么群可以记作 G = ( A , ⋅ ) G=(A,\cdot) G=(A,⋅)。
李群是指具有连续(光滑)性质的群。像整数群 Z \mathbb{Z} Z那样的群没有连续性质，所以不是李群。而 S O ( n ) SO(n) SO(n)和 S E ( n ) SE(n) SE(n)，它们在实数空间上是连续的。我们能够直观的想象一个刚体能够在连续地空间中运动，所以它们是李群。

二、李代数

I.李代数的介绍

对于任意旋转矩阵，我们知道其也是一个正交矩阵，满足： R R T = I \mathbf{R}\mathbf{R}^T=\mathbf{I} RRT=I在实际应用场景下， R \mathbf{R} R是机器人或小车的旋转，会随着时间连续变化，于是旋转矩阵就为时间的连续函数 R ( t ) \mathbf{R}(t) R(t)。于是有： R ( t ) R ( t ) T = I \mathbf{R}(t)\mathbf{R}(t)^T=\mathbf{I} R(t)R(t)T=I，由于李群是连续的，对时间求导后得： R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0 \dot{\mathbf{R}}(t)\mathbf{R}(t)^T+\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=0 R˙(t)R(t)T+R(t)R˙(t)T=0 R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T \dot{\mathbf{R}}(t)\mathbf{R}(t)^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t)^T)^T R˙(t)R(t)T=−(R˙(t)R(t)T)T
我们可以看出 R ˙ ( t ) R ( t ) T \dot{\mathbf{R}}(t)\mathbf{R}(t)^T R˙(t)R(t)T是一个反对称矩阵，根据先前介绍的叉乘运算，我们引入了向量转反对称矩阵的运算(也即 ∧ ^\wedge ∧)，对于任意反对称矩阵，我们都能找到一个与之对应的向量。于是我们令： R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ {\color{Cyan} \dot{\mathbf{R}}(t)\mathbf{R}(t)^T=\phi(t)^\wedge} R˙(t)R(t)T=ϕ(t)∧，根据正交阵的特性，其逆矩阵为自己的转置矩阵，于是等式两边右乘 R ( t ) \mathbf{R}(t) R(t)有： R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) \dot{\mathbf{R}}(t)=\phi(t)^\wedge\mathbf{R}(t)=\begin{bmatrix} 0&-\phi_3&\phi_2 \\ \phi_3&0&-\phi_1 \\ -\phi_2&\phi_1 &0 \end{bmatrix}\mathbf{R}(t) R˙(t)=ϕ(t)∧R(t)=⎣⎡​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​⎦⎤​R(t)可见，每对旋转矩阵求导一次，我们只需左乘一个 ϕ ( t ) ∧ \phi(t)^\wedge ϕ(t)∧矩阵即可。假设旋转矩阵满足 R ( 0 ) = I \mathbf{R}(0)=\mathbf{I} R(0)=I，我们对 R ( t ) \mathbf{R}(t) R(t)在 t = 0 t=0 t=0处进行一阶泰勒展开： R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ ( t ) \mathbf{R}(t)\approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)=\mathbf{I}+\phi(t_0)^\wedge(t) R(t)≈R(t0​)+R˙(t0​)(t−t0​)=I+ϕ(t0​)∧(t)
在 t 0 t_0 t0​附近，假设 ϕ \phi ϕ保持为常数 ϕ ( t 0 ) = ϕ 0 \phi(t_0)=\phi_0 ϕ(t0​)=ϕ0​，那么通过求解 R \mathbf{R} R的微分方程，解得 R ( t ) = e x p ( ϕ 0 ∧ t ) {\color{Magenta} \mathbf{R}(t)=exp(\phi_0^\wedge t)} R(t)=exp(ϕ0∧​t)
不失一般性，我们找出了给定某时刻的 R \mathbf{R} R，我们就能求得一个 ϕ \phi ϕ，它描述了 R R R在局部的导数关系，那么这里的 ϕ \phi ϕ就是我们 S O ( n ) SO(n) SO(n)群的李代数 s o ( 3 ) \mathfrak{so}(3) so(3)。
在变换矩阵中，我们如此类推，也能得到 S E ( n ) SE(n) SE(n)群的李代数 s e ( 3 ) \mathfrak{se}(3) se(3)。

II.指数映射

在清楚了旋转矩阵或者变换矩阵能够构成对应的李群后，我们也清楚了它们各自的李代数，也就是旋转向量 ϕ = θ a \phi=\theta\mathbf{a} ϕ=θa，其中，这里 a \mathbf{a} a是长度为1的方向向量， θ \theta θ描述了旋转向量的长度。接下来我们就能通过以 e e e为底数的指数函数的泰勒展开式来进行推导，最后得出 R = e x p ( ϕ ∧ ) = e x p ( θ a ∧ ) = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧ {\color{Magenta} \begin{aligned} \mathbf{R} &=exp(\phi^\wedge) \\ &= exp(\theta\mathbf{a}^\wedge) \\ &=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{aa}^T+\sin\theta\mathbf{a}^\wedge \end{aligned}} R​=exp(ϕ∧)=exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧​这也就是我们之前提到过的罗德里格斯公式，详细的推导证明可以查看——罗德里格斯公式推导
而当我们知道了旋转矩阵，需要求出旋转向量的时候，我们可以通过旋转矩阵的迹公式求出旋转角 θ \theta θ以及通过求 R \mathbf{R} R的特征向量求出旋转向量的方向。
同样地，变换矩阵的指数映射求解也能从相同的推导中得出，过程大同小异，只是引入了平移量。

三、BCH公式及近似

我们使用李代数的一个动机是为了进行优化，而在优化问题中导数又是重中之重的任务。所以我们来考虑在 S O ( 3 ) SO(3) SO(3)群中的两个矩阵的乘法，是否能对应到李代数 s o ( 3 ) \mathfrak{so}(3) so(3)中的加法呢？如果成立的话，相当于： e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) = e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) exp(\phi_1^\wedge)exp(\phi_2^\wedge)=exp((\phi_1+\phi_2)^\wedge) exp(ϕ1∧​)exp(ϕ2∧​)=exp((ϕ1​+ϕ2​)∧)
但是，很遗憾，该式只在变量是标量的情况下成立；在变量为矩阵时并不成立。
两个李代数指数映射乘积的完整形式，由 B a k e r − C a m p b e l l − H a u s d o r f f Baker-Campbell-Hausdorff Baker−Campbell−Hausdorff公式给出。在 B C H BCH BCH公式下，当扰动量很小时，我们可以分别对应到左乘模型和右乘模型： ln ⁡ ( exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2  if  ϕ 1 i s s m a l l J l ( ϕ 1 ) − 1 ϕ 2 + ϕ 1  if  ϕ 2 i s s m a l l \ln(\exp(\phi_1^\wedge)\exp(\phi_2^\wedge))^\vee \approx \begin{cases} \mathbf{J}_l(\phi_2)^{-1}\phi_1+\phi_2 & \text{ if } \phi_1 is \quad small \\ \mathbf{J}_l(\phi_1)^{-1}\phi_2+\phi_1 & \text{ if } \phi_2 is \quad small \end{cases} ln(exp(ϕ1∧​)exp(ϕ2∧​))∨≈{Jl​(ϕ2​)−1ϕ1​+ϕ2​Jl​(ϕ1​)−1ϕ2​+ϕ1​​ if ϕ1​issmall if ϕ2​issmall​
上式中第一个对应的是左乘模型(或左扰动模型)，第二个对应的是右乘模型。(或右扰动模型)

四、李代数求导

当一切准备就绪之后，我们就要考虑我们的优化问题了。回顾之前，我们引入了具有连续性质的李群，并知道了李代数的求解方法。由于实际场景应用当中，每个观测点得到的位姿数据都会存在噪声，于是我们要得到机器人或者小车在什么位置能够得到最佳观测数据，也就是归结到我们的优化问题上了。
于是，我们的目标就是，优化旋转矩阵，得到最佳的观测位置。 z = T p + w \mathit{z=Tp+w} z=Tp+w e = z − T p \mathit{e=z-Tp} e=z−Tp
现在，我们通过对李代数求导， ∂ ( R p ) ∂ R = ∂ ( exp ⁡ ( ϕ ∧ ) p ) δ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( ϕ + δ ϕ ) ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ ≈ lim ⁡ δ ϕ → 0 ( I + ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 ( J l δ ϕ ) ∧ exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 − ( exp ⁡ ( ϕ ∧ ) p ) ∧ J l δ ϕ δ ϕ = − ( R p ) ∧ J l {\color{Magenta}\begin{aligned} \frac{\partial (Rp)}{\partial R} &= \frac{\partial(\exp(\phi^\wedge)p)}{\delta\phi}\\ &= \lim_{\delta\phi\rightarrow 0}\frac{\exp((\phi+\delta\phi)^\wedge)\mathit{p}-\exp(\phi^\wedge)\mathit{p}}{\delta\phi}\\ &=\lim_{\delta\phi\rightarrow0}\frac{\exp(\mathit(J_l\delta\phi)^\wedge)\exp(\phi^\wedge)\mathit{p}-\exp(\phi^\wedge)\mathit{p}}{\delta\phi}\\ &\approx\lim_{\delta\phi\rightarrow 0}\frac{(I+(J_l\delta\phi)^\wedge)\exp(\phi^\wedge)p-\exp(\phi^\wedge)p}{\delta\phi}\\ &=\lim_{\delta\phi\rightarrow 0}\frac{(J_l\delta\phi)^\wedge\exp(\phi^\wedge)p}{\delta\phi}\\ &=\lim_{\delta\phi\rightarrow 0}\frac{-(\exp(\phi^\wedge)p)^\wedge J_l\delta\phi}{\delta\phi}\\ &= -(Rp)^\wedge J_l \end{aligned}} ∂R∂(Rp)​​=δϕ∂(exp(ϕ∧)p)​=δϕ→0lim​δϕexp((ϕ+δϕ)∧)p−exp(ϕ∧)p​=δϕ→0lim​δϕexp((Jl​δϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p​≈δϕ→0lim​δϕ(I+(Jl​δϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p​=δϕ→0lim​δϕ(Jl​δϕ)∧exp(ϕ∧)p​=δϕ→0lim​δϕ−(exp(ϕ∧)p)∧Jl​δϕ​=−(Rp)∧Jl​​

五、扰动模型

以上述介绍过的左乘模型为例子，对 R R R的一次扰动量设为 Δ R \Delta R ΔR。这个扰动可以左乘或右乘，最后结果会由细微的差异。我在这里先以左扰动为例，设左扰动 Δ R \Delta R ΔR对应的李代数为 φ \varphi φ，然后对 φ \varphi φ求导，即：
∂ ( R p ) ∂ φ = lim ⁡ φ → 0 exp ⁡ ( φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ ≈ lim ⁡ φ → 0 ( 1 + φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 φ ∧ R p φ = lim ⁡ φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ {\color{blue}\begin{aligned} \frac{\partial(Rp)}{\partial\varphi} &=\lim_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge)\exp(\phi^\wedge)p-\exp(\phi^\wedge)p}{\varphi}\\ &\approx\lim_{\varphi\rightarrow 0}\frac{(1+\varphi^\wedge)\exp(\phi^\wedge)p-\exp(\phi^\wedge)p}{\varphi}\\ &=\lim_{\varphi\rightarrow 0}\frac{\varphi^\wedge Rp}{\varphi}=\lim_{\varphi\rightarrow 0}\frac{-(Rp)^\wedge\varphi}{\varphi}=-(Rp)^\wedge \end{aligned}} ∂φ∂(Rp)​​=φ→0lim​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​≈φ→0lim​φ(1+φ∧)exp(ϕ∧)p−exp(ϕ∧)p​=φ→0lim​φφ∧Rp​=φ→0lim​φ−(Rp)∧φ​=−(Rp)∧​
可见，扰动模型相比于直接对李代数求导，省去了一个雅可比 J l J_l Jl​的计算，这使得扰动模型更加实用。

六、Sophus实践

在了解到李群和李代数、李代数求导以及扰动模型后，我们在Kdevelop上进行基本的实践操作：(详情注释已经在代码中体现)

编译执行后得到如下结果：

这里使用的是非模板类的Sophus，在下载后非模板类Sophus后，我们需要在CMakeLists文本中加入寻找库的命令：

#为使用sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
project(useSophus)

include_directories(${Sophus_INCLUDE_DIRS})
add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus ${Sophus_LIBRARIES})

另外要注意的一点是，在github源上的Sophus存在一些小的bug(https://github.com/strasdat/Sophus.git)
其中so2.cpp文件中有一个类出现了问题：

unit_complex_.real() = 1.;
unit_complex_.imag() = 0.;

应该修改为：

unit_complex_.real(1.);
unit_complex_.imag(0.);

最终，Sophus才能编译成功，开发者才能成功使用Sophus库开发项目。

七、总结

到目前为止，SLAM中的系统的位置姿态表述基础知识已经完全叙述完了，之前提到的旋转向量、旋转矩阵、四元数、欧拉角还有李代数和李群等等基本构件，都是SLAM中必不可少的元件，通过初步接触和学习，希望在后续开发中得到深入与场景应用。

标签：wedge,phi,mathbf,矩阵,李群,exp,lim,Sophus,代数
来源： https://blog.csdn.net/weixin_42024702/article/details/115288693

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。