SLAM总结（二）-数学基础之位姿表示

1.位姿的不同表达方式：位姿的表达方式是表达2个坐标系之间相对关系的数学方法，由旋转和位移组成。SLAM中一般有每帧传感器帧坐标系在世界坐标（世界坐标系的重要作用是提供一个统一的参考，坐标系的原点及姿态如何并不是特别重要，根据实际选定即可，如SLAM中原点一般以初始化成功的地方，可能在空间的任何地方）下的位姿Twc（待求状态变量）、两传感器帧之间的相对位姿。这里统一使用右手坐标系，表达方式如下：
1)变换矩阵，可以表示旋转和平移
旋转（姿态）R用3x3的正交矩阵(旋转矩阵)表示，具有3个自由度。位移t用3x1的列向量表示。为了简化计算通常将两者组合成（欧式）变换矩阵T，最后一列为4x1的齐次坐标，如下式所示。

常见坐标表示T_WC,T_c^w(从左往右，从上到下)都表示从（from）C坐标系到（to）W坐标系的变换，以W作为参考坐标系，a、b、c分别是C的x、y、z轴在W中的向量，t是C的坐标原点在W的位置。对于相似变换S_WC中，sR为w坐标系下的非单位正交向量，t为c坐标系原点在w坐标系的坐标，尺度与w坐标系（参考坐标系）一样。
注：正交矩阵的性质
a)行向量、列向量的模都为1。
b)行向量、列向量两两相互正交，即点积（内积）为0
c)矩阵的转置等于矩阵的逆，即U^TU=U^-1U=I
d)正交矩阵相乘结果仍是正交矩阵，构成群
e)保范性：正交矩阵乘个向量，即(Ux)^T(Ux) = x^TU^TUx = x^Tx
f)正交矩阵的特征值只能是1或-1
2)四元数：只能表示旋转，q=q_Rb表示R绕旋转轴u旋转θ角与b系重合
单位四元数q表示旋转，其形式为q=[q0,q1,q2,q3]^T。q1,q2,q3为虚部，q0为实部。其约束是四