在 SLAM 中,除了表示之外,我们还要对它们进行估计和优化。因为在 SLAM 中位姿是未知的,而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题,求解最优的 R,t,使得误差最小化。如前所言,旋转矩阵自身是带有约束的(正交且行列式为 1)。它们作为优化变量时,会引入额外的约束,使优化变得困难。通过李群——李代数间的转换关系,我们希望把位姿估计变成无约束的优化问题,简化求解方式。
我们说三维旋转矩阵构成了特殊正交群 SO(3),而变换矩阵构成了特殊欧氏群 SE(3):
旋转矩阵也好,变换矩阵也好,它们对加法是不封闭的。换句话说,对于任意两个旋转矩阵 R1 ,R2 ,它们按照矩阵加法的定义,和不再是一个旋转矩阵。对于变换矩阵亦是如此。我们发现,这两种矩阵并没有良好定义的加法,相对的,它们只有一种较好的运算:乘法。SO(3) 和 SE(3) 关于乘法是封闭的:
乘法对应着旋转或变换的复合——两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个运算的集合,我们把它叫做群。
群
群(Group)是一种集合加上一种运算的代数结构。我们把集合记作 A,运算记作 ·,那么群可以记作 G = (A,·)。群要求这个运算满足以下几个条件:
可以验证,旋转矩阵集合和矩阵乘法构成群,同样变换矩阵和矩阵乘法也构成群(因此才能称它们为旋转矩阵群和变换矩阵群)。其他常见的群包括整数的加法 (Z,+),去掉 0 后的有理数的乘法(幺元为 1)(Q\0,·) 等等。矩阵中常见的群有:
一般线性群 GL(n) 指 n × n 的可逆矩阵,它们对矩阵乘法成群。
特殊正交群 SO(n) 也就是所谓的旋转矩阵群,其中 SO(2) 和 SO(3) 最为常见。
特殊欧氏群 SE(n) 也就是前面提到的 n 维欧氏变换,如 SE(2) 和 SE(3)。
李群是指具有连续(光滑)性质的群。像整数群 Z 那样离散的群没有连续性质,所以不是李群。而 SO(n) 和 SE(n),它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动,所以它们都是李群。由于 SO(3) 和 SE(3) 对于相机姿态估计尤其重要,我们主要讨论这两个李群。
首先引出 SO(3) 上面的李代数 so(3)。
李代数
考虑任意旋转矩阵 R,我们知道它满足:
现在,我们说,R 是某个相机的旋转,它会随时间连续地变化,即为时间的函数:R(t)。由于它仍是旋转矩阵,有
在等式两边对时间求导,得到:
整理得:
可以看出是一个反对称矩阵。对于任意反对称矩阵,我们能找到一个与之对应的向量。把这个运算用符号∨表示
由于是一个反对称矩阵,我们可以找到一个三维向量
与之对应。
等式两边右乘 R(t),由于 R 为正交阵,有:
可以看到,每对旋转矩阵求一次导数,只需左乘一个 ϕ∧(t) 矩阵即可。为方便讨论,我们设 t0 = 0,并设此时旋转矩阵为 R(0) = I。按照导数定义,可以把 R(t) 在 0 附近进行一阶泰勒展开:
我们看到 ϕ 反映了 R 的导数性质,故称它在 SO(3) 原点附近的正切空间 (Tangent Space) 上。同时在 t0 附近,设 ϕ 保持为常数 。
上式是一个关于 R 的微分方程,而且我们知道初始值 R(0) = I,解之,得:
可以验证上式对微分方程和初始值均成立。不过,由于做了一定的假设,所以它只在 t = 0 附近有效。我们看到,旋转矩阵 R 与另一个反对称矩阵 ϕ 0 通过指数关系发生了联系。也就是说,当我们知道某个时刻的 R 时,存在一个向量 ϕ,它们满足这个矩阵指数关系。