视觉SLAM十四讲-第三讲-三维空间刚体运动

视觉SLAM十四讲从理论到实践高翔清华大学2016年冬第三讲三维空间刚体运动Chapter3:3DSpaceRigidBodyMotion2第三讲三维空间刚体运动•本讲目标•理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角。•掌握Eigen库的矩阵、几何模块使用方法。33.1点、向量和坐标系，旋转矩阵4第三讲三维空间刚体运动•点存在于三维空间之中•点和点可以组成向量•点本身由原点指向它的向量所描述•向量•带指向性的箭头•可以进行加法、减法等运算5第三讲三维空间刚体运动•定义坐标系后，向量可以由坐标表示•坐标系：由三个正交的轴组成•构成线性空间的一组基•左手系和右手系6R3第三讲三维空间刚体运动•向量的运算可以由坐标运算来表达•加减法•内积•外积7这个定义之后还要用第三讲三维空间刚体运动•目前为止都很平凡•但是有一个基本问题：•进而：•在SLAM中：•固定的世界坐标系和移动的机器人坐标系•机器人坐标系随着机器人运动而改变，每个时刻都有新的坐标系8坐标系之间是如何变化的？如何计算同一个向量在不同坐标系里的坐标？第三讲三维空间刚体运动•两个不同的坐标系•如何描述左侧到右侧的变化？•直观看来由两个部分组成：•原点间的平移•三个轴的旋转•平移是一个向量•旋转是什么？9第三讲三维空间刚体运动•旋转•设某坐标系发生了一次旋转，变成了•对于某个固定的向量（向量不随坐标系旋转），它的坐标怎么变化？•坐标关系：10第三讲三维空间刚体运动•左乘，得：11第三讲三维空间刚体运动•中间的矩阵称为旋转矩阵•根据定义可以验证：•R是一个正交矩阵；•R的行列式为+1。•满足这两个性质的矩阵称为旋转矩阵•SpecialOrthogonalGroup特殊正交群•旋转矩阵描述了两个坐标的变换关系•比如：•反之：•于是：•进一步，三个坐标系亦有：12𝑅𝑎1=𝑅12𝑎2𝑎2=𝑅21𝑎1𝑅21=𝑅12−1=𝑅12𝑇𝑎3=𝑅32𝑎2=𝑅32𝑅21𝑎1=𝑅31𝑎1第三讲三维空间刚体运动•加上平移：•两个坐标系的刚体运动可以由完全描述。13𝑅,𝑡第三讲三维空间刚体运动•齐次坐标与变换矩阵•用旋转+平移方式有一点不便之处，比如发生了两次变换：•这时：•叠加起来过于复杂14第三讲三维空间刚体运动•改变形式，写成：•记•那么多次变换就可写成：•这种用四个数表达三维向量的做法称为齐次坐标•引入齐次坐标后，旋转和平移可以放入同一个矩阵，称为变换矩阵•称为特殊欧氏群（SpecialEuclideanGroup）15𝑎=𝑎1第三讲三维空间刚体运动•类似的，可定义反向的变换：16•例子•在SLAM中，通常定义世界坐标系与机器人坐标系•一个点的世界坐标为，机器人坐标系下为，那么满足关系：•反之亦然•在实际编程中，可使用或来描述机器人的位姿。𝑇𝑊𝑇𝑅𝑝𝑊𝑝𝑅𝑝𝑅=𝑇𝑅𝑊𝑝𝑊𝑇𝑅𝑊𝑇𝑊𝑅3.2实践部分：EIGEN173.3旋转向量、欧拉角18第三讲三维空间刚体运动•除了旋转矩阵之外的旋转表示•三维旋转：三自由度，用向量表示•方向为旋转轴、长度为转过的角度•称为角轴（Angle-Axis）或旋转向量（RotationVector）19𝑅3𝑤=𝜃𝑛第三讲三维空间刚体运动•角轴与旋转矩阵的不同•旋转矩阵：九个量，有正交性约束和行列式值约束•角轴：三个量，没有约束•注意它们只是表达方式的不同，但表达的东西可以是同一个•角轴也就是第四章要介绍的李代数20•转换关系•轴角转旋转矩阵：罗德里格斯公式（Rodrigues'sFormula）•旋转矩阵转轴角•角度：•轴：第三讲三维空间刚体运动•欧拉角（EulerAngles）•将旋转分解为三次不同轴上的转动，以便理解•例如：按Z-Y-X顺序转动•轴可以是定轴或动轴，顺序亦可不同，因此存在许多种定义方式不同的欧拉角•常见的有yaw-pitch-roll（偏航-俯仰-滚转）角等等。211.绕物体的Z轴旋转，得到偏航角yaw；2.绕旋转之后的Y轴旋转，得到俯仰角pitch；3.绕旋转之后的X轴旋转，得到滚转角roll。第三讲三维空间刚体运动•万向锁（GimbalLock）•ZYX顺序中，若Pitch为正负90度，则第三次旋转和第一次绕同一个轴，使得系统丢失了一个自由度——存在奇异性问题22第三讲三维空间刚体运动•由于万向锁，欧拉角不适于插值和迭代，往往只用于人机交互中。•可以证明，用三个实数来表达三维旋转时，会不可避免地碰到奇异性问题。•SLAM程序中很少直接使用欧拉角表达姿态233.4四元数24第三讲三维空间刚体运动•四元数（Quaternion）•一种扩展的复数•回忆：（单位圆上的）复数可以表达二维平面的旋转•四元数有三个虚部，可以表达三维空间中的旋转：•虚部之间的关系：25自己和自己的运算像复数自己和别人的运算像叉乘第三讲三维空间刚体运动•和复数一样，单位四元数可以表达三维空间的一次旋转•四元数的一些运算和性质：26加减法乘法乘法共轭模长逆数乘点乘第三讲三维空间刚体运动•四元数和角轴的关系•角轴到四元数：•四元数到角轴：•类似可知四元数亦可转换为旋转矩阵、欧拉角27第三讲三维空间刚体运动•如何用四元数旋转一个空间点？•设点经过一次以表示的旋转后，得到了，它们关系如何表示？1.将的坐标用四元数表示（虚四元数）：2.旋转之后的关系为：•四元数相比于角轴、欧拉角的优势：紧凑、无奇异性28𝑝𝑝′𝑞𝑝可以验证这货也是虚四元数3.5*相似、仿射、射影变换（略）293.6实践部分：EIGEN几何模块303.7可视化演示31