Eigen库的学习使用

---

前言

记录下Eigen的学习，参考SLAM十四讲。Eigen库是一个线性运算的C++模板库，支持线性代数、矩阵、矢量运算。
常用的头文件：

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

---

一、基本使用

定义：Eigen::Matrix<float,rows,cols> 第一个参数类型：float(单精度)、double(双精度)；后两个参数类型:矩阵的行列。

1.矩阵输入输出：

直接使用重载运算符<<输入输出；for遍历输入输出

Eigen::Matrix<float,2,3> matrix_23; //定义2X3矩阵，float型
matrix_23<<1,2,3,4,5,6;  //输入
for(int i=0;i<matrix_23.rows();i++)             //遍历输出
{
    for(int j=0;j<matrix_23.cols();j++)
    {
        cout<<matrix_23(i,j)<<"\t";
    }
    cout<<endl;
}
Eigen::Matrix3d matrix_33;      //3X3方阵，double
for(int i=0;i<3;i++)            //矩阵输入
{
    for(int j=0;j<3;j++)
    {
        matrix_33(i,j) = 1;
    }
}
cout<<matrix_33<<endl;

Eigen::vector3d vec_3d;    //向量  3行1列
vec_3d<<1,2,3;
cout<<vec_3d<<endl;

Eigen::Matrix<double,3,1> vec_3d_2;   //同样表示3行1列---矩阵
vec_3d_2 = vec_3d;
cout<<vec_3d_2<<endl;

2.一些特殊的矩阵

Eigen::Matrix<double,Dynamic,Dynamic> matrix_dynamic  //动态矩阵
Eigen::MatrixXd::Zero(3,3);   //3x3零矩阵
Eigen::MatrixXd::Ones(5,5);   //5x5全1矩阵
Eigen::MatrixXd::Identity(2,2);  //2x2单位矩阵
Eigen::MatrixXd::Random(3,3);   //3x3随机矩阵

3.矩阵的基本运算

Matrix3d matrix_33;
matrix_33 << 1,2,3,4,5,6,7,8,9;
cout<<matrix_33.sum()<<endl;         //矩阵元素之和
cout<<matrix_33.prod()<<endl;        //矩阵元素之积
cout<<matrix_33.mean()<<endl;       //平均值
cout<<matrix_33.norm()<<endl;        //2范数--元素平方和的1/2次方
cout<<matrix_33.minCoeff()<<endl;    //返回矩阵最小元素
cout<<matrix_33.maxCoeff()<<endl;    //返回矩阵最大元素
//matrix_33.minCoeff(&i,&j)     //返回最小元素的位置
cout<<matrix_33.trace()<<endl;          //矩阵的迹---对角线元素和
cout<<matrix_33.transpose()<<endl;       //矩阵转置，原矩阵不变
//cout<<matrix_33.transposeInplace();    //转置并改变赋值给原矩阵
cout<<matrix_33.determinant()<<endl;       //行列式
cout<<matrix_33.conjugate()<<endl;            //共轭矩阵---实部不变，虚部取反
cout<<matrix_33.inverse()<<endl;               //矩阵的逆---行列式0，没有逆
cout<<matrix_33.adjoint();           //共轭转置

Matrix<float,2,3> matrix_23;
cout<<"矩阵的行数"<<matrix_23.rows()<<endl;
cout<<"矩阵的列数"<<matrix_23.cols()<<endl;
matrix_23 << 1,2,3,4,5,6;
cout<<"矩阵第二行的元素:"<<matrix_23.row(1)<<endl;   //按行输出
cout<<"矩阵第二列的元素:"<<endl;
cout<<matrix_23.col(1)<<endl;      //按列输出
cout<<matrix_23.block<2,2>(0,1)<<endl;  //从矩阵0行1列位置处取2X2矩阵

4.向量计算

Vector3d vec_3d;
Matrix<float,3,1> vf_3d;
Matrix<float,2,3> matrix_23;
vec_3d<<3,2,1;
vf_3d<<4,5,6;
matrix_23<<1,2,3,4,5,6;
Matrix<double,2,1> matrix_21 = matrix_23.cast<double>()* vec_3d;
cout<<matrix_21<<endl;

Vector3d v(1,2,3);
Vector3d w(4,5,6);
cout<<"向量点积：";
cout<<v.dot(w)<<endl;
cout<<"向量叉积:";
cout<<v.cross(w)<<endl;
cout<<"向量的模：";
cout<<v.norm()<<endl;

5.特征值和特征向量

Matrix3d matrix_33;
matrix_33 = Matrix3d::Random();
cout<<matrix_33<<endl;

//自伴随特征求解
SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
//特征求解
EigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);

cout<<"特征值："<<endl;
cout<<eigen_solver.eigenvalues()<<endl;
cout<<"特征向量："<<endl;
cout<<eigen_solver.eigenvectors()<<endl;

6.方程求解

/*
       matrix_NN * x = v_Nd
*/
#define MATRIX_SIZE 50
Matrix<double,MATRIX_SIZE ,MATRIX_SIZE > matrix_NN = MatrixXd::Random(MATRIX_SIZE ,MATRIX_SIZE );
//设置半正定矩阵
matrix_NN = matrix_NN * matrix_NN.transpose();
//v_Nd矩阵
Matrix<double,MATRIX_SIZE ,1> v_Nd = MatrixXd::Random(MATRIX_SIZE ,1);

//可逆矩阵求解方程
Matrix<double,MATRIX_SIZE ,1> x1 = matrix_NN.inverse() * v_Nd;
cout<<x1<<endl;
cout<<"------------------------分割线1------------------------"<<endl;
//QR分解求解，可以求解非方阵
Matrix<double,MATRIX_SIZE ,1> x2 = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout<<x2<<endl;
cout<<"------------------------分割线2------------------------"<<endl;
//cholesky分解求解
Matrix<double,MATRIX_SIZE ,1> x3 = matrix_NN.ldlt().solve(v_Nd);
cout<<x3<<endl;

7.旋转矩阵、旋转向量、四元数

Eigen::AngleAxisd t_v(M_PI/4,Vector3d(0,0.1));    //旋转向量，绕（0，0，1）z轴旋转M_PI/4
Matrix3d t_r = t_v.matrix();                //旋转矩阵，将旋转向量转为旋转矩阵
Quaterniond t_q(t_v);                            //四元数

//对旋转向量赋值
AngleAxisd V1(M_PI/4,Vector3d(0,0,1));        //使用旋转角度和旋转轴（单位向量）来初始化
cout<<"Rotation vector 1:";
cout<<V1.matrix()<<endl;

AngleAxisd V2;
V2.fromRotationMatrix(t_r);  //使用fromRotationMatrix()函数对旋转向量赋值（四元数没有该方法）
cout<<"Rotation vector 2:";
cout<<V2.matrix()<<endl;

AngleAxisd V3;
V3 = t_r;         //直接用旋转矩阵对旋转向量赋值
cout<<"Rotation vector 3:"; 
cout<<V3.matrix()<<endl;

AngleAxisd V4(t_r);     // 使用旋转矩阵对旋转向量初始化
cout<<"Rotation vector 4:";
cout<<V4.matrix()<<endl;

AngleAxisd V5;
V5 = t_q;           //使用四元数对旋转向量赋值
cout<<"Rotation vector 5:";
cout<<V5.matrix()<<endl;

AngleAxisd V6(t_q);           //使用四元数对旋转向量初始化
cout<<"Rotation vector 6:";
cout<<V6.matrix()<<endl;
cout<<"-----------------------------------------------------------------"<<endl;
//对旋转矩阵赋值
Matrix3d R1 = Matrix3d::Identity();        //旋转矩阵函数初始化旋转矩阵  

Matrix3d R2;
R2 = t_v.matrix();        //使用旋转向量的成员函数matrix()对旋转矩阵赋值

Matrix3d R3;
R3 = t_v.toRotationMatrix();          //使用旋转向量的成员函数toRotationMatrix()对旋转矩阵赋值

Matrix3d R4;
R4 = t_q.matrix();               //使用四元数的成员函数matrix()对旋转矩阵赋值

Matrix3d R5;
R5 = t_q.toRotationMatrix();   //使用四元数的成员函数toRotationMatrix()对旋转矩阵赋值
cout<<"-----------------------------------------------------------------"<<endl;
//对四元数赋值  Eigen中的四元数前三维是虚部，最后一维是实部
 //使用旋转角度和旋转轴向量初始化四元数 q=[cos(A/2)，n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI/4)/2), 0*sin((M_PI/4)/2),0*sin((M_PI/4)/2),1*sin((M_PI/4)/2));
cout<<"Quaterniond Q1:"<<endl;
cout<<Q1.coeffs()<<endl;

Quaterniond Q2;
Q2 = t_r;       //旋转矩阵对四元数直接赋值
cout<<"Quaterniond Q2:"<<endl;
cout<<Q2.coeffs()<<endl;

Quaterniond Q3(t_r);   //旋转矩阵对四元数赋值
cout<<"Quaterniond Q3:"<<endl;
cout<<Q3.coeffs()<<endl;

Quaterniond Q4;
Q4  = t_v;    //旋转向量对四元数初始化
cout<<"Quaterniond Q4:"<<endl;
cout<<Q4.coeffs()<<endl;

Quaterniond Q5(t_v);    //旋转向量对四元数初始化
cout<<"Quaterniond Q5:"<<endl;
cout<<Q5.coeffs()<<endl;

8.欧拉角、平移向量、变换矩阵

//旋转矩阵转欧拉角
Matrix3d rotation_matrix = Matrix3d::Identity();
Vector3d enler_angles = rotation_matrix.enlerAngles(2,1,0); //0:x轴，1：y轴，2：z轴(2,1,0)代表输出角度顺序
cout<<"yaw,pitch,roll:"<<enler_angles.transpose()<<endl;

//旋转矩阵和平移向量定义变换矩阵Isometry3d
Matrix3d rotation_m = Matrix3d::Identity();
Vector3d t1;
t1<<3.214096e-14,-1.998401e-15,-4.041212e-14;
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();    //初始化
T1.rotate(rotation_m );    //导入旋转矩阵
T1.pretranslate(t1);       //导入平移向量
cout<<T1.matrix()<<endl;

//四元数和平移向量定义变换矩阵
Quaterniond q1(0.35,0.2,0.3,0.1);
q1.normalize();          //归一化
Vector3d t2(0.3, 0.3, 0.3);
Eigen::Isometry3d T2(q1);
T2.pretranslate(t2);
cout<<T2.matrix()<<endl;

//通过Eigen::Matrix4d构造变换矩阵
Matrix3d rotation_m2 = Matrix3d::identity();  //初始化
Eigen::Matrix4d T3;
T3.setIdentity();
T3.block<3,3>(0,0) = rotation_m2;
T3.topRightCorner(3,1) = t2;
cout<<T3.matrix()<<endl;

---

总结

本文简要的介绍了Eigen的学习使用，对于常见的函数给出了使用方法,其它的函数用到去查官方文档即可。