Eigen库
一、Eigen
Eigen是一个C++开源线性代数库。相比于其他库，Eigen 特殊之处在于，它是一个纯用头文件搭建起来的库。我们在使用时，只需引入Eigen的头文件即可，不需要链接它的库文件（因为它没有库文件）。
Eigen是矩阵的基本数据单元，他是一个模板类。它的前三个参数为：数据类型、行、列。
头文件
#include<Eigen/Core> //Eigen部分
#include<Eigen/Dense> //稠密矩阵的代数运算
函数部分
基本运算
①.声明一个2*3的float矩阵
Eigen::Matrix<float,2,3>matrix_23;
②.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<<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 = matrix_23.cast<double>()*v_3d;
⑧.矩阵的基本运算
转置：
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
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);
//方法一：直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x= matrix_NN.inverse()*v_Nd;
//方法二：通常用矩阵分解来求，例如 QR分解，速度会快很多
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
完整的程序
#include<iostream>
#include<ctime>
using namespace std;
#define MATRIX_SIZE 50
int main(int argc,char** argv)
{
Eigen::Matrix<float,2,3>matrix_23;
Eigen::Vector3d v_3d;
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<<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 = matrix_23.cast<double>()*v_3d;
cout<< result <<endl;
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;
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 invers is "<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout<<"time use in Qr compsition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
return 0;
}
说明
- 为了实现更好的效率，在 Eigen 中你需要指定矩阵的大小和类型。
- 像使用 float、double那样的内置数据类型那样使用 Eigen 的矩阵，这应该是他的设计初衷。
- 在C++程序中，我们可以把一个 float 数据和 double 数据相加、相乘，编译器会自动把数据类型转换为最合适的那种。而在Eigen 中，出于性能的考虑，必须显示地对矩阵类型进行转换。
- 在计算过程中你也需要保证矩阵维数的正确性。
矩阵运算
①. 定义一个旋转矩阵
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity(); //Identity()表示单位阵
②. 定义一个旋转向量
Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d (0,0,1));
③. 旋转向量转换成旋转矩阵（两种方式）
//方式一
cout.precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix()<<endl;
//方式二
rotation_matrix = rotation_vector.toRotationMatrix();
④. 设定一个位置坐标
Eigen::Vector3d v(1,0,0);
⑤. 使用旋转向量进行坐标变换
Eigen::Vector3d v_rotated = rotation_vector*v;
⑥. 使用旋转向量进行坐标变换
v_rotated = rotation_matrix *v;
⑦. 定义一个欧拉角，并将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); //ZYX顺序
⑧. 定义一个欧式变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); //初始化为单位阵
⑨. 使用 旋转向量 旋转 欧式变换矩阵
T.rotate( rotation_vector );
⑩. 设置欧氏变换矩阵的平移向量
T.pretranslate(Eigen::Vector3d(1,3,4));
①①. 使用欧氏变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; //相当于 R*V+t
①②. 定义一个四元数，并直接把旋转向量赋值給四元数（反之亦然）
Eigen::Quaterniond q = Eigen::Quaterniond( rotation_vector );
①③. 使用四元数旋转一个向量
v_rotated = q*v;
完整程序
#include<iostream>
#include<cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
int main( int argc,char** argy )
{
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d (0,0,1));
cout.precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix()<<endl;
rotation_matrix = rotation_vector.toRotationMatrix();
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated = rotation_vector*v;
cout<<"(1,0,0) after totation = "<<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);
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate( rotation_vector );
T.pretranslate(Eigen::Vector3d(1,3,4));
cout<<"Transform matrix = \n"<<T.matrix()<<endl;
Eigen::Vector3d v_transformed = T*v;
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
Eigen::Quaterniond q = Eigen::Quaterniond( rotation_vector );
cout<<"quaternion = \n"<<q.coeffs()<<endl;
v_rotated = q*v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
return 0;
}
Eigen对上述形式的表达方式总结
- 旋转矩阵（3*3）：Eigen::Matrix3d
- 旋转向量（3*1）：Eigen::AngleAxisd
- 欧拉角（3*1）：Eigen::Vector3d
- 四元数（4*1）：Eigen::Quaterniond
- 欧式变换矩阵（4*4）：Eigen::Isometry3d
- 仿射变换（4*4）：Eigen::Affine3d
- 射影变换（4*4）：Eigen::Projective3d
