視覺SLAM十四講(三)——三維空間剛體運動(下)

理論部分請看 :三維空間剛體運動

1、Eigen的使用

首先安裝 Eigen：ios

sudo apt-get install libeigen3-dev

通常都安裝在ruby

/usr/include/eigen3/

中
代碼：dom

#include <iostream>
#include <ctime>

using namespace std;

//Eigen 部分
#include <Eigen/Core>
//稠密矩陣的代數運算
#include <Eigen/Dense>

#define MATRIX_SIZE 50

//本程序演示了 Eigen 基本類型的使用

int main(int argc,char** argv){
   //聲明一個 2×3 的 float 矩陣
   Eigen::Matrix<float,2,3> matrix_23;
   //Eigen 經過 typedef 提供了許多內置類型，不過底層仍然是 Eigen::Matrix
   //例如 Vector3d 實質上是 Eigen::Matrix<double,3,1> 
   Eigen::Vector3d v_3d;
   //Matrix3d 實質上是 Eigen::Matrix<double,3,3>
   Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
   //若是不肯定矩陣大小，可使用動態大小的矩陣
   Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
   //更簡單的
   Eigen::MatrixXd matrix_x;

   //矩陣操做
   //輸入數據
   matrix_23 << 1,2,3,4,5,6;
   //輸出
   cout<<"2*3矩陣 "<<matrix_23<<endl;

   //用（）訪問矩陣中的元素
   for(int i = 0;i<1;i++)
      for(int j = 0;j<2;j++)
          cout<<"矩陣元素: "<<matrix_23(i,j)<<endl;
   v_3d << 3,2,1;
   //矩陣和向量相乘
   //Eigen::Matrix<double,2,1> result_wrong_type = matrix_23 * v_3d; 混合兩種不一樣類型的矩陣，這是錯誤的
   //應該這樣顯示轉換
   Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
   cout<<"和向量相乘："<<result<<endl;

   //一樣不能搞錯矩陣的維度
   //試着取消下面的註釋，看看會報什麼錯
   //Eigen::Matrix<double,2,3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

   //一些矩陣運算
   matrix_33 = Eigen::Matrix3d::Random();
   cout<<"矩陣運算："<<matrix_33<<endl<<endl;

   cout<<"轉置："<<matrix_33.transpose()<<endl;   //轉置
   cout<<"各元素和："<<matrix_33.sum()<<endl;         //各元素和
   cout<<"跡："<<matrix_33.trace()<<endl;       //跡
   cout<<"數乘："<<10 * matrix_33<<endl;          //數乘
   cout<<"逆："<<matrix_33.inverse()<<endl;     //逆
   cout<<"行列式："<<matrix_33.determinant()<<endl; //行列式

   //特徵值
   //實對稱矩陣能夠保證對角化成功
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver (matrix_33.transpose() * matrix_33);
   cout<<"Eigen values = "<<eigen_solver.eigenvalues()<<endl;
   cout<<"Eigen vectors = "<<eigen_solver.eigenvectors()<<endl;

   //解方程
   //求解 matrix_NN * x = v_Nd 這個方程
   //N 的大小在上賣弄宏裏定義，矩陣由隨機數生成
   //直接求逆是最直接的，可是運算量大

   Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
   matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
   Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
   v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);

   clock_t time_stt = clock();  //計時
   //直接求逆
   Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
   cout<<"time use in normal inverse is "<<1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC <<" ms"<<endl;

   //一般用矩陣分解來求，例如 QR 分解，速度會快不少
   time_stt = clock();
   x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
   cout<<"time use in Qr composition is "<<1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC<<" ms"<<endl;

   return 0;
}

編譯方法爲：
在源代碼所在文件夾再建立一個 CMakeLists.txt,寫入：ui

cmake_minimum_required (VERSION 2.8) include_directories("/usr/include/eigen3") project(EigenMatrix) add_executable(eigenMatrix eigenMatrix.cpp)

而後 url

cmake .

make

再運行就能夠了spa

./eigenMatrix

程序中已經給出較詳細註釋，這裏就不在解釋了.net

2、Eigen 幾何模塊

代碼：3d

#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc,char** argv){
   Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
   //旋轉向量使用 AngleAxis，它底層不直接是 Matrix3d，但運算能夠當作矩陣(由於重載了運算符)
   Eigen::AngleAxisd rotation_vector (M_PI/4,Eigen::Vector3d(0,0,1));   //沿Z軸旋轉45度
   cout .precision(3);
   cout<<"rotation matrix = \n"<<rotation_vector.matrix()<<endl; //用 matrix() 轉換成矩陣
   //也能夠直接賦值
   rotation_matrix = rotation_vector.toRotationMatrix();
   //用 AngleAxis 能夠進行座標變換
   Eigen::Vector3d v(1,0,0);
   Eigen::Vector3d v_rotated = rotation_vector *v;
   cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
   //或者用旋轉矩陣
   v_rotated = rotation_matrix *v;
   cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

   //歐拉角：能夠將旋轉矩陣直接轉換成歐拉角
   Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);   //ZYX 順序，即yaw pitch roll 順序
   cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

   //歐式變換矩陣使用 Eigen::Isometry
   Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); //雖然稱爲3d，實質上是4×4矩陣
   T.rotate(rotation_vector);  //按照rotation_vector 進行旋轉
   T.pretranslate(Eigen::Vector3d(1,3,4));  //把平移向量設成(1,3,4)
   cout<<"Transform matrix = \n"<<T.matrix()<<endl;

   //用變換矩陣進行座標變換
   Eigen::Vector3d v_transformed = T*v;  //至關於 R*v + t
   cout<<"v transformed = "<<v_transformed.transpose()<<endl;
   //相對於仿射和射影變換，使用 Eigen::Affine3d 和Eigen::Projective3d 便可,略

   //四元數
   //能夠直接把 AngleAxis 賦值給四元數，反之亦然
   Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
   cout<<"quaternion = \n"<<q.coeffs()<<endl;  //注意 coeffs 的順序是 (x,y,z,w) ，w 爲實部，前三者爲虛部
   //也能夠把旋轉矩陣賦值給它
   q = Eigen::Quaterniond(rotation_matrix);
   cout<<"quaternion = \n"<<q.coeffs()<<endl;
   //使用四元數旋轉一個向量，使用重載的乘法便可
   v_rotated = q * v;   //數學上是 qvq^{-1}
   cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

   return 0;   
}

CMakeLists.txt: code

cmake_minimum_required(VERSION 2.8) include_directories("/usr/include/eigen3") project(UseGeometry) add_executable(useGeometry useGeometry.cpp)

編譯運行方法同上。orm