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>  /* 求方阵特征值、特征向量 */

参考文献

Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换
Eigen教程1----Eigen的介绍

  • 1
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值