本章需要掌握的知識點有：旋轉矩陣，變換矩陣，四元數，歐拉角定義和數學表達；同時也要掌握Eigen庫關于矩陣、幾何模塊的使用方法。

文章目錄

• • 3.1 旋轉矩陣
  • • 3.1.1 點，向量和矩陣的關系
    • 3.1.2 坐標系間的歐式變換
    • 3.1.3 變換矩陣與齊次坐標
  • 3.2 Eigen實踐
  • 3.3 旋轉向量和歐拉角（理解）
  • • 3.3.1 旋轉向量
    • 3.3.2 歐拉角
  • 3.4 四元數（常用）
  • • 3.4.3 用四元數表示旋轉
    • 3.4.4 四元數到其他旋轉表示的轉換
  • 3.6 Eigen幾何模塊
  • • 3.6.1 幾何模塊數據演示
    • 3.6.2 坐標轉換
  • 3.7 可視化演示

本章對應視頻為： https://www.bilibili.com/video/BV16t411g7FR?p=2

【高翔】視覺SLAM十四講

3.1 旋轉矩陣

3.1.1 點，向量和矩陣的關系

這里相對比較簡單，只需要理解向量內積和外積的計算方法即可，向量內積為：
$$a?b=a^{T}b=|a||b|cos\theta$$
其中$\theta$為向量$a,b$之間的夾角，向量內積的結果是一個標量。
外積相對比較復雜，公式為：
$$a\times b=\begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& a_3\end{array}=?\begin{array}{c}{a}_2{b}_3?a_3{b}_2\\ a_3{b}_1?a_1{b}_3\\ a_1{b}_2?a_2{b}_1\end{array}?=?\begin{array}{ccc}0& ?a_3& a_2\\ a_3& 0& ?a_1\\ ?a_2& a_1& 0\end{array}?b=a\wedge b$$
外積的結果是一個向量，方向垂直于這兩個向量。大小為$|a||b|sin\theta$。這里引入了反對稱矩陣，任意向量都有著唯一的一個反對稱矩陣，這里為：
$$a\wedge =?\begin{array}{ccc}0& ?a_3& a_2\\ a_3& 0& ?a_1\\ ?a_2& a_1& 0\end{array}?$$

---

3.1.2 坐標系間的歐式變換

這里引入了旋轉矩陣$\mathbb{R}$的概念，描述了向量從一組基中如何旋轉變換到另一組基，旋轉矩陣是一個行列式為1的正交矩陣，反之，行列式為1的正交矩陣也是一個旋轉矩陣。
除了旋轉變換外，還有平移，因此世界坐標系中的向量$a$，經過一次旋轉（用$\mathbb{R}$描述）和一次平移$t$后，得到了新的向量$a^{{}^{\prime }}$。
$$a^{{}^{\prime }}=Ra+t$$

---

3.1.3 變換矩陣與齊次坐標

這里引入齊次坐標和變換矩陣，對于上式可得：
$$\left[\begin{array}{c}{a}^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$$
這里將旋轉和平移寫在了一個矩陣里，整個關系變為線性關系，矩陣$T$為變換矩陣。變換矩陣的逆矩陣為：
$$T^{?1}=\left[\begin{array}{cc}{R}^T& ?R^{T}t\\ 0^T& 1\end{array}\right]$$

---

3.2 Eigen實踐

下面介紹了`Eigen關于向量，矩陣的使用。

#include <iostream>using namespace std;#include <ctime> // Eigen 核心部分 #include <Eigen/Core> // 稠密矩陣的代數運算（逆，特征值等） #include <Eigen/Dense>using namespace Eigen;#define MATRIX_SIZE 50/**************************** * 本程序演示了 Eigen 基本類型的使用 ****************************/int main(int argc, char **argv) {// Eigen 中所有向量和矩陣都是Eigen::Matrix，它是一個模板類。它的前三個參數為：數據類型，行，列// 聲明一個2*3的float矩陣Matrix<float, 2, 3> matrix_23;// 同時，Eigen 通過 typedef 提供了許多內置類型，不過底層仍是Eigen::Matrix// 例如 Vector3d 實質上是 Eigen::Matrix<double, 3, 1>，即三維向量Vector3d v_3d;// 這是一樣的Matrix<float, 3, 1> vd_3d;// Matrix3d 實質上是 Eigen::Matrix<double, 3, 3>Matrix3d matrix_33 = Matrix3d::Zero(); //初始化為零// 如果不確定矩陣大小，可以使用動態大小的矩陣Matrix<double, Dynamic, Dynamic> matrix_dynamic;// 更簡單的MatrixXd matrix_x;// 這種類型還有很多，我們不一一列舉// 下面是對Eigen陣的操作// 輸入數據（初始化）matrix_23 << 1, 2, 3, 4, 5, 6;// 輸出cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;// 用()訪問矩陣中的元素cout << "print matrix 2x3: " << endl;for (int i = 0; i < 2; i++) {for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";cout << endl;}// 矩陣和向量相乘（實際上仍是矩陣和矩陣）v_3d << 3, 2, 1;vd_3d << 4, 5, 6;// 但是在Eigen里你不能混合兩種不同類型的矩陣，像這樣是錯的// Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;// 應該顯式轉換Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;// 同樣你不能搞錯矩陣的維度// 試著取消下面的注釋，看看Eigen會報什么錯// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;// 一些矩陣運算// 四則運算就不演示了，直接用+-*/即可。matrix_33 = Matrix3d::Random(); // 隨機數矩陣cout << "random matrix: \n" << matrix_33 << endl;cout << "transpose: \n" << matrix_33.transpose() << endl; // 轉置cout << "sum: " << matrix_33.sum() << endl; // 各元素和cout << "trace: " << matrix_33.trace() << endl; // 跡cout << "times 10: \n" << 10 * matrix_33 << endl; // 數乘cout << "inverse: \n" << matrix_33.inverse() << endl; // 逆cout << "det: " << matrix_33.determinant() << endl; // 行列式// 特征值// 實對稱矩陣可以保證對角化成功SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;// 解方程// 我們求解 matrix_NN * x = v_Nd 這個方程// N的大小在前邊的宏里定義，它由隨機數生成// 直接求逆自然是最直接的，但是求逆運算量大Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN= MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);matrix_NN = matrix_NN * matrix_NN.transpose(); // 保證半正定Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);clock_t time_stt = clock(); // 計時// 直接求逆Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;cout << "time of normal inverse is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;// 通常用矩陣分解來求，例如QR分解，速度會快很多time_stt = clock();x = matrix_NN.colPivHouseholderQr().solve(v_Nd);cout << "time of Qr decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;// 對于正定矩陣，還可以用cholesky分解來解方程time_stt = clock();x = matrix_NN.ldlt().solve(v_Nd);cout << "time of ldlt decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;return 0; }

CMakeLists.txt文件：

cmake_minimum_required(VERSION 2.8) project(useEigen)set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-O3")# 添加Eigen頭文件 include_directories("/usr/include/eigen3") add_executable(eigenMatrix eigenMatrix.cpp)

執行結果：

matrix 2x3 from 1 to 6: 1 2 3 4 5 6 print matrix 2x3: 1 2 3 4 5 6 [1,2,3;4,5,6]*[3,2,1]=10 28 [1,2,3;4,5,6]*[4,5,6]: 32 77 random matrix: 0.680375 0.59688 -0.329554 -0.211234 0.823295 0.5364590.566198 -0.604897 -0.444451 transpose: 0.680375 -0.211234 0.5661980.59688 0.823295 -0.604897 -0.329554 0.536459 -0.444451 sum: 1.61307 trace: 1.05922 times 10: 6.80375 5.9688 -3.29554 -2.11234 8.23295 5.364595.66198 -6.04897 -4.44451 inverse: -0.198521 2.22739 2.83571.00605 -0.555135 -1.41603-1.62213 3.59308 3.28973 det: 0.208598 Eigen values = 0.02428990.9921541.80558 Eigen vectors = -0.549013 -0.735943 0.3961980.253452 -0.598296 -0.760134 -0.796459 0.316906 -0.514998 time of normal inverse is 0.454ms x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.778619.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734 time of Qr decomposition is 0.078ms x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734 time of ldlt decomposition is 0.021ms x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734

---

3.3 旋轉向量和歐拉角（理解）

3.3.1 旋轉向量

旋轉矩陣使用9個變量來描述旋轉，顯得有些冗余；同樣變換矩陣使用16個變量來描述變換，也是很冗余。因此有沒有一種方式能夠緊湊的描述旋轉和平移呢？
一次旋轉只有3個自由度，因此這里引入了旋轉向量，它的方向與旋轉軸一致，長度等于旋轉角，這樣就可以使用一個三維向量來描述旋轉。
從旋轉矩陣到旋轉向量的過程需要羅德里格斯公式轉換，這里直接給出旋轉矩陣和旋轉向量之間的轉換關系：
$$R=cos\theta I+(1?cos\theta )n{n}^T+sin\theta n\wedge$$
這里，$R$為旋轉矩陣，$n$為一個單位長度的向量，$\theta$為角度。對上式兩邊取跡，可以求出轉角：
$$\theta =arccos\frac{tr(R)?1}{2}$$
因為，旋轉軸上的向量在旋轉后不發生變化，可得：
$$Rn=n$$
因此，$n$為矩陣$R$特征值為1對應的特征向量，再歸一化，即可求出旋轉向量。

3.3.2 歐拉角

歐拉角其實就是3個分離的轉角，常見的有繞物體的$Z$軸旋轉的偏航角(yaw)，繞$Y$旋轉的俯仰角(pitch)，繞$X$軸旋轉的滾轉角(roll)。
工程上常會聽到$rpy$角，對應的旋轉順序為$ZYX$。

---

3.4 四元數（常用）

3.4.3 用四元數表示旋轉

四元數由實部和虛部組成，常見形式為：$q=q_0+q_{1}i+q_{2}j+q_{3}k$。關于四元數的運算這里不展開了，就是普通復數運算。
那么如何使用四元數來表達一個點的旋轉呢？假設有一個空間三維點$p=[x,y,z]$，以及一個單位四元數$q$指定的旋轉，三維點$p$經過旋轉之后變為$p^{\prime }$。用矩陣描述的話，則二者關系為$p^{\prime }=Rp$，用四元數表示則為：
$$p=[0,x,y,z{]}^T=[0,v{]}^T,p^{\prime }=qp{q}^{?1}$$
最后$p^{\prime }$的虛部為旋轉之后的坐標。

3.4.4 四元數到其他旋轉表示的轉換

這里總結四元數到旋轉矩陣之間的關系，設四元數為：$q=[s,v{]}^T$，則：
$$R=v{v}^T+s^{2}I+2sv\wedge +(v\wedge {)}^2$$
四元數到旋轉向量之間的關系
$$\begin{gathered} \theta =2arccos{q}_0 \\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2} \end{gathered}$$

---

3.6 Eigen幾何模塊

3.6.1 幾何模塊數據演示

這里給出如何使用Eigen庫進行四元數、旋轉矩陣、歐拉角、旋轉向量的運算。

#include <iostream> #include <cmath>using namespace std;#include <Eigen/Core> #include <Eigen/Geometry>using namespace Eigen;// 本程序演示了 Eigen 幾何模塊的使用方法int main(int argc, char **argv) {// Eigen/Geometry 模塊提供了各種旋轉和平移的表示// 3D 旋轉矩陣直接使用 Matrix3d 或 Matrix3fMatrix3d rotation_matrix = Matrix3d::Identity();// 旋轉向量使用 AngleAxis, 它底層不直接是Matrix，但運算可以當作矩陣（因為重載了運算符）AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); //沿 Z 軸旋轉 45 度cout.precision(3);cout << "rotation matrix =\n" << rotation_vector.matrix() << endl; //用matrix()轉換成矩陣// 也可以直接賦值rotation_matrix = rotation_vector.toRotationMatrix();// 用 AngleAxis 可以進行坐標變換Vector3d v(1, 0, 0);Vector3d v_rotated = rotation_vector * v;cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;// 或者用旋轉矩陣v_rotated = rotation_matrix * v;cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;// 歐拉角: 可以將旋轉矩陣直接轉換成歐拉角Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX順序，即yaw-pitch-roll順序cout << "yaw pitch roll = " << euler_angles.transpose() << endl;// 歐氏變換矩陣使用 Eigen::IsometryIsometry3d T = Isometry3d::Identity(); // 雖然稱為3d，實質上是4＊4的矩陣T.rotate(rotation_vector); // 按照rotation_vector進行旋轉T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量設成(1,3,4)cout << "Transform matrix = \n" << T.matrix() << endl;// 用變換矩陣進行坐標變換Vector3d v_transformed = T * v; // 相當于R*v+tcout << "v tranformed = " << v_transformed.transpose() << endl;// 對于仿射和射影變換，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略// 四元數// 可以直接把AngleAxis賦值給四元數，反之亦然Quaterniond q = Quaterniond(rotation_vector);cout << "quaternion from rotation vector = " << q.coeffs().transpose()<< endl; // 請注意coeffs的順序是(x,y,z,w),w為實部，前三者為虛部// 也可以把旋轉矩陣賦給它q = Quaterniond(rotation_matrix);cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;// 使用四元數旋轉一個向量，使用重載的乘法即可v_rotated = q * v; // 注意數學上是qvq^{-1}cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;// 用常規向量乘法表示，則應該如下計算cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;return 0; }

3.6.2 坐標轉換

這是坐標轉換的小例子，顯示了不同坐標系下的點的坐標轉換。

#include <iostream> #include <vector> #include <algorithm> #include <Eigen/Core> #include <Eigen/Geometry>using namespace std; using namespace Eigen;int main(int argc, char** argv) {Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);q1.normalize();q2.normalize();Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);Vector3d p1(0.5, 0, 0.2);Isometry3d T1w(q1), T2w(q2);T1w.pretranslate(t1);T2w.pretranslate(t2);Vector3d p2 = T2w * T1w.inverse() * p1;cout << endl << p2.transpose() << endl;return 0; }

---

3.7 可視化演示

這是一個顯示運動軌跡的程序：

#include <pangolin/pangolin.h> #include <Eigen/Core> #include <unistd.h>// 本例演示了如何畫出一個預先存儲的軌跡using namespace std; using namespace Eigen;// path to trajectory file string trajectory_file = "./examples/trajectory.txt";void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);int main(int argc, char **argv) {vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;ifstream fin(trajectory_file);if (!fin) {cout << "cannot find trajectory file at " << trajectory_file << endl;return 1;}while (!fin.eof()) {double time, tx, ty, tz, qx, qy, qz, qw;fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;Isometry3d Twr(Quaterniond(qw, qx, qy, qz));Twr.pretranslate(Vector3d(tx, ty, tz));poses.push_back(Twr);}cout << "read total " << poses.size() << " pose entries" << endl;// draw trajectory in pangolinDrawTrajectory(poses);return 0; }/*******************************************************************************************/ void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {// create pangolin window and plot the trajectorypangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f).SetHandler(new pangolin::Handler3D(s_cam));while (pangolin::ShouldQuit() == false) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f, 1.0f, 1.0f, 1.0f);glLineWidth(2);for (size_t i = 0; i < poses.size(); i++) {// 畫每個位姿的三個坐標軸Vector3d Ow = poses[i].translation();Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));glBegin(GL_LINES);glColor3f(1.0, 0.0, 0.0);glVertex3d(Ow[0], Ow[1], Ow[2]);glVertex3d(Xw[0], Xw[1], Xw[2]);glColor3f(0.0, 1.0, 0.0);glVertex3d(Ow[0], Ow[1], Ow[2]);glVertex3d(Yw[0], Yw[1], Yw[2]);glColor3f(0.0, 0.0, 1.0);glVertex3d(Ow[0], Ow[1], Ow[2]);glVertex3d(Zw[0], Zw[1], Zw[2]);glEnd();}// 畫出連線for (size_t i = 0; i < poses.size(); i++) {glColor3f(0.0, 0.0, 0.0);glBegin(GL_LINES);auto p1 = poses[i], p2 = poses[i + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}pangolin::FinishFrame();usleep(5000); // sleep 5 ms} }

總結

以上是生活随笔為你收集整理的视觉SLAM十四讲（3）：三维空间刚体运动的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。