学做网站培训机构,好看简洁的logo设计,免费发帖论坛大全,jsp网站开发什么框架1. 理解李群与李代数的概念#xff0c;掌握 SO(3), SE(3) 与对应李代数的表示方式。 2. 理解 BCH 近似的意义。 3. 学会在李代数上的扰动模型。 4. 使用 Sophus 对李代数进行运算。 目录
前言
一、李群李代数基础
1 群
2 李代数的引出
3 李代数的定义
4 李代数 so(3… 1. 理解李群与李代数的概念掌握 SO(3), SE(3) 与对应李代数的表示方式。 2. 理解 BCH 近似的意义。 3. 学会在李代数上的扰动模型。 4. 使用 Sophus 对李代数进行运算。 目录
前言
一、李群李代数基础
1 群
2 李代数的引出
3 李代数的定义
4 李代数 so(3)
5 李代数 se(3)
二、指数与对数映射
1 SO(3) 上的指数映射
2 SE(3) 上的指数映射
三、李代数求导与扰动模型
1 BCH 公式与近似形式
2 SO(3) 李代数上的求导
3 李代数求导
4 扰动模型左乘
5 SE(3) 上的李代数求导
四、实践Sophus
总结 前言
三维世界中刚体运动的描述:旋转矩阵、旋转向量、欧拉角、四元数等。除了表示之外还要对它们进行估计和优化。旋转矩阵自身带有约束正交且行列式为1)。作为优化变量时会引入额外的约束使优化变得困难。李代数上可以变成无约束优化。
哔哩哔哩课程链接视觉SLAM十四讲ch4_哔哩哔哩_bilibili 一、李群李代数基础
三维旋转矩阵构成了特殊正交群 SO(3)而变换矩阵构成了特殊欧氏群 SE(3) 旋转矩阵也好变换矩阵也好它们对加法是不封闭的。换句话说对于任意两个旋转矩阵 R1, R2它们按照矩阵加法的定义和不再是一个旋转矩阵 对于变换矩阵亦是如此。我们发现这两种矩阵并没有良好定义的加法相对的它们只有一种较好的运算乘法。SO(3) 和 SE(3) 关于乘法是封闭的: 道乘法对应着旋转或变换的复合——两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个运算的集合把它叫做群。
1 群
旋转矩阵集合和矩阵乘法构成群。同样变换矩阵和矩阵乘法也构成群。因此称它们为旋转矩阵群和变换矩阵群。
群Group是一种集合加上一种运算的代数结构。我们把集合记作 A运算记作 · 那么群可以记作 G (A, ·)。群要求这个运算满足以下几个条件 常见的群有
一般线性群 GL(n) 指 n × n 的可逆矩阵它们对矩阵乘法成群。 特殊正交群 SO(n) 也就是所谓的旋转矩阵群其中 SO(2) 和 SO(3) 最为常见。 特殊欧氏群 SE(n) 也就是前面提到的 n 维欧氏变换如 SE(2) 和 SE(3)。
李群是指具有连续光滑性质的群。像整数群 Z 那样离散的群没有连续性质所以不是李群。而 SO(n) 和 SE(n)它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动所以它们都是李群。
2 李代数的引出
任意旋转矩阵 R满足 如果R 是某个相机的旋转它会随时间连续地变化即为时间的函数R(t)。由于它仍是旋转矩阵有 对于任意反对称矩阵我们亦能找到一个与之对应的向量。把这个运算用符号 ∨ 表示 等式两边右乘 R(t)由于 R 为正交阵有 可以看到每对旋转矩阵求一次导数只需左乘一个 ϕ ∧(t) 矩阵即可。设 t0 0并设此时旋转矩阵为 R(0) I。按照导数定义可以把 R(t) 在 0 附近进行一阶泰勒展开 我们看到 ϕ 反映了 R 的导数性质故称它在 SO(3) 原点附近的正切空间 (Tangent Space) 上。同时在 t0 附近设 ϕ 保持为常数 ϕ(t0) ϕ0。 由于做了一定的假设所以它只在 t 0 附近有效。旋转矩阵 R 与另一个反对称矩阵 ϕ0 通过指数关系发生了联系。也就是说当知道某个时刻的 R 时存在一个向量 ϕ它们满足这个矩阵指数关系。 如果上式成立那么给定某时刻的 R就能求得一个 ϕ它描述了 R 在局部的导数关系。与 R 对应的 ϕ 有什么含义呢后面会看到ϕ 正是对应到 SO(3) 上的李代数 so(3) 其次矩阵指数 exp(ϕ ∧) 如何计算——事实上这正是李群与李代数间的指数/对数映射。 3 李代数的定义 每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定义如下 李代数由一个集合 V一个数域 F 和一个二元运算 [, ] 组成。如果它们满足以下几条性质称 (V, F, [, ]) 为一个李代数记作 g。 其中二元运算被称为李括号。从表面上来看李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算李括号表达了两个元素的差异。它不要求结合律而要求元素和自己做李括号之后为零的性质。 4 李代数 so(3) SO(3) 对应的李代数是定义在 R 3上的向量记作 ϕ。每个 ϕ 都可以生成一个反对称矩阵 由于 ϕ 与反对称矩阵关系很 紧密在不引起歧义的情况下就说 so(3) 的元素是 3 维向量或者 3 维反对称矩阵不加区别 与 SO(3) 的关系由指数映射给定 5 李代数 se(3) 对于 SE(3)它也有对应的李代数 se(3)。为省略篇幅我们就不描述如何引出 se(3)了。与 so(3) 相似se(3) 位于 R 6 空间中 把每个se(3)元素记作ξ它是一个六维向量。前三维为平移记作 ρ后三维为旋转记作 ϕ实质上是 so(3) 元素①。同时拓展了 ∧ 符号的含义。在 se(3) 中同样使用 ∧ 符号将一个六维向量转换成四维矩阵但这里不再表示反对称 仍使用 ∧ 和 ∨ 符号来指代“从向量到矩阵”和“从矩阵到向量”的关系以保持和 so(3) 上的一致性。读者可以简单地把 se(3) 理解成“由一个平移加上一个 so(3) 元素构成的向量”尽管这里的 ρ 还不直接是平移。同样李代数 se(3) 亦有类似于 so(3) 的李括号 二、指数与对数映射 1 SO(3) 上的指数映射
exp(ϕ ∧) 是如何计算的它是一个矩阵的指数在李群和李代数中称为指数映射Exponential Map。同样会先讨论 so(3) 的指数映射再讨论 se(3) 的情形。
任意矩阵的指数映射可以写成一个泰勒展开但是只有在收敛的情况下才会有结果其结果仍是一个矩阵。 同样地对 so(3) 中任意一元素 ϕ亦可按此方式定义它的指数映射 由于 ϕ 是三维向量可以定义它的模长和它的方 向分别记作 θ 和 a于是有 ϕ θa。这里 a 是一个长度为 1 的方向向量。首先对于a ∧有以下两条性质 利用这两个性质可以把指数映射写成 so(3) 实际上就是由所谓的旋转向量组成的空间而指数映射即罗德里格斯公式。通过它们把 so(3) 中任意一个向量对应到了一个位于 SO(3) 中的旋转矩阵。反之如果定义对数映射也能把 SO(3) 中的元素对应到 so(3) 中 2 SE(3) 上的指数映射
se(3) 上的指数映射形式如下 ξ 的指数映射左上角的 R 是我们熟知的 SO(3) 中的元素与 se(3) 当中的旋转部分 ϕ 对应。而右上角的 J 则可整理为设 ϕ θa 从左上的 R 计算旋转向量而右上的 t 满足t Jρ。由于 J 可以由 ϕ 得到所以这里的 ρ 亦可由此线性方程解得。 SO(3), SE(3), so(3), se(3) 的对应关系如上图。
三、李代数求导与扰动模型
1 BCH 公式与近似形式
在 SO(3) 中完成两个矩阵乘法时李代数中 so(3)上发生了什么改变呢反过来说当 so(3) 上做两个李代数的加法时SO(3) 上是否对应着两个矩阵的乘积如果成立的话相当于
exp (ϕ ∧ 1 ) exp (ϕ ∧ 2 ) exp ( (ϕ1 ϕ2) ∧ ) .
如果 ϕ1, ϕ2 为标量那显然该式成立但此处我们计算的是矩阵的指数函数而非标量的指数。
在研究下式是否成立ln (exp (A) exp (B)) A B ? 很遗憾该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式由 Baker-Campbell-Hausdorff 公式BCH 公式给出。由于它完整的形式较复杂我们给出它展开式的前几项 其中 [] 为李括号。当处理两个矩阵指数之积时它们会产生一些由李括号组成的余项。特别地考虑 SO(3) 上的李代数 ln (exp (ϕ ∧ 1 ) exp (ϕ ∧ 2 ))∨当 ϕ1 或ϕ2 为小量时小量二次以上的项都可以被忽略掉。此时BCH 拥有线性近似表达 当对一个旋转矩阵 R2李代数为 ϕ2左乘一个 微小旋转矩阵 R1李代数为 ϕ1时可以近似地看作在原有的李代数 ϕ2 上加上了一项 Jl(ϕ2) −1ϕ1。同理第二个近似描述了右乘一个微小位移的情况。于是李代数在 BCH近似下分成了左乘近似和右乘近似两种在使用时须加注意使用的是左乘模型还是右乘模型。
书以左乘为例。左乘 BCH 近似雅可比 Jl 事实上就是: 假定对某个旋转 R对应的李代数为 ϕ。我们给它左乘一个微小旋转记作 ∆R对应的李代数为 ∆ϕ。那么在李群上得到的结果就是 ∆R · R而在李代数上根据 BCH近似为Jl −1 (ϕ)∆ϕ ϕ。合并起来可以简单地写成 反之如果在李代数上进行加法让一个 ϕ 加上 ∆ϕ那么可以近似为李群上带左右雅可比的乘法 这将为之后李代数上的做微积分提供了理论基础。同样的对于 SE(3)亦有类似的BCH 近似公式 这里 J l 形式比较复杂它是一个 6 × 6 的矩阵。由于在计算中不用到该雅可比故这里略去它的实际形式。
2 SO(3) 李代数上的求导
在 SLAM 中要估计一个相机的位置和姿态该位姿是由 SO(3) 上的旋转矩阵或 SE(3) 上的变换矩阵描述的。不妨设某个时刻小萝卜的位姿为 T。它观察到了一个世界坐标位于 p 的点产生了一个观测数据 z。那么由坐标变换关系知 然而由于观测噪声 w 的存在z 往往不可能精确地满足 z T p 的关系。所以通常会计算理想的观测与实际数据的误差 寻找一个最优的 T使得整体误差最小化 李代数解决求导问题的思路分为两种
用李代数表示姿态然后对根据李代数加法来对李代数求导。 对李群左乘或右乘微小扰动然后对该扰动求导称为左扰动和右扰动模型。 第一种方式对应到李代数的求导模型而第二种则对应到扰动模型。下面我们来讨论这两种思路的异同。 3 李代数求导
首先考虑 SO(3) 上的情况。假设对一个空间点 p 进行了旋转得到了 Rp。现在要计算旋转之后点的坐标相对于旋转的导数不严谨地记为 按照导数的定义有 第二行的近似为 BCH 线性近似第三行为泰勒展开舍去高阶项后近似第四行至第五行将反对称符号看作叉积交换之后变号。于是推导了旋转后的点相对于李代数的导数 4 扰动模型左乘
另一种求导方式是对 R 进行一次扰动 ∆R。这个扰动可以乘在左边也可以乘在右边最后结果会有一点儿微小的差异我们以左扰动为例。设左扰动 ∆R 对应的李代数为φ。然后对 φ 求导即 5 SE(3) 上的李代数求导
给出 SE(3) 上的扰动模型而直接李代数上的求导就不再介绍了。假设某空间点 p 经过一次变换 T对应李代数为 ξ得到 T p。现在给 T 左乘一个扰动∆T exp (δξ ∧)设扰动项的李代数为 δξ [δρ, δϕ] T那么 把最后的结果定义成一个算符 ⊙它把一个齐次坐标的空间点变换成一个 4 × 6的矩阵。 四、实践Sophus
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
Sophus 本身亦是一个 cmake 工程。想必你已经了解如何编译 cmake 工程了。Sophus 库只须编译即可 无须安装。
mkdir build
cd build
cmake ..
make sudo make install 如果遇到了关于fmt的问题 git clone https://github.com/fmtlib/fmt.git
cd fmt
mkdir buildcd build
cmake ..
make
make install 在build文件里面生成useSophus文件运行结果 2.22045e-16 -1 01 2.22045e-16 00 0 1
SO(3) from quaternion:
2.22045e-16 -1 01 2.22045e-16 00 0 1
they are equal
so3 0 0 1.5708
so3 hat0 -1.5708 01.5708 0 -0-0 0 0
so3 hat vee 0 0 1.5708
SO3 updated 0 -1 01 0 -0.00010.0001 2.03288e-20 1
*******************************
SE3 from R,t
2.22045e-16 -1 0 11 2.22045e-16 0 00 0 1 00 0 0 1
SE3 from q,t
2.22045e-16 -1 0 11 2.22045e-16 0 00 0 1 00 0 0 1
se3 0.785398 -0.785398 0 0 0 1.5708
se3 hat 0 -1.5708 0 0.7853981.5708 0 -0 -0.785398-0 0 0 00 0 0 0
se3 hat vee 0.785398 -0.785398 0 0 0 1.5708
SE3 updated
2.22045e-16 -1 0 1.00011 2.22045e-16 0 00 0 1 00 0 0 1总结
以上就是今天要讲的内容本文重点介绍了旋转的表示但是在 SLAM 中除了表示之 外我们还要对它们进行估计和优化。因为在 SLAM 中位姿是未知的而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题求解最优的 R, t使得误差最小化。
如前所言旋转矩阵自身是带有约束的正交且行列式为 1。它们作为优化变量时会引入额外的约束使优化变得困难。通过李群——李代数间的转换关系希望把位姿估计变成无约束的优化问题简化求解方式。
