SLAM知识点——eigen库学习

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。Eigen是一个用纯头文件搭建起来的库，这意味这你只要能找到它的头文件，就能使用它。Eigen头文件的默认位置是“/usr/include/eigen3”. 由于Eigen库相较于OpenCV中的Mat等库而言更加高效，许多上层的软件库也使用Eigen进行矩阵运算，比如SLAM中常用的g2o,Sophus等。此外Eigen库还被被用于Caffe，Tensorflow等许多深度学习相关的框架中。
————————————————
原文链接：https://blog.csdn.net/u011092188/article/details/77430988/

1 eigen库安装

ubuntu下面是没有安装的需要手动安装，使用下述指令安装，自动装到/usr/include/eigen3

sudo apt-get install libeigen3-dev

eigen是个头文件库，只有h文件，没有cpp文件，在写cmake的时候，只有包含头文件include_directories，不用target_link_libraries

CMakeLists.txt demo:

cmake_minimum_required( VERSION 2.8 )
project( useEigen )
set( CMAKE_BUILD_TYPE "Debug" )
set( CMAKE_CXX_FLAGS "-O3" )  # 这个是干嘛的？？
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigenMatrix eigenMatrix.cpp )

注意，是CMakeLists.txt不是CMakeList.txt

补充：
（1）cmake工具安装：sudo apt-get install cmake
或者，安装包安装，下载地址：https://cmake.org/files/
（2）kdevelop安装：sudo apt-get install kdevelop

2 eigen matrix的用法

eigen.cpp

#include
#include
#include
// 稠密矩阵的代数运算（逆，特征值等）
#include
#define MATRIX_SIZE 50
using namespace std;


int main(int argc, char** argv)
{
	cout<<"-----------1----------"<<endl;

	Eigen::Vector3d v_3d;  //向量
	Eigen::VectorXd v(6);  //需要指定大小(6)
	Eigen::Matrix<float,2,3> matrix_23;  //行、列
	Eigen::MatrixXd m(3,3);
	Eigen::Matrix3d m1;
	Eigen::Matrix<float,3,1> vd_3d;
	Eigen::Matrix<double, -1, -1> matrix_dynamic;
	Eigen::MatrixXd matrix_x;
	Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
	Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
	
	matrix_23<<1,1,2,3,4,5;
	v_3d<<1,2,3;
	vd_3d<<5,6,7;
	v<<1,2,3,4,5,6;
	m<<1,2,3,4,5,6,7,8,9;	
	//Eigen::Vector3d v(1,0,0);
	
	Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
	matrix_33 = Eigen::Matrix3d::Random();
	matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
	v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE, 1);
	
	//类型转换
	Eigen::Matrix<double,2,1> result = matrix_23.cast<double>()*v_3d; //注意类型必须相同，维度也不能搞错
	
	for(int i=0;i<2;i++)
		for(int j=0;j<3;j++)
			cout<<matrix_23(i,j)<<"\t";

	cout<<"matrix_33: \n"<<matrix_33<<endl;
	cout<<"transpose: \n"<<matrix_33.transpose()<<endl;
	cout<<"sum: \n"<<matrix_33.sum()<<endl;
	cout<<"trace: \n"<<matrix_33.trace()<<endl;
	cout<<"10*: \n"<<10*matrix_33<<endl;
	cout<<"inverse: \n"<<matrix_33.inverse()<<endl;
	cout<<"determinant: \n"<<matrix_33.determinant()<<endl; // |matrix_33|	


	cout<<"-----------2----------"<<endl;
    // 实对称矩阵可以保证对角化成功
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>eigen_matrix(matrix_33.transpose()*matrix_33);
	cout<<"Eigen values = \n: "<<eigen_matrix.eigenvalues()<<endl;
	cout<<"Eigen vectors = \n: "<<eigen_matrix.eigenvectors()<<endl;
	
    // 求解 matrix_NN * x = v_Nd 这个方程
	clock_t t_start = clock();
    // (1) 直接求逆, 但是求逆运算量大	
	Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse()*v_Nd;
	//cout<<"answer x:"<
	cout<<"time1: "<<1000*(clock()-t_start)/double(CLOCKS_PER_SEC)<<"ms"<<endl;

	// (2) 通常用矩阵分解来求，例如QR分解，速度会快很多
	clock_t t_start1 = clock();
	x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
	//cout<<"answer x:"<
	cout<<"time2: "<<1000*(clock()-t_start1)/double(CLOCKS_PER_SEC)<<"ms"<<endl;
	
	return 0;
}

注意：CLOCKS_PER_SEC这个宏，在Windows平台下是1000，而到了Linux平台下就是1000000了。不适用该宏，直接使用clock()-t_start1，在windows下面出来的是毫秒，在linux下面出来的是毫秒的1000倍。
linux:
std::cout << 1000 * (clock() - t1) / double(CLOCKS_PER_SEC)<< std::endl; //1
std::cout << clock() - t1<< std::endl; //1000
windows:
std::cout << 1000 * (clock() - t1) / double(CLOCKS_PER_SEC)<< std::endl; //1
std::cout << clock() - t1<< std::endl; //1

输出结果：

-----------1----------
matrix_33: 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 
1.61307
trace: 
1.05922
10*: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
determinant: 
0.208598

-----------2----------
Eigen values = 
: 0.0242899
 0.992154
  1.80558
Eigen vectors = 
: -0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time1: 94.697ms
time2: 4.432ms

3 eigen geometry的用法

demo1.cpp

#include
#include
#include
#include
// Eigen 几何模块
using namespace std;
using namespace Eigen;  //使用Eigen命名空间写起来简单些

int main(int arc, char** argv)
{
	/*
		旋转矩阵（3X3）:Eigen::Matrix3d
		旋转向量（3X1）:Eigen::AngleAxisd
		四元数（4X1）:Eigen::Quaterniond
		平移向量（3X1）:Eigen::Vector3d
		变换矩阵（4X4）:Eigen::Isometry3d
	*/
	
	// 1 常用的三种表示：旋转向量、旋转矩阵、四元素
	Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //沿Z轴旋转45度
 	Eigen::Matrix3d rotation_matrix(rotation_vector); 
	Eigen::Quaterniond rotation_Q(rotation_vector);


	// 2 旋转向量赋初值
	// 2.1 旋转角度和旋转轴向量->旋转向量
	Eigen::AngleAxisd R_V1(M_PI/4, Eigen::Vector3d(0,0,1));
	//cout<

	// 2.2 旋转矩阵->旋转向量
	Eigen::AngleAxisd R_V2, R_V3;
	R_V2 = rotation_matrix; //赋值
	Eigen::AngleAxisd R_V4(rotation_matrix);  //初始化
	R_V3.fromRotationMatrix(rotation_matrix);  //注意此方法为旋转向量独有,四元数没有
	
	// 2.3 四元数->旋转向量
	AngleAxisd R_V5;
	R_V5 = rotation_Q; //赋值
	AngleAxisd R_V6(rotation_Q); //初始化
	
	cout<<"R_V1:\n "<<R_V1.matrix()<<endl;
	cout<<"R_V2:\n "<<R_V2.matrix()<<endl;
	cout<<"R_V3:\n "<<R_V3.matrix()<<endl;
	cout<<"R_V4:\n "<<R_V4.matrix()<<endl;
	cout<<"R_V5:\n "<<R_V5.matrix()<<endl;
	cout<<"R_V6:\n "<<R_V6.matrix()<<endl;


	// 3 四元数赋初值
	// 注意Eigen库中的四元数前三维是虚部,最后一维是实部
	// 3.1旋转角度和旋转轴向量->四元数
	float an = M_PI/4;
	float nx=0, ny=0, nz=1;  //wrong: float nx, ny, nz = 0, 0, 1
	Eigen::Quaterniond Q1(cos(an/2), nx*sin(an/2), ny*sin(an/2), nz*sin(an/2));
	//两种输出四元数的方式
	cout<<"Q1: \n"<<Q1.coeffs()<<endl; //(0,0,0.382683,0.92388)最后一维是实部
	cout << Q1.x()<< " " << Q1.y() << " " << Q1.z() <<" " << Q1.w() << endl;  //0 0 0.382683 0.92388
	

	// (1) Matrix->vector
	cout<<"vector->matrix:\n "<<rotation_vector.matrix()<<endl;
	cout<<"vector->matrix: "<<rotation_vector.toRotationMatrix()<<endl;
	
	//3.2 旋转矩阵->四元数
	Quaterniond Q2;
	Q2 = rotation_matrix;
	Quaterniond Q3(rotation_matrix);
	cout<<"Q2: \n"<<Q2.coeffs()<<endl;  //0 0 0.382683 0.92388
	cout<<"Q3: \n"<<Q3.coeffs()<<endl;  //0 0 0.382683 0.92388
	
	//3.3 旋转向量->四元数
	Quaterniond Q4;
	Q4 = rotation_vector;
	Quaterniond Q5(rotation_vector);
	cout<<"Q4: \n"<<Q4.coeffs()<<endl;  //0 0 0.382683 0.92388
	cout<<"Q5: \n"<<Q5.coeffs()<<endl;  //0 0 0.382683 0.92388
		
	//4 旋转矩阵赋初值
	//4.1 旋转矩阵函数->旋转矩阵
	Eigen::Matrix3d M1 = Eigen::Matrix3d::Identity(); //3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f;  Identity()单位矩阵
    
	//4.2 旋转向量->旋转矩阵
	Eigen::Matrix3d M2 = rotation_vector.matrix();
	Eigen::Matrix3d M3 = rotation_vector.toRotationMatrix();
	
	//4.3 四元数->旋转矩阵
	Eigen::Matrix3d M4 = rotation_Q.matrix(); //rotation_vector.toRotationMatrix()
	Eigen::Matrix3d M5 = rotation_Q.toRotationMatrix(); //rotation_vector.toRotationMatrix()
	cout<<"M1: \n"<<M1<<endl;
	cout<<"M2: \n"<<M2<<endl; 
	cout<<"M3: \n"<<M3<<endl;
	cout<<"M4: \n"<<M4<<endl; 
	cout<<"M5: \n"<<M5<<endl;
	return 0;
}

demo2.cpp

#include
#include
#include
#include
// Eigen 几何模块
using namespace std;
using namespace Eigen;  //使用Eigen命名空间写起来简单些

int main(int arc, char** argv)
{
	Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //沿Z轴旋转45度
 	Eigen::Matrix3d rotation_matrix(rotation_vector); 
	Eigen::Quaterniond rotation_Q(rotation_vector);

	// 用旋转向量、旋转矩阵旋转一个向量
	Eigen::Vector3d v(1,0,0);
	cout<<"v: "<<v<<endl;
	Eigen::Vector3d v1 = rotation_vector*v;
	cout<<"v1: "<<v1<<endl;
	Eigen::Vector3d v2 = rotation_matrix*v;
	cout<<"v2: "<<v2<<endl;
	Eigen::Vector3d v3 = rotation_Q*v;
	cout<<"v3: "<<v3<<endl;
	
	// 欧拉角 
	// 旋转矩阵->欧拉角
	Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即yaw pitch roll顺序
	cout<<"euler_angles: "<<euler_angles<<endl;
	
	// 变换矩阵
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();  //虽然称为3d，实质上是4＊4的矩阵。3d双精度，3f单精度，2d，2f，Identity说的是旋转为0平移也为0
	T.rotate(rotation_vector);
	T.pretranslate(Eigen::Vector3d(1,3,4));
	cout<<"T: "<<T.matrix()<<endl;  //不能直接输出T
	// 变换矩阵坐标变换
	Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t，4×4矩阵与3×1向量相乘，利用*重载，可以转换为R*v+t
	cout<<"v tranformed = "<<v_transformed.transpose()<<endl;   //加个transpose()是为了看起来方便

    // 使用四元数旋转一个向量，使用重载的乘法即可
	Eigen::Vector3d v_rotated = rotation_Q*v; // 注意数学上是qvq^{-1}，见补充附图
    cout<<"v tranformed = "<<v_rotated.transpose()<<endl;   //加个transpose()是为了看起来方便
	
	return 0;
}

补充：

图片来源：高翔-视觉SLAM十四讲PPT

4 eigen常见报错

报错1：使用cv::cv2eigen的时，编译报错

/usr/local/include/opencv4/opencv2/core/eigen.hpp:63:22: error: ‘Eigen’ does not name a type; did you mean ‘eigen’?
 void eigen2cv( const Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& src, OutputArray dst )

解决1：由于头文件的引用顺序错误导致，即在include "opencv2/core/eigen.hpp"之前必须先include “eigen3/Eigen/Dense”．

5 Eigen版本查询

pkg-config --modversion eigen3