Eigen简单介绍及ROS中应用

Eigen是一个C++开源线性代数库。提供有关矩阵的的线性代数运算，解方程等功能。在ROS中也可以方便的使用Eigen库，在学习SLAM的时候碰见了，特此记录一下。
附上官方说明：http://eigen.tuxfamily.org/dox/classEigen_1_1Transform.html

一、安装Eigen

sudo apt-get install libeigen3-dev

二、头文件

头文件	说明
#include <Eigen/Core	包括矩阵和数组类，基本线性代数(包括三角形和自伴随积)操作以及数组操作
#include <Eigen/Geometry>	#include <Eigen/Geometry> 包括变换，平移，缩放，旋转(四元数，角度轴)
#include <Eigen/LU>	#include <Eigen/LU> 可用求解器进行逆、行列式、LU分解
#include <Eigen/Cholesky>	LLT 以及LDLT Cholesky 分解
#include <Eigen/Householder>	householder变换
#include <Eigen/SVD>	最小二乘求解器(JacobiSVD, BDCSVD)的SVD分解
#include <Eigen/QR>	QR分解与求解器
#include <Eigen/Eigenvalues>	特征值、特征向量分解
#include <Eigen/Sparse>	稀疏矩阵及相关线性代数基础
#include <Eigen/Dense>	包括 Core, Geometry, LU, Cholesky, SVD, QR, and Eigenvalues的头文件
#include <Eigen/Eigen>	整个Eigen库

三、Eigen的使用

1.矩阵、数组、向量的定义

1. Eigen::Matrix<double, 3, 9> Ai;//声明一个3*9的矩阵`

 Eigen::Vector3d bi;//声明一个3*1的向量
 Eigen::Matrix<double,3,1> bi;//等价于上面的语句

3. 动态矩阵（矩阵大小在运行时确定）

 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
Eigen::MatrixXd matrix_x;

4. 初始化

Ai<< 1 , 2 , 3 , 4 , 5 , 6，7 , 8 , 9，
11,12,13,14,15,16,17,18,19，
21,22,23,24,25,26,27,28,29；
Ai.row(0)<< 1 , 2 , 3 , 4 , 5 , 6，7 , 8 , 9; 
Ai.row(1)<<11,12,13,14,15,16,17,18,19;  
Ai.row(2)<<21,22,23,24,25,26,27,28,29; //按行赋值

2.计算

1. (1) 加减乘除：+，-，×，/直接运算
2. Ai.transpose()，Ai.trace()，Ai.determinant()，Ai.sum()，Ai.inverse()，分别为求转职、迹，行列式，元素和，逆矩阵

3.块操作

1. 基本格式

matrix.block(i,j,p,q);     （1）
matrix.block<p,q>(i,j);    （2）

定义（1）表示返回从矩阵的(i, j)开始，每行取p个元素，每列取q个元素所组成的临时新矩阵对象，原矩阵的元素不变。
定义（2）中block（p, q）可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵，返回该子矩阵组成的临时 矩阵对象，原矩阵的元素不变。

2. 操作方法（https://blog.csdn.net/Augusdi/article/details/12907341）

#include <Eigen/Dense>
#include <iostream>
using namespace std;
int main()
{
Eigen::MatrixXf m(4,4);
m << 1, 2, 3, 4,
5, 6, 7, 8,
9,10,11,12,
13,14,15,16;
cout << "Block in the middle" << endl;
cout << m.block<2,2>(1,1) << endl << endl;
for (int i = 1; i <= 3; ++i)
{
cout << "Block of size " << i << "x" << i << endl;
cout << m.block(0,0,i,i) << endl << endl;
}
}

运行结果
Block in the middle
 6  7
10 11
Block of size 1x1
1
Block of size 2x2
1 2
5 6
Block of size 3x3
 1  2  3
 5  6  7
 9 10 11

4.方程Ai×x=bi

1. 直接求逆
   Eigen::Matrix<double,3,1> x = Ai.inverse()*bi;

2. A不可逆，算最小二乘解，直接套公式即可

3. QR分解
   x = Ai.colPivHouseholderQr().solve(bi);

5.其他运算

（https://www.cnblogs.com/lovebay/p/11215028.html）

四、ROS中应用——里程计标定（本应用来自于深蓝学院激光slam学习第二讲的作业）

bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{

    if(now_len<INT_MAX)
    {
      //TODO: 构建超定方程组
	Eigen::Matrix<double,3,9> Ai;
        Eigen::Vector3d bi;
	Ai.row(0)<< Odom(0), Odom(1),Odom(2),0,0,0,0,0,0;  // populate the first row--shorthand method
	Ai.row(1)<<0,0,0,Odom(0), Odom(1),Odom(2),0,0,0;  //second row
	Ai.row(2)<<0,0,0,0,0,0,Odom(0), Odom(1),Odom(2);  // yada, yada
 	bi << scan(0),scan(1),scan(2);
	A.block<3,9>(3*now_len,0) = Ai;//A.conservativeResize(len*3,9);
        b.block<3,1>(3*now_len,0) = bi;//b.conservativeResize(len*3);
        //end of TODO
        now_len++;
        return true;
    }
    else
    {
        return false;
    }
}

Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;

    //TODO:求解线性最小二乘


    Eigen::Matrix<double,9,1> X_ = A.colPivHouseholderQr().solve(b);
    correct_matrix << X_(0),X_(1),X_(2),X_(3),X_(4),X_(5),X_(6),X_(7),X_(8);

    //end of TODO

    return correct_matrix;
}

参考文献

1. http://eigen.tuxfamily.org/dox/classEigen_1_1Transform.html
2. https://blog.csdn.net/qq_34935373/article/details/93202722
3. https://blog.csdn.net/Augusdi/article/details/12907341
4. https://www.cnblogs.com/lovebay/p/11215028.html