本文參考：計算機視覺life 僅作筆記用
書接上回，上回不清不楚的介紹了旋轉矩陣&旋轉向量和四元組
現在回顧一下重點：
本著繞誰誰不變的變則
假設繞z軸旋轉θ，旋轉矩陣為：

再回顧一下旋轉向量的表示以及這個基本記不住的羅德里格斯公式，記不住也沒事：

一個例子

已知旋轉矩陣定義是沿著Z軸旋轉45°。請按照該定義初始化旋轉向量、旋轉矩陣、四元數、歐拉角。請編程實現：

1、以上四種表達方式的相互轉換關系并輸出，并參考給出的結果驗證是否正確。

2、假設平移向量為（1,2,3）,請輸出旋轉矩陣和該平移矩陣構成的歐式變換矩陣，并根據歐式變換矩陣提取旋轉向量及平移向量。

#include <iostream>
#include <Eigen/Dense>using namespace Eigen;int main() {// 初始化旋轉參數double angle = M_PI/4; // 45°轉換為弧度Vector3d axis = Vector3d::UnitZ(); // Z軸// 初始化旋轉向量Vector3d rotation_vector = angle * axis;// 初始化旋轉矩陣Matrix3d rotation_matrix;// AngleAxisd(angle, axis) 創建了一個表示繞著給定軸 axis 旋轉 angle 弧度的旋轉向量。//在Eigen庫中，AngleAxisd 類表示一個旋轉向量，它由一個旋轉軸和一個旋轉角度組成。//它的構造函數可以接受旋轉角度和旋轉軸作為參數，也可以接受一個旋轉矩陣作為參數，并從中提取旋轉軸和旋轉角度。//調用 toRotationMatrix() 方法將該旋轉向量轉換為對應的旋轉矩陣。rotation_matrix = AngleAxisd(angle, axis).toRotationMatrix();// 初始化四元數Quaterniond quaternion(rotation_matrix);// 初始化歐拉角Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX順序// 輸出四種表達方式 // 旋轉向量std::cout << "Rotation Vector: " << rotation_vector.transpose() << std::endl;  // .transpose()方法被用于將向量和矩陣輸出成為行向量的形式，以方便顯示在控制臺上。// 旋轉矩陣std::cout << "Rotation Matrix:\n" << rotation_matrix << std::endl;// coeffs()方法用于獲取四元數對象的系數，返回一個四維向量。std::cout << "Quaternion: " << quaternion.coeffs()<< std::endl;// 歐拉角std::cout << "Euler Angles (ZYX): " << euler_angles.transpose() << std::endl;// 假設平移向量為（1,2,3）Vector3d translation_vector(1, 2, 3);// 構造歐式變換矩陣Matrix4d euclidean_transform_matrix = Matrix4d::Identity();  // 創建一個4x4的單位矩陣的靜態方法// 將旋轉矩陣賦值給歐幾里德變換矩陣的左上角的3x3子矩陣，即旋轉部分euclidean_transform_matrix.block<3,3>(0,0) = rotation_matrix;// 將平移向量賦值給歐幾里德變換矩陣的右上角的3x1子矩陣，即平移部分euclidean_transform_matrix.block<3,1>(0,3) = translation_vector;/* 在Eigen庫中，AngleAxisd()是用于創建一個旋轉向量（Angle-Axis）的構造函數。Angle-Axis旋轉表示通過一個旋轉軸和一個旋轉角度來描述旋轉。具體來說，AngleAxisd()的使用方法如下Eigen::AngleAxisd angle_axis(angle, axis);在這段代碼中，AngleAxisd(rotation_matrix)創建了一個AngleAxisd類型的對象，該對象代表了由旋轉矩陣rotation_matrix表示的旋轉。然后，.axis()方法用于獲取該旋轉向量的旋轉軸，而.angle()方法用于獲取旋轉角度。接著，AngleAxisd(rotation_matrix).axis() * AngleAxisd(rotation_matrix).angle()部分將旋轉向量的軸乘以旋轉角度，這將得到一個旋轉向量，其方向由旋轉軸決定，大小由旋轉角度決定。這個旋轉向量被賦值給了extracted_rotation_vector。*/// 從歐式變換矩陣分解出旋轉向量和平移向量Vector3d extracted_rotation_vector = AngleAxisd(rotation_matrix).axis() * AngleAxisd(rotation_matrix).angle();/* block<3,1>(0,3)是Eigen庫中的一個函數，用于從矩陣中提取特定的塊。在這里，block<3,1>(0,3)表示提取了從第0行開始，第3列開始的一個3x1的塊，即歐氏變換矩陣的前三個元素（0, 1, 2行，3列），這通常是表示平移向量的部分。*/ Vector3d extracted_translation_vector = euclidean_transform_matrix.block<3,1>(0,3);// 輸出歐式變換矩陣、提取的旋轉向量和平移向量// 歐式變換矩陣std::cout << "\nEuclidean Transformation Matrix:\n" << euclidean_transform_matrix << std::endl;// 旋轉向量std::cout << "Extracted Rotation Vector: " << extracted_rotation_vector.transpose() << std::endl;// 平移向量std::cout << "Extracted Translation Vector: " << extracted_translation_vector.transpose() << std::endl;return 0;
}

多看幾遍代碼，俺就算基本入第一道門了。。。