剛體的旋轉

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}}, \textbf{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= \left[ \begin{matrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{matrix} \right]$

• 旋轉矩陣到四元數
  假設$R={m_{ij}},i,j\in[1,2,3]$

$q_0= \frac{\sqrt{tr(R)+1}}{2} \space\space tr(R)代表跡$

$q_1 =\frac{m_{23}-m_{32}}{4q_0}$

$q_2 = \frac{m_{31}-m_{13}}{4q_0}$

$q_3 =\frac{m_{12}-m_{21}}{4q_0}$

注意：當$q_0\rightarrow 0$的時候，也就是R的跡趨近－１，其他三個分量很大，導致解不穩定，則應用下面的公式轉化

1. 如果$max( m_{11}, m_{22}, m_{33}) = m_{11}$

$t= \sqrt{1+m_{11}-m_{22}-m_{33}}\space\space$

$q_0 =\frac{m_{32}-m_{23}}{t}$

$q_1 =\frac{t}{4}$

$q_2 = \frac{m_{31}+m_{13}}{t}$

$q_3 =\frac{m_{12}+m_{21}}{t}$

2. 如果$max( m_{11}, m_{22}, m_{33}) = m_{22}$

$t= \sqrt{1-m_{11}+m_{22}-m_{33}}\space\space$

$q_0 =\frac{m_{13}-m_{31}}{t}$

$q_1 = \frac{m_{12}+m_{21}}{t}$

$q_2 = \frac{t}{4}$

$q_3 =\frac{m_{32}+m_{23}}{t}$

3. 如果$max( m_{11}, m_{22}, m_{33}) = m_{33}$

$t= \sqrt{1-m_{11}-m_{22}+m_{33}}\space\space$

$q_0 =\frac{m_{21}-m_{12}}{t}$

$q_1 =\frac{m_{13}+m_{31}}{t}$

$q_2 = \frac{m_{23}-m_{32}}{t}$

$q_3 =\frac{t}{4}$

---

四元數和歐拉角

單位四元數：$(q_0, q_1, q_2, q_3)$   　歐拉角：$(yaw, pitch, roll)$, 分別爲繞Z軸、Y軸、X軸的旋轉角度，記作$\psi,\theta, \phi$

• 四元數到歐拉角

$roll = \phi = \arctan(\frac{2(q_0q_1+q_2q_3)}{1-2(q_1^2+q_2^2)})$

$pitch = \theta =\arcsin(2(q_0q_2-q_3q_1))$

$yaw = \psi = \arctan(\frac{2(q_0q_3+q_1q_2)}{1-2(q_2^2+q_3^2)})$

• 歐拉角到四元數

$q=\left[ \begin{matrix} q_0 \\ q_1 \\ q_2\\ q_3 \end{matrix} \right]=\left[ \begin{matrix} w \\ x \\ y\\ z \end{matrix} \right]=\left[ \begin{matrix} \cos\frac{\psi}{2} \cos\frac{\theta}{2} \cos\frac{\phi}{2} +\sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} \\ \cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}-\sin\frac{\psi}{2}\sin\frac{\theta}{2} \cos\frac{\phi}{2} \\ \cos\frac{\psi}{2} \sin\frac{\theta}{2} \cos\frac{\phi}{2} +\sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}\\ \sin\frac{\psi}{2} \cos\frac{\theta}{2} \cos\frac{\phi}{2}- \cos\frac{\psi}{2} \sin\frac{\theta}{2} \sin\frac{\phi}{2} \end{matrix} \right]$

---

角軸和旋轉矩陣

旋轉角$\theta$和單位方向軸$n=(x, y, z)$

• 角軸到旋轉矩陣

$R=\left[ \begin{matrix} \cos\theta+x^2(1-\cos\theta) & -z\sin\theta+xy(1-\cos\theta) & y\sin\theta+xz(1-\cos\theta)\\ z\sin\theta+xy(1-\cos\theta) & \cos\theta+y^2(1-\cos\theta) & -x\sin\theta+yz(1-\cos\theta) \\ -y\sin\theta+xz(1-\cos\theta) & x\sin\theta+yz(1-\cos\theta) & \cos\theta +z^2(1-\cos\theta) \end{matrix} \right]$

• 旋轉矩陣到角軸
  $\theta = \arccos(\frac{tr(R)-1}{2})$

  $\textbf{n} = R\textbf{n}$
  關於旋轉軸$\textbf{n}$，由於旋轉軸的向量在旋轉後不會變化，即$n=Rn$，也說明n是矩陣R的特徵值１對應的特徵向量，求解此方程，再歸一化即可。
  
  

---

角軸和歐拉角

• 角軸到歐拉角

• 歐拉角到角軸

---

歐拉角和旋轉矩陣

歐拉角：$(yaw, pitch, roll)$, 分別爲繞Z軸、Y軸、X軸的旋轉角度，記作$\psi,\theta, \phi$。Ｂ:Body系，Ｗ: word系

• 歐拉角到旋轉矩陣

$R_{bw}= \left[ \begin{matrix} \cos\psi\cos\theta & \sin\psi\cos\theta & -\sin\theta \\ \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \sin\psi\sin\theta\sin\phi +\cos\psi\cos\phi& \cos\theta\sin\phi\\ \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi& \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi & \cos\theta\cos\phi \end{matrix} \right]$

$R_{wb}=R_{bw}^T= \left[ \begin{matrix} \cos\psi\cos\theta & \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi \\ \sin\psi\cos\phi & \sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi \\ -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{matrix} \right]$

• 旋轉矩陣到歐拉角
  假設$R={m_{ij}},i,j\in[1,2,3]$

1. 正常情況：
   $roll = \phi = \arctan(\frac{m_{32}}{m_{33}})$

$pitch = \theta = \arctan(\frac{-m_{31}}{\sqrt{m_{32}^2+m_{33}^2}})$

$yaw = \psi = \arctan(\frac{m_{21}}{m_{11}})$

2. 如果