视觉SLAM实践入门——（2）Eigen库的安装和基本用法

安装

安装命令

sudo apt install libeigen3-dev

查找库的位置

sudo updatedb
locate eigen3

 

这个库只使用了头文件搭建，在程序为了使用这个库，需要包含相关头文件。库默认安装路径是/usr/include/eigen3，而kdevelop默认头文件路径是/usr/include，因此为了使用这个库有（但不限于）以下几种办法：

方法1：包含头文件时，加上/eigen3前缀，如：

#include <eigen3/Eigen/Core>

方法2：将/usr/include/eigen3/Eigen这个目录移动到/usr/include中：

mv -rf /usr/include/eigen3/Eigen /usr/include

包含头文件

#include <Eigen/Core>

方法3：在工程中的CMakeLists.txt中添加库路径：

include_directories( "/usr/include/eigen3" )

方法4：使用find_package搜索命令库

 

基本用法

1、矩阵、向量运算以及方程求解

#include <iostream>
#include <ctime>    
#include <Eigen/Core>
#include <Eigen/Dense> 
using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 50

int main(void)
{
  //Eigen中所有向量和矩阵都是用Eigen::Matrix，这是一个模板类，前三个参数是：数据类型、行数、列数    
  Matrix<double, 2, 3> matrix23d;	//声明2*3矩阵
  matrix23d << 1,2,3,4,5,6;	//设置矩阵数据


  //用()访问矩阵元素，并输出矩阵
  //或者用这行代码直接输出矩阵cout << "matrix23d = \n" << matrix23d << endl;
  cout << "matrix23d = " << endl;
  for(int i = 0; i < 2; i++)	
  {
    for(int j = 0; j < 3; j ++) cout << matrix23d(i,j) << " " ;
    cout << endl;
  }


  //Vector3d实质上是Eigen::Matrix<double, 3, 1>，3表示3维，d表示double数据类型，f表示float数据类型
  Vector3d v3d;	//声明一个3维向量，等价于Matrix<double, 3, 1> v3d;


  //Matrix3d实质上是Eigen::Matrix<double, 3, 3>，3*3矩阵
  Matrix3d matrix33d = Matrix3d::Zero();	//初始化为0
  matrix33d << 1,2,3,4,6,8,9,10,10;
  //matrix33d.setZero();	//或者先声明，再清0


  //动态矩阵（维度大小可以变化）
  int rows = 3, cols = 3;
  Matrix<double, Dynamic, Dynamic> matrixX;	//或者MatrixXd matrixX;
  matrixX.resize(rows, cols);
  matrixX.setZero();
  cout << "动态矩阵 = \n" << matrixX << endl;
  rows = 3, cols = 4;
  matrixX.resize(rows, cols);
  matrixX.setZero();
  cout << "动态矩阵 = \n" << matrixX << endl;


  //随机数矩阵
  Matrix3d randomMatrix3d;
  randomMatrix3d.setRandom();
  cout << "随机数矩阵 = \n" << randomMatrix3d << endl;


  //矩阵和向量乘法
  v3d << 3,2,1;
  cout << "matrix23d * v3d = " << (matrix23d * v3d).transpose() << endl;    //transpose是转置
  //不同类型的矩阵和向量进行运算，需要显式类型转换
  Matrix<float, 2, 3> matrix23f;
  matrix23f << 1,2,3,4,5,6;
  cout << "matrix23f * v3d = " << (matrix23f.cast<double>() * v3d).transpose() << endl;    //cast<double>()表示将矩阵元素强制转换为double类型


  //转置
  cout << "matrix23d的转置 = \n" << matrix23d.transpose() << endl;
  //元素和
  cout << "matrix23d元素求和 = " << matrix23d.sum() << endl;
  //迹
  cout << "matrix33d元素的迹 = " << matrix33d.trace() << endl;
  //数乘
  cout << "10 * matrix33d = \n" << 10*matrix33d << endl;
  //行列式
  cout << "matrix33d的行列式 = " << matrix33d.determinant() << endl;
  //逆     
  cout << "matrix33d的逆 = \n" << matrix33d.inverse() << endl;

  //特征值（实对称矩阵一定可以对角化）  
  //对于3维全1矩阵，理论特征值是0、0、3，计算机求解答案是-1.28198e-16、2.22045e-16、3，前两个特征值幂为-16，约等于0
  matrix33d << 1,1,1,1,1,1,1,1,1;
  cout << matrix33d << endl;
  SelfAdjointEigenSolver<Matrix3d> eigenSolver(matrix33d);
  //eigenSolver.compute(matrix33d);
  cout << "特征值 = \n" << eigenSolver.eigenvalues().transpose() << endl;
  cout << "特征向量 = \n" << eigenSolver.eigenvectors() << endl;


  //解方程：matrixNN * x = vNd（待求x）
  //直接求逆运算量大，可利用矩阵分解求解（如方法2、3）
  Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrixNN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrixNN = matrixNN * matrixNN.transpose();	//保证半正定
  Matrix<double, MATRIX_SIZE, 1> vNd = MatrixXd::Random(MATRIX_SIZE, 1);
  
  //法1：直接求逆（x = matrixNN^(-1) * vNd）
  clock_t timeStart = clock();
  Matrix<double, MATRIX_SIZE, 1> x = matrixNN.inverse() * vNd;
  cout << "解方程，直接求逆用时:" << (clock() - timeStart) / (double)CLOCKS_PER_SEC << "s" << endl;
  cout << "结果为:\n" << x.transpose() << endl;
  
  //法2：QR分解
  timeStart = clock();
  x = matrixNN.colPivHouseholderQr().solve(vNd);
  cout << "解方程，QR分解用时：" << (clock() - timeStart) / (double)CLOCKS_PER_SEC << "s" << endl;
  cout << "结果为:\n" << x.transpose() << endl;
  
  //法3：cholesky分解（用于正定阵）
  timeStart = clock();
  x = matrixNN.ldlt().solve(vNd);
  cout << "解方程，cholesky分解用时：" << (clock() - timeStart) / (double)CLOCKS_PER_SEC << "s" << endl;
  cout << "结果为:\n" << x.transpose() << endl;


  return 0;
}

 

2、Eigen几何模块（旋转矩阵、变换矩阵、旋转向量、欧拉角、四元数）

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace <Eigen/Geometry>

int main(int argc, char **argv)
{
  cout.precision(3);	//设置输出浮点数精度为3位，例如0.123456，输出时显示0.123
  
  //定义三维空间向量v
  Vector3d v(1,0,0);


  //定义旋转矩阵，Identity函数将其初始化为单位阵
  Matrix3d rotationMatrix = Matrix3d::Identity();	
  cout << "旋转矩阵初始化后 = \n" << rotationMatrix << endl;	


  //定义旋转向量，M_PI/4表示180°/4=45°，向量的物理意义是沿着Z轴旋转45°
  AngleAxisd rotationVector(M_PI/4, Vector3d(0,0,1));


  //旋转向量对应的旋转矩阵
  rotationMatrix = rotationVector.matrix();	
  //或者rotationMatrix = rotationVector.toRotationMatrix()
  cout << "由旋转向量映射的旋转矩阵 = \n" << rotationMatrix << endl;	


  //利用旋转矩阵和旋转向量对v进行旋转
  Vector3d vRotated = rotationMatrix * v;	//利用向量
  cout << "v经过旋转矩阵作用后 = \n" << vRotated.transpose() << endl;	
  vRotated = rotationVector * v;	//利用矩阵
  cout << "v经过旋转向量作用后 = \n" << vRotated.transpose() << endl;	


  //定义欧拉角，2 1 0表示按ZYX顺序分析；roll pitch yaw
  Vector3d eulerAngles = rotationMatrix.eulerAngles(2,1,0);	
  cout << "roll pitch yaw = " << eulerAngles.transpose() << endl;


  //定义变换阵，变换阵使用的是齐次坐标，主要元素是旋转阵和平移向量
  //可以使用()对变换阵元素赋值，也可以直接改变变换阵赋的旋转阵平移向量，但不能使用<<对变换阵元素赋值
  Isometry3d T = Isometry3d::Identity();	//初始化为单位阵
  T.rotate(rotationVector);	//设置旋转阵，也可以利用旋转向量进行设置
  T.pretranslate(Vector3d(1,3,4));	//设置平移向量
  //T(0, 0) = 0, T(0, 1) = 1;    //可以这样改变T阵元素，但不能使用T <<1,2,3……;
  cout << "变换矩阵 = \n" << T.matrix() << endl;


  //定义四元数
  Quaterniond q = Quaterniond(rotationVector);	//也可以用旋转矩阵进行初始化
  cout << "四元数（虚部（3个），实部） = " << q.coeffs().transpose() << endl;    //coeffs的作用是按(x,y,z,w)顺序返回四元数各个系数并存放在一个四维向量中，其中（w是实部）
  

  //利用四元数进行旋转
  //v是一个三维空间中的向量，*运算利用了重载，数学上的表达是qvq^(-1)，运算结果是三维空间向量
  vRotated = q*v;	
  cout << "v经过四元数作用后 = " << vRotated.transpose() << endl;
  //利用数学定义计算，将向量写成四元数进行运算，运算结果是四元数
  cout << "v经过四元数作用后 = " << (q*Quaterniond(0,1,0,0)*q.inverse()).coeffs().transpose() << endl;


  return 0;
}

 

3、坐标变换实例

    相机1（R1）、相机2（R2）均位于世界坐标系（W）中。相机1的位姿是q1=（0.35，0.2，0.3，0.1）T，t1=（0.3，0.1，0.1）T；相机2的位姿是q2=（-0.5，0.4，-0.1，0.2）T，t2=（-0.1，0.5，0.3）T。某个向量p在相机1坐标系R1下的坐标为PR1=（0.5，0，0.2）T，求这个向量在相机2坐标系下的坐标PR2。（相机的位姿描述了相机在世界坐标系中的位置以及朝向，也就是描述了相机如何从世界坐标系原点运动（旋转和平移）到当前位置、当前朝向）

#include <iostream>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

int main(void)
{    
    Quaterniond q1(0.35,0.2,0.3,0.1), q2(-0.5,0.4,-0.1,0.2);
    Vector3d t1(0.3,0.1,0.1), t2(-0.1,0.5,0.3);
    Vector3d ppr1(0.5,0,0.2);

    //归一化处理，为了变换前后向量模不变
    q1.normalize();
    q2.normalize();
     
    //利用四元数（表示旋转）和平移向量设置变换阵
    Isometry3d T1(q1), T2(q2);
    T1.pretranslate(t1);
    T2.pretranslate(t2);
  
    //求解坐标
    //p1 = T1 * pw，虚设的pw是向量p的世界坐标
    //p2 = T2 * pw
    //p2 = T2 * T1^(-1) p1
    Vector3d ppr2;
    ppr2 = T2 * T1.inverse() * ppr1;
    cout << ppr2.transpose() << endl;

    return 0;
}