Eigen是一个C++模板库,提供了广泛而强大的线性代数功能。它被设计用于高性能数值计算和科学计算,并且具有易于使用的接口和高度优化的实现。
其官方网址http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html
这里对它的使用做部分总结。
我目前一直使用的版本是3.3.9版本
下载地址https://eigen.tuxfamily.org/index.php?title=Main_Page
一路:
mkdir build
cd build
cmake ..
sudo make install
make doc
最后记得创建软链接(sudo ln -s 源文件 目标文件):
sudo ln -s /usr/local/include/eigen3/Eigen /usr/local/include/Eigen
安装完成后平时想随手编译一个eigen的测试程序,可以直接通过g++编译,如:
g++ test.cpp -o test -std=c++11 -o3
若添加对应的链接,可以加一个 -I /usr/local/include/eigen3
若使用cmake,也无需额外添加什么,若需要可以添加:
find_package(Eigen3 REQUIRED)、target_link_libraries(test ${EIGEN3_LIBS})
注意点1
eigen的加速优化。
eigen会根据编译器和硬件架构自动选择最佳的优化策略,并不需要手动指定特定的SIMD指令集什么的。
所以有时候我们会发现,大矩阵的运算时间竟然比小矩阵的运算时间要短,这大概是simd加速运算带来的现象。
建议编译的时候加上 -o3 优化选项
注意点2
eigen内存对齐。
http://eigen.tuxfamily.org/dox-devel/group__DenseMatrixManipulation__Alignement.html
Eigen使用内存对齐的原因是为了确保数据在内存中按照特定的对齐要求进行存储。
内存对齐的好处是它允许处理器更有效地访问内存,并允许向量化指令集(如SSE、AVX等)进行并行操作。如果数据没有按照正确的对齐要求进行存储,处理器可能需要额外的开销来访问和处理这些数据,从而降低性能。
不过,启用了c++17不需要理会这个问题。
当一个对齐的Eigen数据对象被传递给一个不对齐的Eigen数据对象时,可能会引发段错误。
当一个对齐的Eigen数据对象被传递给一个不对齐的Eigen数据对象时,可能会引发段错误。
当一个对齐的Eigen数据对象被传递给一个不对齐的Eigen数据对象时,可能会引发段错误。
eigen里,是否需要确定使用内存对齐有以下几个场景:
-
固定大小的数据类型,
Eigen::Vector2d
Eigen::Vector4d
Eigen::Vector4f
Eigen::Matrix2d
Eigen::Matrix2f
Eigen::Matrix4d
Eigen::Matrix4f
Eigen::Affine3d
Eigen::Affine3f
Eigen::Quaterniond
Eigen::Quaternionf
这些数据类型在默认情况下是对齐的 -
在类和结构体内声明EIGEN_MAKE_ALIGNED_OPERATOR_NEW
如:class Foo { ... Eigen::Vector4d v; ... public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; ...... ...... ...... ...... ...... ...... ...... ...... ...... struct Foo_d { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vector4d v; ... };
-
在STL容器里进行对齐声明
(目前发现,似乎是不用管这个了,不照着这个操作也不会出现段错误了)
如:std::map<int, Eigen::Vector4d, std::less<int>, Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4d> > > ...... ...... ...... ...... ...... ...... ...... ...... ...... std::vector<Eigen::Vector4f,Eigen::aligned_allocator<Eigen::Vector4f> >
-
作为函数参数时,需要通过引用传递它们,也就是加上 & 修饰符
void my_function(const Eigen::Vector2d& v);
矩阵类型定义、赋值、运算
1. eigen预定义的类型
这边举几个例子:Eigen::Matrix3f、Eigen::Matrix3Xcd、Eigen::Matrix3cd、Eigen::Vector3d
其中:
- 3 表示一个3_3大小的矩阵,类似的有 2_2 大小、4 *4 大小
- Matrix 表示是一个矩阵,类似的有Vector表示一个向量(n行1列的矩阵)
- 但是3X则代表3行X列的动态大小矩阵,在定义的时候需要指定大小,如:Eigen::Matrix3Xd matrix(3, 4);类似的有 2_X大小、4 _X 大小
- 若此处为X,也就是MatrixXf则代表行列数都不确定的动态大小矩阵,在定义的时候需要指定大小,如:Eigen::MatrixXd matrix(8, 6); 8*6大小
- f代表数据类型为float,类似的有d(double)、i(int)
- 但是cf代表复数,它的每一个元素都有一个实部一个虚部,数据类型为std::complex< float>,类似的还有cd、ci
std::complex< float> c1(1.0f, 2.0f); // 创建一个复数对象,实部为1.0,虚部为2.0
除去这些预定义类型外,我们可以自己决定数据类型和矩阵大小。
如:Eigen::Matrix< double, 3, 3>,表示一个元素为double类型的,3*4大小的矩阵
动态矩阵相关。
动态矩阵定义时需要指定大小,如:MatrixXf matrix(4, 4); 或者 Matrix< float,Dynamic,Dynamic> matrix(4, 4);
若不指定大小,如:MatrixXf b; 则表示定义了一个0*0大小的矩阵。
resize()可以修改动态矩阵的大小,静态矩阵则是不可以resize()的:matrix.resize(4, 3);//重新定义大小为4x3的矩阵
当给动态矩阵赋值一个动态矩阵时,原先的动态矩阵的尺寸也会发生改变
在Eigen中获取矩阵大小,可以使用.rows()和.cols()成员函数来获取矩阵的行数和列数
MatrixXf a(2, 2);
cout << "a is of size:\n" << a.rows() << "x" << a.cols() << endl;
MatrixXf b(3, 3);
a = b;
cout << "a is now of size:\n" << a.rows() << "x" << a.cols() << endl; // 此时a的大小是3*3,变为与b一样大
2. 矩阵的赋值,以及读取
向量(n行1列的矩阵)相关,我们可以在定义的的时候赋值,如:Eigen::Vector3d vector_3(10,50,60);
矩阵相关,这里以 3*3大小的float类型矩阵为例子。
Eigen::Matrix3f matrix_1;
若要赋值,有几种方式:
- 初始化为单位矩阵
Eigen::Matrix3d matrix; matrix.setIdentity(); // 将矩阵设置为单位矩阵 ...... ...... ...... ...... ...... ...... ...... ...... ...... Eigen::Matrix3d matrix = Eigen::Matrix3d::Identity(); // 使用构造函数初始化为单位矩阵
-
通过fill()进行统一赋值
Eigen::Matrix3f matrix_1; matrix_1.fill(10);
-
通过 << 进行赋值
matrix_1 << 3.9, 0.5, 0.8, -0.8, 0.2, 1.7, -0.6, -0.9, 0.2;
-
通过索引的方式对某一个元素赋值
例如将第一行第三列的值变为10.0matrix_1(0,2) = 10.0;
-
获取某一行或某一列
在Eigen中,可以使用.row()和.col()成员函数来获取矩阵的某一行或某一列。// 给矩阵的第2行赋值 matrix_1.row(1) << 10.0, 11.0, 12.0; // 给矩阵的第3列赋值 matrix_1.col(2) << 20.0, 21.0, 22.0;
-
通过block方式对某一块进行赋值
matrix_1.block<2, 2>(1, 0),其中(1, 0)代表以第2行第1列的元素作为起点,<2, 2>表示从顶点开始2行2列的范围。
它也可以表示为:matrix_1.block(1, 0, 2, 2);
即表示,将(1, 0)、(1, 1)、(2, 0)、(2, 1)matrix_1.block<2, 2>(1, 1) << 8.8, 8.1, 8.3, 8.5;
-
其它方式
另外Eigen库提供了许多类似于block的函数
topLeftCorner():提取矩阵的左上角部分。matrix_1.topLeftCorner(1, 2);
bottomLeftCorner():提取矩阵的左下角部分。
matrix_1.bottomLeftCorner(1, 2);
bottomRightCorner():提取矩阵的右下角部分。
matrix_1.bottomRightCorner(1, 2);
col():提取矩阵的某一列。
matrix_1.col(2); // 提取第3列
row():提取矩阵的某一行。
matrix_1.row(1); // 提取第2行
读取的话,跟赋值一个样子。。。注意矩阵大小和数据类型即可。
3. 矩阵的运算
-
矩阵的特殊运算
转置矩阵:matrix_1.transpose()
逆矩阵:matrix_1.inverse()
共轭矩阵:matrix_1.conjugate();
伴随矩阵: matrix_1.adjoint();
对角矩阵: matrix_1.diagonal(); -
矩阵的乘除加减
eigen重载了运算符 +、-、_、/、+=、-=、_=、/=
直接用 _ 表示乘法就可以了,matrix_A _ matrix_B。
除法就是乘法取逆,matrix_A _ matrix_B.inverse()
加减法,直接用 +、- 表示乘法就可以了
要注意,两个矩阵的加减是矩阵中对应的元素相加减,相加减的前提是:两个矩阵要是通行矩阵,即具有相同的行和列数
要注意,矩阵的乘法操作的顺序非常重要,因为矩阵和向量的乘法操作是不可交换的
要注意,矩阵的维度,维度不匹配的矩阵相加减乘除会报错的
要注意,矩阵的数据类型,数据类型不匹配的矩阵相加减乘除会报错的
要注意,矩阵_或/一个常数(matrix_A _ 2),代表对矩阵的每一个元素都_或/一个常数。加减则不行,但 - 表负(-matrix_A)时,对每个元素取负数 -
矩阵的数据类型转换
上面提到,矩阵的数据类型,数据类型不匹配的矩阵相加减乘除会报错
我们可以进行数据类型转换,使用.cast();,float为我们要更改的类型,如: Eigen::Matrix3d doubleMatrix; doubleMatrix << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; // 将double类型矩阵转换为float类型 Eigen::Matrix3f floatMatrix = doubleMatrix.cast<float>();
-
矩阵的数组化操作
在Eigen中,有时需要对对矩阵进行逐元素操作。
例如,需要将 matrix 的元素都加一个常数。Eigen::Matrix3d matrix; matrix << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; matrix = matrix.array() + 2.0; // 逐元素操作 std::cout << "matrix: \n" << matrix << std::endl; // 打印结果
同时在Eigen中,可以使用Eigen::Map类将数组转换为矩阵
double data[9] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0}; // 将数组与矩阵关联 Eigen::Map<Eigen::Matrix3d> matrix(data);
欧拉角、四元素、旋转矩阵、轴角变换
- 在eigen里的定义
欧拉角在eigen里的常用表示是一个3维向量,以弧度为单位,如:
四元素在eigen里的常用表示为Quaterniond类型:Eigen::Vector3d euler_angles3(2.1, 1.5, 3.1);
旋转矩阵在eigen里常用表示为一个3*3的矩阵Eigen::Quaterniond quat(1.0, 0.0, 0.0, 0.0); // 创建一个四元数,参数分别为实部、虚部 i、虚部 j、虚部 k
注意要满足旋转矩阵的性质
轴角变换在eigen里常用表示为AngleAxisd类型Eigen::Matrix3d matrix = Eigen::Matrix3d::Identity(); // 使用构造函数初始化为单位矩阵
轴角变换的轴向量应为单位向量(长度为1):确保轴向量表示一个有效的旋转轴
轴角变换的轴向量应不等于零向量:确保旋转轴是非零向量,因为零向量无法表示旋转
轴角变换的旋转角度单位为弧度double angle = 0.7854; // 旋转角度(单位为弧度) Eigen::Vector3d axis(0.5774, 0.5774, 0.5774); // 旋转轴向量 Eigen::AngleAxisd rotation(angle, axis); // 使用角度和轴向量初始化 ...... ...... ...... ...... ...... ...... ...... ...... ...... Eigen::AngleAxisd rollAngle( 1.57, Eigen::Vector3d::UnitX()); // Eigen::Vector3d::UnitX()表示绕X旋转,即roll角
- 相互转换
原理上来讲,他们的转换关系是这样子的,但是在eigen里毕竟是封装好了的,都可以互转。
欧拉角的转换:
欧拉角可以直接转为转轴角
欧拉角转四元素:欧拉角先转为绕xyz的轴角,再转四元素
欧拉角转旋转矩阵:欧拉角先转为绕xyz的轴角,再转旋转矩阵
四元素的转换:// 欧拉角转轴角 Eigen::AngleAxisd rollAngle(3, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd pitchAngle(0.5, Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle(0, Eigen::Vector3d::UnitZ()); ...... ...... ...... ...... ...... ...... ...... ...... ...... // 欧拉角转四元素:欧拉角先转为绕xyz的轴角,再转四元素 Eigen::Quaterniond quaternion; quaternion = Eigen::AngleAxisd(3, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()); ...... ...... ...... ...... ...... ...... ...... ...... ...... // 欧拉角转旋转矩阵:欧拉角先转为绕xyz的轴角,再转旋转矩阵 Eigen::AngleAxisd rollAngle(3, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd pitchAngle(0.5, Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle(0, Eigen::Vector3d::UnitZ()); Eigen::Matrix3d ritation_matrix = rollAngle * pitchAngle * yawAngle;
四元素可以直接转旋转矩阵
四元素可以直接转欧拉角
四元素可以直接转轴角
旋转矩阵的转换:// 四元素转旋转矩阵 Eigen::Quaterniond quaternion(0.9239, 0.3827, 0, 0); Eigen::Matrix3d rotationMatrix = quaternion.toRotationMatrix(); ...... ...... ...... ...... ...... ...... ...... ...... ...... // 四元素转欧拉角 Eigen::Vector3d euler_angles = quaternion.toRotationMatrix().eulerAngles(2, 1, 0); // (2, 1, 0)表示zyx旋转顺序(yaw, pitch, roll) ...... ...... ...... ...... ...... ...... ...... ...... ...... // 四元素转轴角 Eigen::Vector3d axis = quaternion.vec().normalized();// 获取旋转轴向量 double angle = 2.0 * std::acos(quaternion.w());// 获取旋转角度
旋转矩阵可以直接转四元素
旋转矩阵可以直接转欧拉角
旋转矩阵可以直接转轴角
轴角的转换:// 旋转矩阵转四元素 Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Identity(); Eigen::Quaterniond quaternion(rotationMatrix); ...... ...... ...... ...... ...... ...... ...... ...... ...... // 旋转矩阵转欧拉角 Eigen::Vector3d euler_angles = rotationMatrix.eulerAngles(2, 1, 0); // (2, 1, 0)表示zyx旋转顺序(yaw, pitch, roll) ...... ...... ...... ...... ...... ...... ...... ...... ...... // 旋转矩阵转轴角 Eigen::AngleAxisd axisAngle(rotationMatrix);
轴角可以直接转四元素
轴角可以直接转欧拉角
轴角可以直接转旋转矩阵// 轴角转四元素 double angle = 0.7854; // 旋转角度(单位为弧度) Eigen::Vector3d axis(0.5774, 0.5774, 0.5774); // 旋转轴向量 Eigen::AngleAxisd angle_axi(angle, axis); // 使用角度和轴向量初始化 Eigen::Quaterniond quaternion(angle_axi); ...... ...... ...... ...... ...... ...... ...... ...... ...... // 轴角转欧拉角 Eigen::Vector3d euler_angles = angle_axi.eulerAngles(2, 1, 0);// (2, 1, 0)表示zyx旋转顺序(yaw, pitch, roll) ...... ...... ...... ...... ...... ...... ...... ...... ...... // 轴角转旋转矩阵 Eigen::Matrix3d rotationMatrix = angle_axi.toRotationMatrix()
轴角变换常用来做插值,下面给个例子:
// 轴角变换插值
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
int main() {
Eigen::Matrix3d start_matrix;
Eigen::Matrix3d end_matrix;
{
Eigen::AngleAxisd rollAngle(0, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(1, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(1.5, Eigen::Vector3d::UnitZ());
start_matrix = rollAngle * pitchAngle * yawAngle;
}
{
Eigen::AngleAxisd rollAngle(3, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(0.5, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(0, Eigen::Vector3d::UnitZ());
end_matrix = rollAngle * pitchAngle * yawAngle;
}
Eigen::Matrix3d Rs = end_matrix * start_matrix.inverse();
Eigen::AngleAxisd interpolated_angle_axis(Rs);
Eigen::Vector3d iaxis = interpolated_angle_axis.axis();
double iangle = interpolated_angle_axis.angle();
std::cout << "iangle: " << iangle << std::endl;
std::cout << "iaxis: " << iaxis.transpose() << std::endl;
// std::cout << iaxis.norm() << std::endl;
Eigen::Vector3d euler_angles2 = start_matrix.eulerAngles(0, 1, 2);
std::cout << "start_matrix rpy: " << euler_angles2.transpose() << std::endl;
Eigen::Vector3d euler_angles1 = end_matrix.eulerAngles(0, 1, 2);
std::cout << "end_matrix rpy: " << euler_angles1.transpose() << std::endl;
for(double ratio = 0; ratio <= 1.0; ratio+=0.1){
Eigen::Matrix3d R_tmp = Eigen::AngleAxisd(ratio * iangle, iaxis).toRotationMatrix() * start_matrix;
Eigen::Vector3d euler_angles = R_tmp.eulerAngles(0, 1, 2);
std::cout << "angle: " << ratio * iangle << "\t rpy: " << euler_angles.transpose() << std::endl;
}
return 0;
}
运行结果:
over~
欣赏一下我的壁纸
评论(8)
您还未登录,请登录后发表或查看评论