SLAM开荒笔记之Eigen库
Eigen库简介
SLAM中,经常会需要计算位姿、进行KF系列的滤波操作等,这就要求高效地执行矩阵运算, Eigen 库就是 C++ 中一个很好用的矩阵运算库,矩阵乘法、位姿变换矩阵的各种形式转换、转置、LU分解、SVD分解等都可以通过该库高效的实现。
下面是本人对该库的学习记录。
如何在C++中调用该库
Eigen库本身只依赖C++标准库,所以不需要另外安装其他依赖。Eigen库在Ubuntu下的安装也比较简单,输入命令行:
sudo apt-get install libeigen3-dev
即可完成安装,安装后如果想查询安装位置,可通过:locate eigen3
查询。
在.cpp代码中,要调用该库只需要包含相应的头文件即可,一个比较简单的做法是
#include<Eigen/Eigen> /* 包括Eigen库的所有模块 */
由于Eigen库没有.so或.a文件,所以不需要进行库链接(即target_link_libraries()
),在相应的 CMakeLists.txt 文件中,仅仅需要添加该库的路径,有两种等价的写法
写法1:include_directories( "/usr/include/eigen3" )
写法2:
find_package( Eigen3 REQUIRED )
include_directories( ${Eigen3_INCLUDE_DIRS} )
位姿变换中常用数据类型声明
以绕 z z z轴旋转 p i / 4 pi/4 pi/4为例。
- 初始化一个四元数
Eigen::Quaterniond q(w, a, b, c);
作用:声明一个四元数 q = w + a i + b j + c k q=w+a i+bj+ck q=w+ai+bj+ck
代码实现:
Eigen::Quaterniond q(0, 0, 0.383, 0.924);//初始化把w放前面
cout << "四元数输出方式1: q = \n" << q.coeffs() << endl; /* coeffs的顺序为(a,b,c,w), w为实部 */
cout << "四元数输出方式2:\nw = " << q.w() << "\nx = " << q.x()
<< "\ny = " << q.y() << "\nz = " << q.z() << endl;
代码运行结果:
四元数输出方式1: q =
0
0.383
0.924
0
四元数输出方式2:
w = 0
x = 0
y = 0.383
z = 0.924
- 初始化一个旋转矩阵
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
rotation_matrix << 0.707, -0.707, 0,
0.707, 0.707, 0,
0, 0, 1;
cout << "rotation matrix = \n" << rotation_matrix << endl;
代码运行结果:
rotation matrix =
0.707 -0.707 0
0.707 0.707 0
0 0 1
- 初始化一个旋转向量(轴角表示法)
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));
cout << "rotation_vector axis = \n" << rotation_vector.axis()
<< "\nrotation_vector angle = " << rotation_vector.angle() << endl;
代码运行结果:
rotation_vector axis =
0
0
1
rotation_vector angle = 0.785398
- 初始化一个欧拉角
Eigen::Vector3d euler_angles = Eigen::Vector3d(M_PI/4, 0, 0); /* zyx顺序,即roll pitch yaw 顺序 */
cout << "欧拉角(zyx) = " << euler_angles.transpose() <<endl;
代码运行结果:
欧拉角(zyx) = 0.785398 0 0
- 初始化一个平移向量
Eigen::Vector3d t(1, 2, 3);
cout << "平移向量 = " << t.transpose() <<endl;
代码运行结果
平移向量 = 1 2 3
- 初始化一个齐次欧式变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rotation_matrix);
T.pretranslate(t);
cout << T.matrix()<<endl;
代码运行结果:
0.707 -0.707 0 1
0.707 0.707 0 2
0 0 1 3
0 0 0 1
- 注意事项
所有和角度相关的量,输入和输出都是使用的弧度制, π \pi π的宏定义为M_PI。
常用函数
- 旋转向量、旋转矩阵、欧拉角转四元数
Eigen::Quaterniond q1=Eigen::Quaterniond(rotation_vector); /* 旋转向量能直接初始化四元数 */
Eigen::Quaterniond q2=Eigen::Quaterniond(rotation_matrix); /* 旋转矩阵能直接初始化四元数 */
//Eigen::Quaterniond q3=Eigen::Quaterniond(euler_angles); /* 欧拉角不能直接初始化四元数 */
/* 要想用欧拉角初始化四元数, 可以先将欧拉角转化为轴角表示,进而转化为四元数 */
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX()));
Eigen::Quaterniond q3 = yawAngle*pitchAngle*rollAngle;
cout << "q = " << q.coeffs().transpose() << "\n"
<< "q1 = " << q1.coeffs().transpose() << "\n"
<< "q2 = " << q2.coeffs().transpose() << "\n"
<< "q3 = " << q3.coeffs().transpose() << endl;
代码运行结果
q = 0 0.383 0.924 0
q1 = 0 0 0.382683 0.92388
q2 = 0 0 0.382638 0.923851
q3 = 0 0 0.382683 0.92388
- 四元数、旋转向量、欧拉角转旋转矩阵
Eigen::Matrix3d rotation_matrix1 = q.matrix(); /* 四元数能通过matrix方法直接初始化旋转矩阵 */
Eigen::Matrix3d rotation_matrix2 = rotation_vector.matrix(); /* 旋转向量能通过matrix方法直接初始化旋转矩阵 */
//Eigen::Matrix3d rotation_matrix2 = rotation_vector.toRotationMatrix(); /* 两种方法可选 */
/* 要想将欧拉角转换为旋转矩阵, 可以先将欧拉角转化为轴角表示,进而转化为旋转矩阵 */
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX()));
/* Eigen库不支持隐式将轴角赋值给旋转矩阵,只能显式赋值 */
Eigen::Matrix3d rotation_matrix3; /* 轴角表示不能直接初始化旋转矩阵,如果不用matrix方法,就只能通过赋值运算转换 */
rotation_matrix3 = yawAngle*pitchAngle*rollAngle;
cout << "rotation matrix = \n" << rotation_matrix << "\n"
<< "rotation matrix1 = \n" << rotation_matrix1 << "\n"
<< "rotation matrix2 = \n" << rotation_matrix2 << "\n"
<< "rotation matrix3 = \n" << rotation_matrix3 << endl;
代码运行结果
rotation matrix =
0.707 -0.707 0
0.707 0.707 0
0 0 1
rotation matrix1 =
-1.00093 0 0
0 -0.707552 0.707784
0 0.707784 0.706622
rotation matrix2 =
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
rotation matrix3 =
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
- 四元数、旋转矩阵、欧拉角转旋转向量
Eigen::AngleAxisd rotation_vector1(q);
Eigen::AngleAxisd rotation_vector2(rotation_matrix);
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd rotation_vector3;
rotation_vector3 = yawAngle*pitchAngle*rollAngle;
cout << "rotation_vector axis = " << rotation_vector.axis().transpose() << " rotation_vector angle = " << rotation_vector.angle() << "\n"
<< "rotation_vector1 axis = " << rotation_vector1.axis().transpose() << " rotation_vector1 angle = " << rotation_vector1.angle() << "\n"
<< "rotation_vector2 axis = " << rotation_vector2.axis().transpose() << " rotation_vector2 angle = " << rotation_vector2.angle() << "\n"
<< "rotation_vector3 axis = " << rotation_vector3.axis().transpose() << "
代码运行效果
rotation_vector axis = 0 0 1 rotation_vector angle = 0.785398
rotation_vector1 axis = 0 0.382911 0.923785 rotation_vector1 angle = 3.14159
rotation_vector2 axis = 0 0 1 rotation_vector2 angle = 0.785336
rotation_vector3 axis = 0 0 1 rotation_vector3 angle = 0.785398
- 四元数、旋转矩阵、旋转向量转欧拉角
Eigen::Vector3d euler_angles1 = q.matrix().eulerAngles(2,1,0); /* 2-z轴, 1-y轴, 0-x轴 8/
Eigen::Vector3d euler_angles2 = rotation_vector.matrix().eulerAngles(2,1,0);
Eigen::Vector3d euler_angles3 = rotation_matrix.eulerAngles(2,1,0);
cout << "欧拉角(zyx)euler_angles = " << euler_angles.transpose() << "\n"
<< "欧拉角(zyx)euler_angles1 = " << euler_angles.transpose() << "\n"
<< "欧拉角(zyx)euler_angles2 = " << euler_angles.transpose() << "\n"
<< "欧拉角(zyx)euler_angles3 = " << euler_angles.transpose() << endl;
代码运行结果
欧拉角(zyx)euler_angles = 0.785398 0 0
欧拉角(zyx)euler_angles1 = 0.785398 0 0
欧拉角(zyx)euler_angles2 = 0.785398 0 0
欧拉角(zyx)euler_angles3 = 0.785398 0 0
- 其他常用矩阵操作函数
M.transpose(); /* 矩阵M的转置 */
M.inverse(); /* 矩阵M的逆 */
M.determinant(); /* 矩阵M的行列式 */
M.sum(); /* 矩阵M各元素和 */
12*M; /* 矩阵数乘 */
/* 求矩阵M(要求实对称矩阵)特征值和特征向量 */
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( M );
cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;
Eigen::Matrix3d::Identity(); /* 单位三维矩阵 */
Eigen::Matrix3d::Random(); /* 三维随机数矩阵 */
头文件选择
Eigen的主要模块有基本模块Core,其他功能模块Geometry、LU、Clolesky、Householder、SVD、QR、Eigenvalues、Sparse,两个整合模块Dense、Eigen。一般情况下,#include <Eigen/Dense>
就足够了。
/* Eigen 头文件选择 */
#include <Eigen/Eigen> /* 包含了Eigen库的所有模块 */
#include <Eigen/Dense> /* 稠密矩阵的代数运算(逆,特征值等),包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块 */
#include <Eigen/Sparese> /* 稀疏矩阵相关的内容,如存储和基本线性运算 */
#include <Eigen/Core> /* 基本的Matrix和Array类,基本的线性代数运算和数组操作 */
#include <Eigen/Geometry> /* SLAM中常用的位姿变换(包括四元数、轴角等) */
#include <Eigen/LU> /* 矩阵求逆,求行列式, 矩阵LU分解 */
#include <Eigen/Cholesky> /* LLT和LDLT Cholesky分解 */
#include <Eigen/Householder> /* Householder变换,用于线性代数运算 */
#include <Eigen/SVD> /* SVD分解 */
#include <Eigen/QR> /* QR分解 */
#include <Eigen/Eigenvalues> /* 求方阵特征值、特征向量 */