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.矩阵、数组、向量的定义
- Eigen::Matrix<double, 3, 9> Ai;//声明一个3*9的矩阵`
Eigen::Vector3d bi;//声明一个3*1的向量
Eigen::Matrix<double,3,1> bi;//等价于上面的语句
- 动态矩阵(矩阵大小在运行时确定)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
Eigen::MatrixXd matrix_x;
- 初始化
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) 加减乘除:+,-,×,/直接运算
- Ai.transpose(),Ai.trace(),Ai.determinant(),Ai.sum(),Ai.inverse(),分别为求转职、迹,行列式,元素和,逆矩阵
3.块操作
- 基本格式
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列的子矩阵,返回该子矩阵组成的临时 矩阵对象,原矩阵的元素不变。
- 操作方法(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
-
直接求逆
Eigen::Matrix<double,3,1> x = Ai.inverse()*bi; -
A不可逆,算最小二乘解,直接套公式即可
-
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;
}
参考文献
- http://eigen.tuxfamily.org/dox/classEigen_1_1Transform.html
- https://blog.csdn.net/qq_34935373/article/details/93202722
- https://blog.csdn.net/Augusdi/article/details/12907341
- https://www.cnblogs.com/lovebay/p/11215028.html