在 SLAM 中,除了表示位姿之外,我们还要对它们进行估计和优化。因为在 SLAM 中位姿是未知的,而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题,求解最优的 R, t,使得误差最小化。而估计、优化是需要进行迭代计算的。但是,旋转矩阵自身是带有约束的(正交且行列式为 1)。它们作为优化变量时,会引入额外的约束,使优化变得困难。通过李群——李代数间的转换关系,我们希望把位姿估计变成无约束的优化问题,简化求解方式。
三维旋转矩阵构成了特殊正交群 SO(3),而变换矩阵构成了特殊欧氏群 SE(3):
李群是指具有连续(光滑)性质的群。像整数群 Z 那样离散的群没有连续性质,所以不是李群。而 SO(n) 和 SE(n),它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动,所以它们都是李群。
考虑任意旋转矩阵R,满足 : (旋转完一定可以逆旋转回来)
令R随时间变化(连续运动),有:
两侧对时间求导,可得: 即:
可以看出,这是一个反对称矩阵(转置取负),记为:
两侧右乘R(t):,可以认为,对R求导后,左侧多出一个∅(t)
单位元附近泰勒展开:
可见∅反映了一阶导数性质,它位于正切空间(tangent space)上,在t0附近,假设∅不变,有微分方程:
已知初始情况:R(0)=I ,解之,得:
上式说明,对任意t,都可以找到一个R和一个的∅对应关系,该关系称为指数映射。在这里这个∅称为SO(3)对应的李代数:so(3)
李代数
定义:
其中二元运算[,]被称为李括号,直观上说,李括号表达了两个元素的差异。对于每个李群,都有与之对应的李代数,李代数描述了李群单位元附件的正切空间性质。那么对于李代数so(3),我们可以定义:
其中
李括号内的运算为: 同理可得,SE(3)亦有李代数se(3)
指数映射和对数映射
指数映射反映了从李代数到李群的对应关系 ,但是ϕ^是一个矩阵,对于矩阵,如何定义求指数运算?由于ϕ^是向量,定义其角度和模长可得: 然后对exp(ϕ^)泰勒展开可得:
整理:
而在旋转矩阵中罗德里格斯公式即这个公式,所以,我们可以认为so(3) 的物理意义就是旋转向量。反之,给定旋转矩阵时,亦能求李代数:称为对数映射,但实际当中没必要这样求,矩阵到向量的转换关系可以由矩阵的迹得到
在此我们已经得出了李群和李代数的对应关系,下面总结一下在SO(3)和SE(3)中的对应关系。