刚体的旋转

Eigen中的旋转操作

旋转矩阵

• 构造

Eigen::Matrix3d rotation_matrix;
rotation_matrix<< 0.707107,  -0.707107,  0,
         		  0.707107,   0.707107,  0,
         		  0,          0,         1;

---

四元数

• 构造

//直接构造
Eigen::Quaterniond(1,0,0,0);

// 从旋转矩阵
Eigen::Matrix3d R; 
      R<< 0.707107,  -0.707107,  0,
          0.707107,   0.707107,  0,
          0,          0,         1;

Eigen::Quaterniond q(R);


// 从角轴
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度
Eigen::Quaterniond q(rotation_vector);

//从数组, 数组的顺序应该是[x y z w]
double array[4]= {
   1,2,3,4};
Eigen::Quaterniond q1(array); //相当于直接构造　q1(4,1,2,3);

• 读取系数

// 1. 
q.coeffs()  // x, y, z, w 的顺序
q.vec()　　　// x, y, z  虚数部分
q.x();
q.y();
q.z();
q.w();

• 其他常用操作

1. 归一化　normalize() :直接改原始的q，normalized() const ：返回一个归一化的q，原始不改变
2. 返回旋转矩阵toRotationMatrix
3. 取模 norm()
4. 模长的平方squaredNorm()
5. 取共轭conjugate()
6. 取逆 inverse()
7. 点乘dot

---

欧拉角

• 注意事项：

1. 常用的一种欧拉角顺序 yaw-pitch-roll, 所对应的坐标轴：Z, Y, X
2. 我们如果定义欧拉角：Eigen::Vector3d　euler_angle(X, Y, Z) ,　euler_angle(0) : roll …

// 1. 可以从旋转矩阵得到欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // 旋转顺序z,y,x; 得到欧拉角(yaw, pitch roll) 

---

角轴

• 注意事项：

1. 角轴的表示的轴必须是单位向量, 角是弧度制
2. 角轴的表示遵守右手定则，大拇指朝上，四指的方向是正
3. 角轴１(-M_PI/4, 0, 0, 1) 和 角轴2(M_PI/4, 0, 0, -1) 表示的是同一个旋转，绕z轴旋转-45度

• 构造

// 1. 从角轴
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度, 注意，角度制：弧度，轴：必须是单位的

// 2. 从四元数
Eigen::Quaterniond q1(1,1,1,1);
Eigen::AngleAxisd rotation_vector(q1);　// q1可以不是归一化的，自动取q1的归一化之后的q取构造

// 3.从旋转矩阵
Eigen::Matrix3d R;
     R<< 0.707107,  -0.707107,  0,
         0.707107,   0.707107,  0,
         0,          0,         1;
Eigen::AngleAxisd rotation_vector(R);

• 其他常用操作

1. 转到旋转矩阵 toRotationMatrix() const
2. 取角度(弧度制)angle()
3. 取轴(单位向量)axis() 自动单位化输出
4. 相反的旋转inverse() const

变换矩阵

• 构造

// 1. 可以从角轴和平移
    Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
    Eigen::Vector3d t(1,1,1);
    


    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(n);
    T.pretranslate(t);




// 2. 可以从四元数和平移
    Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
    Eigen::Quaterniond q(n);
    Eigen::Vector3d t(1,1,1);
    


    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(q);
    T.pretranslate(t);




// 3. 可以从旋转矩阵和平移
    Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d R = n.matrix();
    Eigen::Vector3d t(1,1,1);
    


    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(R);
    T.pretranslate(t);




// 4. 除了创建好对象再传递　旋转　和平移之外，还可以以旋转创建，再传递平移,以下以旋转矩阵为例，其他两种一样可以
    Eigen::Isometry3d T(R);
    T.pretranslate(t);

• 常用操作

1. p = T*v T内部重载了符号，虽然T是四维的, v和p都是3维的，但是相当于p = Rv+t的操作
2. translation() 取平移部分
3. rotation()　取旋转矩阵

---

各种旋转之间的转化

四元数和角轴

单位四元数：$(q_0,q_1,q_2,q_3)$   角轴：$n\theta$    

• 四元数到角轴，得到轴是单位的

$$\theta =2arc\cos q_0$$

$$n=[n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}$$

• 角轴到四元数轴，$n=(n_x,n_y,n_z)$是单位的，得到四元数是单位的

$$q=[q_0,q_1,q_2,q_3{]}^T=[\cos \frac{\theta }{2},\text{n}sin\frac{\theta }{2},{]}^T=[\cos \frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2}{n}_{z}sin\frac{\theta }{2}{]}^T$$

---

四元数和旋转矩阵

单位四元数：$(q_0,q_1,q_2,q_3)$   旋转矩阵R：

• 四元数到旋转矩阵

R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R= \left[ \begin{matrix} 1-2q_2^2-2q_3^2 &amp; 2q_1q_2-2q_0q_3 &amp; 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 &amp; 1-2q_1^2-2q_3^2 &amp; 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 &amp; 2q_2q_3+2q_0q_1 &amp; 1-2q_1^2-2q_2^2 \end{matrix} \right] R=⎣⎡​1−2q22​−2q32​2q1​<