第三讲_三维空间刚体运动
如何描述刚体（尺寸有限无形变的固体）在三维空间中的运动
旋转矩阵
点和向量，坐标系
相机可以看作三维空间中的刚体，则，它的位姿：位置由三维坐标表示，姿态？
用向量的外积表示向量的旋转
第一项为行列式，第三项把向量a写成矩阵的形式。
a×b的结果是垂直于ab平面大小为|a||b|sin(a,b)方向满足右手法则的旋转向量。
写成a的反对称矩阵与b相乘，则多次旋转时计算为线性的。
坐标系间的欧氏变换
坐标系是由一组基来表示的，坐标系的旋转，实质上是这组基的旋转。
坐标系旋转前后对同一个点的表示：
两边左乘基矩阵的转置（正交基）
a = Ra’
R为旋转矩阵。a’ = R-1a = RTa
旋转矩阵是一个行列式为1的正交矩阵。反之也成立。
加上平移后：a’ = Ra + t;
旋转矩阵的集合称为特殊正交群。
变换矩阵与齐次坐标
矩阵和向量的乘法表示，向量的变换，是一种线性变换，而加法不是。
把旋转和平移写在一个变换矩阵，三维向量的末尾添1变成四维向量，称齐次坐标，这个四维向量的变换只有矩阵的乘法，即线性变换。
齐次坐标的最后一维可以为任意非零的实数，都表示同一个点。当它为0时表示坐标轴。
变换矩阵的集合称为特殊欧式群。
变换矩阵不是正交矩阵。利用初等变换可以方便求出T的逆矩阵。
实践Eigen
sudo apt-cache search 软件名
在Ubuntu软件源中查找是否提供软件
sudo apt-cache show 软件名
查看软件的详细信息
locate 软件名
查看PC是否以及安装某软件
Eigen完全由头文件构成，意味着使用时只需要引用头文件，不需要链接库文件。
C 库函数 clock_t clock(void) 返回程序执行起（一般为程序的开头），处理器时钟所使用的时间。为了获取 CPU 所使用的秒数，您需要除以 CLOCKS_PER_SEC。
旋转向量和欧拉角
旋转向量
旋转矩阵9个量，旋转3个自由度。
对于坐标系，可用一个旋转轴和一个旋转角描述它的旋转。
旋转向量的长度表示旋转角的大小，方向与旋转轴一致。也称轴角（Axis-Angle）
Rn = n
n是R特征值1对应的向量。
解方程：两边同时乘单位矩阵，行列式为0。
IRn = In
(R - I)n = det(R - I) = 0
旋转向量就是李代数。
欧拉角
偏航角——yaw
俯仰角——pitch
滚转角——roll
三维向量[r, p, y]T表示任意旋转。rpy角的旋转顺序是ZYX
**万向锁（奇异性）**当pitch角为±90° Z和X的旋转相同。
不会直接应用欧拉角直接表达姿态，转换成欧拉角，可用来快速验证算法的正确性。
四元数
没有不带奇异性的三维向量描述方式（旋转向量、欧拉角……）
四元数紧凑、无奇异。
四元数由一个实部三个虚部构成。三个虚部可用向量表示。
旋转向量：[nx, ny, nz]T
θ加2Π，四元数变为-q，即任意旋转都可以由两个互为相反数的四元数表示。
用四元数表示旋转
q = [cosθ/2， nsinθ/2]
p’ = qpq-1 四维——三维