视觉SLAM之三维空间刚体运动描述
- 数学工具:旋转矩阵,变换矩阵,四元数,欧拉角
- 编程库:Eigen库
"旋转矩阵"
既然是说刚体运动,首先刚体是什么?刚体指既有位置又有姿态(朝向)的物体;也就是说,要描述刚体的运动,就必须既要描述刚体的位置,还要描述刚体的姿态(朝向).
描述刚体运动,最重要的数学工具是线性代数中的点和向量.而要使用向量,必须要明白坐标和向量的关系.向量是线性空间中的一个对象,而描述这个对象的方法就是坐标.对于坐标和向量的关系,一个不准确的例子就是,把向量看做是线性空间的一个人,坐标是这个人的代号,那么,不管这个这个人叫什么代号,这个人本身没有变化.为什么这个人有不同的代号?因为这个人在学校环境下,有学号,作为公民有身份证号等.回到线性代数的向量,不管坐标是什么,向量作为对象不会改变;为什么一个向量会有不同的坐标呢?是因为描述向量使用的参考系不同,也就是使用的基坐标不同,所以会有不同的坐标.比如:
其中为基坐标,为向量在这组基坐标下的坐标.所以,描述向量之前,必须先指定基坐标.
既然同一个向量会因为不同的基坐标而产生不同的坐标,那么这些坐标之间有事吗关系呢?
有的,假设有基坐标组成的A坐标系和基坐标组成的B坐标系,那么向量的坐标分别为和.那么,因为向量本身没有变,因此有:
如果,在两边同时左乘那么就得到了两个坐标之间的关系:
那么R就描述了两个坐标之间的转换,这个转换只包括旋转,而不包括位移,因此R成为旋转矩阵.
可以看到,旋转矩阵是两组基向量的向量积组成,通常情况下,基坐标都是单位正交向量,因此,旋转矩阵一个单位正交矩阵,反过来,一个单位正交矩阵也可以作为一个旋转矩阵,因此,把这一类矩阵有一个集合表示如下:
表示特殊正交群,其中表示维度.这样的旋转矩阵有个很好的特点.因为旋转矩阵是单位正交矩阵,因此,如果一个旋转矩阵为,那么和这个旋转矩阵相反的旋转矩阵就为.
说完了旋转矩阵,那么在SLAM中怎么应用呢?一般在SLAM中会定义一个世界坐标和一个相机坐标.世界坐标反应现实坐标,一般固定不懂,相机坐标是物体相对与相机的坐标,反应在相机拍摄的照片中.那么如何把图片中的物体找到现实的位置呢?就是使用旋转矩阵,通过世界坐标的基向量和相机坐标的基向量找到两个坐标系下坐标的关系,然后就可以通过旋转矩阵把照片中的物体的坐标找到现实的位置了.
齐次坐标和变换矩阵
旋转矩阵就可以描述坐标在两个坐标系中的关系了,但是这种关系只能描述矩阵的旋转,不能描述如果向量移动了,两个坐标系中的坐标的关系.如果要加上位移,怎么表示呢?其实是需要在两个坐标上同事加上位移就可以了,比如有一个位移,那么:
这样就完成了物体的运动在两个坐标系下的转换.
但是这种表示有一个缺点,就是这种表示并不是线性的,比如:
那么从到的变换表示为:
这样造成多次运动后,转换变得复杂,为了解决这个问题,引入了变换矩阵和齐次坐标.
所谓的齐次坐标,就是在原来的坐标上增加一个维度,由于原来的坐标在增加那个维度的数值可以有无数种,同时,一个低维坐标在高维中也有无数中表示方法,所以,增加维度后的坐标称为齐次坐标,例如:
那怎么把齐次坐标变回原来的坐标呢?如下:
我们把称为非齐次坐标,这样坐标的前三个值就是原来的坐标了.
说明了齐次坐标,怎么使用齐次坐标来克服之前坐标变换是非线性的问题呢?把原来的坐标增加一个维度,值为1,那么可表示为:
那么称为变换矩阵,这个矩阵把旋转和位移统一了起来.那么就可以表示为:
这样,旋转和位移的变换就称为一个线性的运算.
同理,使用特殊欧式群表示这一类的变换矩阵为:
那么一个变换矩阵的反变换可以表示为:
说完了向量的坐标之间的关系,那么如果要找到两个基坐标之间的关系怎么做呢?原理是相同的,只要两边同时右乘坐标,就得到了两组基之间的关系.
向量基本运算
接着说一下向量的基本运算.
向量运算包括数乘,向量加法,向量减法,向量内积,向量外积等.其中,数乘和向量四则运算属于基本内容.这里只简单说向量的内积和外积.
向量外积
向量的外积也叫做点积/点乘,公式为:
向量的内积描述的是两个向量之间的投影关系,内积的结果是一个标量.
向量外积
向量的外积也叫作叉乘,公式为:
外积的结果是一个向量,这个向量垂直与和组成的平面垂直;并且大小为:
外积只在三维向量中有定义.外积可以用来表征两个向量的旋转,因为外积的正负可以表征向量的旋转方向,大小可以计算旋转的角度大小.而内积只能计算角度大小,不能表示旋转的方向.
视觉SLAM十四讲