SLAM荟萃 （2）三维空间下旋转的表示_使用向量定义物体状态

目录

一.旋转矩阵，旋转向量，欧拉角及四元数

二.轴角向量的运用

1.旋转向量：求旋转的残差

2.四元数：球面插值

---

 

一.旋转矩阵，旋转向量，欧拉角及四元数

什么是旋转：

    旋转是三维空间下物体朝向的变换，当物体发生旋转时，物体上所有点在全局坐标系下的坐标也会依靠同样的规则发生改变，旋转的主要作用是进行三维空间全局坐标系下的坐标变换以及描述物体朝向。假设物体原始朝向为A,旋转后朝向为B，可以将物体由A到B的旋转描述为，其表示物体由B姿态下旋转为A姿态，很显然旋转只有乘法，没有加法，并且只满足乘法结合律不满足交换律。我们可以说物体由A姿态旋转到B，再旋转到C，等同于物体由A姿态直接旋转到C姿态.目前eigen下旋转的表达方式有旋转矩阵，旋转向量（轴角法），欧拉角，四元数

旋转矩阵:

    从三维空间的坐标转换出发，当物体由A朝向旋转到B朝向时，物体上每一点在世界坐标系下的坐标都依照某种统一的规则发生了变换，旋转矩阵便描述了这种规则。旋转矩阵是3X3正交矩阵，三维空间下点 p 左乘旋转矩阵，便为将p由B坐标系下的坐标转换为A坐标系下的坐标，旋转矩阵相乘为空间两个3X3矩阵相乘，共需27次乘法和18次加法，共占用9个浮点数的内存，旋转矩阵乘向量共需9次乘法和7次加法.对角线全为1，其他元素为0的旋转代表了单位旋转.

                                                                                              

以上公式代表相机坐标系下一点P，经过由camera系到world系的坐标转换转移到世界坐标系下.

 

旋转向量:

   从实际物理意义出发，物体由朝向A转动到朝向B，也就是物体绕三维空间下一固定轴旋转一定角度，这里我们就可以通过旋转轴n(x,y,z)及旋转角来固定的描述这一旋转过程。其中旋转轴n为单位向量，可以将乘在旋转轴上的方式只通过三个浮点数来描述旋转，显然具有2π的周期性，以及仅通过旋转向量我们无法直观的将旋转运用于坐标变换上，同时无法知道两个连续的旋转向量组合后的旋转向量是什么样子，那么有没有方法来只通过计算和n的方式来实现连续旋转以及坐标转换呢？这里便引入了四元数.

 

四元数：

    通过旋转轴n加旋转角度唯一的描述了空间下的一个旋转，四元数便通过扩展n和以及四元数计算实现旋转的运用，具体旋转向量到四元数形式如下：

 

 

       四元数相关的旋转链接及坐标转换可以参考https://zhuanlan.zhihu.com/p/97186723.

       这里我们引入四元数的目的就是四元数的运算刚好能完成旋转传递以及坐标变换运算.可以发现当四元数乘四元数时需要16次乘法加12次加法，当四元数乘向量，需要做两次四元数叉乘，也就是大约16次乘法加12次加法,所以当采用四元数表达旋转时，旋转相乘运算量小于旋转矩阵，对坐标做变换运行量大于旋转矩阵.这也就是在视觉slam系统中表示旋转，因为涉及到空间坐标系下特征点位置的变换，每帧图像需要对几百个点做变换，因此采用旋转矩阵方式表示旋转.而在vins中，因为涉及到IMU运算，需要不断进行IMU预积分，所以用四元数来存储和表示旋转.值得一提的是这里四元数为单位四元数，四元数表示旋转时的逆旋转为其共轭四元数。

欧拉角：

      我们可以用轴角向量的方式来描述一个旋转，但我们还是很难想象这个旋转对于我们自身原始姿态做了哪些改变，欧拉角便是为了我们人类更好理解旋转而提出的变量，欧拉角将一个旋转分解为3次连续的旋转使之更符合我们人类的理解思维，欧拉角分为静态欧拉角和动态欧拉角两种，两者可以相互转换。https://blog.csdn.net/icevmj/article/details/80280929对此做了很详细的说明。在slam中主要以row-pitch-yaw动态欧拉角为主，表示物体先后按照俯仰，翻滚，偏航角度旋转的值，其中俯仰角取值在[0,pi]其余两个取值为[-pi,pi]，一般物体很少出现大的俯仰和翻滚的朝向变动，更多是在yaw角度上变换。

                                                            

         在动态欧拉角中会出现万向锁问题，这是由于在多次旋转中，每次旋转的旋转轴可能转到空间中同一位置，出现两次旋转描述旋转轴在空间中重复的现象，丢失了旋转自由度

 

总结：

      空间中表示旋转有两种方式，分别是基于物理意义的（轴角向量），和基于坐标变换的（旋转矩阵），将轴角向量的形式进行运算扩展使之能够拼接旋转以及进行坐标变换，便出现了四元数，四元数在连续旋转上运算量低于旋转矩阵，坐标变换上运算量多于旋转矩阵。而欧拉角则是为了方便人类理解旋转的过程，将一个旋转分为三个连续的绕（X,Y,Z）轴旋转，同时依据旋转过程中旋转轴在空间坐标系下是否固定分为动态欧拉角和静态欧拉角。

二.轴角向量的运用

1.旋转向量：求旋转的残差

         在slam中我们经常会需要来估计一个slam系统最终的精度误差，位置误差的计算我们可以通过欧式距离来计算，旋转误差该怎么衡量呢？

         旋转向量表达为物体绕固定轴旋转了角度，通过求预测姿态与ground truth 姿态的相对变换，并转换为旋转向量，这个相对变换的刚好可以代表我们预测旋转与真实旋转之间的误差。Eigen代码函数如下

void CalculateRotationError(const Eigen::Matrix3d& measure,const Eigen::Matrix3d& ground_truth){
  const Eigen::Matrix3d contrast = measure.transpose() * ground_truth;
  const Eigen::AngleAxisd rotation_vector(contrast);
  return rotation_vector.angle()
}

        

 

2.四元数：球面插值

         旋转的另一个使用上就是插值，当我们拥有旋转的ground truth时，ground truth频率是500ms一个，这时我们想评估一个30ms一个的视觉slam的系统位姿精度该怎么办呢？做法就是从500ms每个的真值插值出30ms每帧的pose作为真值。前面提到过不管是旋转矩阵还是四元数都只有乘法，不满足加法特性。 所以这里我们想要做平滑插值也就是求两帧之间的相对旋转，并针对相对旋转角做时间上的平滑，最后经过化简可以得到四元数平滑插值公式为

cos = p*q.