slam中的旋转表示

旋转表示

以下是角轴、四元数、旋转矩阵、欧拉角的表示及其相互转换。

角轴

对于两个坐标系${\mathcal{F}}_1$、${\mathcal{F}}_2$，其坐标轴表示如下，两个坐标系之间的关系我们可以这样表述，坐标系${\mathcal{F}}_1$沿着z轴旋转$\theta$角得到${\mathcal{F}}_2$系，这里的z轴，即为角轴中的旋转轴，$\theta$为角轴中的旋转角。

图 1

角轴=>旋转矩阵

一般的，我们定义旋转轴为$\mathbf{u}=[u_1,u_2,u_3{]}^T$，绕着$\mathbf{u}$旋转的角度为$\theta$，根据罗德里格斯公式，可以将角轴转换为旋转矩阵，具体如下，设空间中一点在${\mathcal{F}}_1$系中坐标为${\mathbf{v}}^1$，在${\mathcal{F}}_2$系中坐标为${\mathbf{v}}^2$，有${\mathbf{v}}^1={\mathbf{C}}_2^1{\mathbf{v}}^2$。
$${\mathbf{C}}_2^1=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{u}{\mathbf{u}}^{\mathbf{T}}+\sin \theta {\mathbf{u}}^{\wedge }\text{\hspace{1em}(1)}$$
Eigen库中的角轴转换为旋转矩阵就是按照公式$(1)$计算的，代码如下，

Eigen::Matrix3f testRz;
Eigen::Vector3f unitz(0, 0, 1); 
testRz = Eigen::AngleAxisf(M_PI/2, unitz);

计算结果如下，

以上为角轴与旋转矩阵的转换关系。

---

角轴=>四元数

Eigen中的四元数定义如下，
$$\mathbf{q}=\cos \frac{\theta }{2}+\mathbf{u}\sin \frac{\theta }{2}\text{\hspace{1em}(2)}$$
其中的$\mathbf{u}$和$\theta$和角轴中的定义一样，公式$(2)$定义的四元数描述的是“将坐标系${\mathcal{F}}_1$沿$\mathbf{u}$旋转$\theta$变成坐标系${\mathcal{F}}_2$”，其中$\mathbf{u}$是定义在${\mathcal{F}}_1$系的。设空间中一点在${\mathcal{F}}_1$系中坐标为${\mathbf{v}}^1$，在${\mathcal{F}}_2$系中坐标为${\mathbf{v}}^2$，有${\mathbf{v}}^1=\mathbf{q}{\mathbf{v}}^2{\mathbf{q}}^{\ast }$，通常情况下，$\mathbf{q}$可记为${\mathbf{q}}_2^1$，是一个单位四元数。利用角轴构造四元数代码如下，

Eigen::AngleAxisf utheta;
utheta = Eigen::AngleAxisf(M_PI/2, unitz);
Eigen::Quaternionf q(utheta);

q的各个分量打印如下，

利用同一角轴构造的旋转矩阵和四元数，表示的是同一个旋转。

四元数

Eigen中四元数还可以采用两个向量来构造，这两个向量可以求出轴和角，设这两个向量为$v_1,v_2$，那么角轴为$v_1\times v_2$，示例代码如下，

Eigen::Vector3f v1(1, 0, 0);
Eigen::Vector3f v2(0, 1, 0);
qutheta = Eigen::Quaternionf::FromTwoVectors(v1, v2);

可以看出$v_1,v_2$构造出的旋转是按z轴以右手系正向旋转90°，其结果应该与由角轴构建的四元数相同，结果如下，

这个可以方便得求出当前坐标系与水平系的旋转，设水平系中，重力为$\mathbf{g}=[0,0,g]$，其中$g\approx 9.8$，这里没有假设g为负，是因为imu水平时，测得的重力为g，是正值，这是由imu测量原理决定的。由imu静止状态下测得的重力为$\hat{\mathbf{g}}$，$\hat{\mathbf{g}}\times \mathbf{g}$得到的旋转就是水平系到测量系的旋转，带入代码中，即可得到$C_{测量系}^{水平系}$。

四元数=>旋转矩阵

四元数转换为旋转矩阵，可以按四元数中定义的$\mathbf{u}$和$\theta$，按式$(1)$计算，还可以按求导来计算，主要思想如下，
$$Rp=q\otimes p\otimes q^{\ast }={\lfloor q\rfloor }_L{\lfloor q^{\ast }\rfloor }_{R}p\text{\hspace{1em}(3)}$$

$$R=(q_0^2-{\Vert q_v\Vert }^2){\mathbf{I}}_{3\times 3}+2q_v{q}_v^T+2q_0{q}_v^{\wedge }\text{\hspace{1em}(4)}$$

旋转矩阵$R$为${\lfloor q\rfloor }_L{\lfloor q^{\ast }\rfloor }_R$的右下角$3\times 3$子块，Eigen库中就是按照公式$(4)$来计算的，以下为Eigen中$q$转为$R$的代码，计算方法与公式$(4)$是一样的。

template<class Derived>
inline typename QuaternionBase<Derived>::Matrix3
QuaternionBase<Derived>::toRotationMatrix(void) const
{
  // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
  // if not inlined then the cost of the return by value is huge ~ +35%,
  // however, not inlining this function is an order of magnitude slower, so
  // it has to be inlined, and so the return by value is not an issue
  Matrix3 res;

  const Scalar tx  = Scalar(2)*this->x();
  const Scalar ty  = Scalar(2)*this->y();
  const Scalar tz  = Scalar(2)*this->z();
  const Scalar twx = tx*this->w();
  const Scalar twy = ty*this->w();
  const Scalar twz = tz*this->w();
  const Scalar txx = tx*this->x();
  const Scalar txy = ty*this->x();
  const Scalar txz = tz*this->x();
  const Scalar tyy = ty*this->y();
  const Scalar tyz = tz*this->y();
  const Scalar tzz = tz*this->z();

  res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
  res.coeffRef(0,1) = txy-twz;
  res.coeffRef(0,2) = txz+twy;
  res.coeffRef(1,0) = txy+twz;
  res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
  res.coeffRef(1,2) = tyz-twx;
  res.coeffRef(2,0) = txz-twy;
  res.coeffRef(2,1) = tyz+twx;
  res.coeffRef(2,2) = Scalar(1)-(txx+tyy);

  return res;
}

欧拉角

目前常用的是yaw->pitch->roll的约定，即${\mathcal{F}}_1$系先按z轴以右手系旋转yaw角得到$\mathcal{T}$系，以$\mathcal{T}$系为基础，按$\mathcal{T}$系的y轴以右手系旋转pitch角得到$\mathcal{I}$系，以$\mathcal{I}$系为基础，按$\mathcal{I}$系的x轴以右手系旋转roll角，得到${\mathcal{F}}_2$系，这里yaw pitch roll即为欧拉角。根据约定的不同，欧拉角有多种形式，但是目前仅使用过上述形式。

欧拉角=>旋转矩阵

在Eigen中可以根据角轴得到旋转矩阵，
$${\mathbf{C}}_2^1={\mathbf{C}}_{\mathcal{T}}^1{\mathbf{C}}_{\mathcal{I}}^{\mathcal{T}}{\mathbf{C}}_2^{\mathcal{I}}\text{\hspace{1em}(5)}$$
式$(5)$中的3个分量均可根据式$(1)$计算得到，以下为代码

Eigen::Matrix3f Rypr;
Eigen::Vector3f Unitz(0, 0, 1);
Eigen::Vector3f Unity(0, 1, 0);
Eigen::Vector3f Unitx(1, 0, 0);
float yaw = M_PI/4, pitch = M_PI/4, roll = M_PI/4;
// 以下得到的就是公式(5)中的C12
Rypr = Eigen::AngleAxisf(yaw, Unitz)
    * Eigen::AngleAxisf(pitch, Unity)
    * Eigen::AngleAxisf(roll, Unitx);

关于欧拉角更详细的内容见此处。

旋转矩阵

旋转矩阵=>欧拉角

根据公式$(5)$可以找到旋转矩阵与欧拉角的关系，在Eigen中根据旋转矩阵${\mathbf{C}}_2^1$，按以下代码可以得到按yaw-pitch-roll的欧拉角，需要注意的是，得到的结果顺序为[yaw, pitch, roll]。代码中的2，1，0代表的是按哪个轴旋转，2代表z，1代表y，0代表x，即$R=R_z{R}_y{R}_x$

//Rypr为C12
Eigen::Vector3f ypr = Rypr.eulerAngles(2, 1, 0);

结果为，

罗德里格斯公式

可以参考这里，利用性质$a^{\wedge }{a}^{\wedge }=a{a}^T-\mathbf{I}$，其中$a$是单位向量，可以得到罗德里格斯公式两种形式，将式$(1)$中的$u{u}^T$换成$u^{\wedge }{u}^{\wedge }+\mathbf{I}$即可。