Eigen库函数实现欧拉角与旋转矩阵转换

59 阅读 0 评论 39 点赞

我是靠谱客的博主含糊睫毛膏，最近开发中收集的这篇文章主要介绍Eigen库函数实现欧拉角与旋转矩阵转换，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

一、简介

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。Eigen是一个用纯头文件搭建起来的库，这意味这你只要能找到它的头文件，就能使用它。Eigen头文件的默认位置是“/usr/include/eigen3”.

由于Eigen库相较于OpenCV中的Mat等库而言更加高效，许多上层的软件库也使用Eigen进行矩阵运算，比如SLAM中常用的g2o,Sophus等。此外Eigen库还被被用于Caffe，Tensorflow等许多深度学习相关的框架中。

Eigen申明变量时有点类似于c语言，类型在变量的前面，而opencv中Mat申明变量时是c++中直接制定获取是构造函数中构造类型。

二、Eigen的学习

1. 向量矩阵基本操作

向量

Eigen::Vector3d v3d; //向量
Eigen::VectorXd v(6); //需要指定大小(6)
v3d << 1,2,3;
cout << "向量 v3d = "<< endl << v3d << endl << endl;
v << 1,2,3,4,5,6;
cout << "向量 v = "<< endl << v << endl << endl;

输出：

向量 v3d =
1
2
3

向量 v =
1
2
3
4
5
6

矩阵基本操作

Eigen::Matrix<float,2,3> m_f23; //行、列
Eigen::MatrixXd m_d24(2,4);
Eigen::Matrix3d m_d33;
m_f23 << 1,2,3,
       4,5,6;
cout << "矩阵 m_f23 = "<< endl << m_f23 << endl << endl;
m_d24 << 1,2,3,4,
       5,6,7,8;
cout << "矩阵 m_d24 = "<< endl << m_d24 << endl << endl;
m_d33 << 1,2,3,
       4,5,6,
       7,8,9;
cout << "矩阵 m_d33 = "<< endl << m_d33 << endl << endl;

输出：

矩阵 m_f23 =
1 2 3
4 5 6

矩阵 m_d24 =
1 2 3 4
5 6 7 8

矩阵 m_d33 =
1 2 3
4 5 6
7 8 9

矩阵利用函数进行赋值

Eigen::Matrix4d m_d44 = Eigen::Matrix4d::Zero();
cout << "Zero 矩阵 m_d44 = " << endl << m_d44 << endl << endl;
m_d44 = Eigen::Matrix4d::Random();
cout << "矩阵 m_d44 = "<< endl << m_d44 << endl << endl;
Eigen::MatrixXd m_dnn(5,5);
m_dnn = Eigen::MatrixXd::Random(5, 5);
cout << "Random 矩阵 m_dnn = "<< endl << m_dnn << endl << endl;

输出：

Zero 矩阵 m_d44 =
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0

矩阵 m_d44 =
0.680375 0.823295 -0.444451 -0.270431
-0.211234 -0.604897 0.10794 0.0268018
0.566198 -0.329554 -0.0452059 0.904459
0.59688 0.536459 0.257742 0.83239

Random 矩阵 m_dnn =
0.271423 -0.514226 -0.740419 0.678224 -0.012834
0.434594 -0.725537 -0.782382 0.22528 0.94555
-0.716795 0.608354 0.997849 -0.407937 -0.414966
0.213938 -0.686642 -0.563486 0.275105 0.542715

获取指定元素的值

for(int i = 0 ;i < 5; i++){
   for(int j = 0; j < 5; j++){
       cout << m_dnn(i,j) << "," ;
   }
   cout << endl << endl;
}

获取矩阵的子矩阵（block）

Eigen::MatrixXf m_block(5,5);
m_block <<1,2,3,4,5,
   6,7,8,9,10,
   11,12,13,14,15,
   16,17,18,19,20,
   21,22,23,24,25;
Eigen::MatrixXf m_sun1 = m_block.block<2,2>(1,1);//从1行1列开始，获取2*2的子矩阵矩阵
cout << "block 矩阵 m_block.block<2,2>(1,1) = "<< endl << m_sun1 << endl << endl;
Eigen::MatrixXf m_sun2 = m_block.block(1,1,2,2);//同上
cout << "block 矩阵 m_block.block(1,1,2,2) = "<< endl << m_sun2 << endl << endl;

cout << "矩阵 m_block m_block.row(0) = "<< endl << m_block.row(0) << endl << endl;
cout << "矩阵 m_block m_block.col(0) = "<< endl << m_block.col(0) << endl << endl;

输出：

block 矩阵 m_block.block<2,2>(1,1) =
7 8
12 13

block 矩阵 m_block.block(1,1,2,2) =
7 8
12 13

矩阵 m_block m_block.row(0) =
1 2 3 4 5

矩阵 m_block m_block.col(0) =
1
6
11
16
21

矩阵组合操作

Eigen::Matrix3f R;
R << 1,2,3,4,5,6,7,8,9;
Eigen::Matrix<float,3,1> T;
T << 11,11,11;
Eigen::Matrix<float,1,4> B;
B << 22,22,22,22;
Eigen::Matrix<float,4,4> H;
H << R,T,B;
cout << "其次矩阵 H = "<< endl << H << endl << endl;
Eigen::Matrix<float,2,4> newH;
newH << H.block(0,0,1,4),B;
cout << "其次矩阵 newH = "<< endl << newH << endl << endl;

输出：

其次矩阵 H =
1 2 3 11
4 5 6 11
7 8 9 11
22 22 22 22

其次矩阵 newH =
1 2 3 11
22 22 22 22

2.矩阵进行运算

矩阵相乘

Eigen::Matrix3d m1,m2;
m1 << 1,0,0,
       0,1,0,
       0,0,2;
m2 << 1,2,3,
       4,5,6,
       7,8,9;
cout << "m1 * m2 = "<< endl << m1 * m2 << endl << endl;

矩阵转置

cout << "转置 m2.transpose() = "<< endl << m2.transpose() << endl << endl;

矩阵求和

cout << "求和 m2.sum() = "<< endl << m2.sum() << endl << endl;

矩阵求逆

cout << "求逆 m2.inverse() = "<< endl << m2.inverse() << endl << endl;

矩阵迹

cout << "迹 m2.trace() = "<< endl << m2.trace() << endl << endl;

矩阵数乘

cout << "数乘 10*m2 = "<< endl << 10 * m2 << endl << endl;

矩阵行列式

cout << "行列式 m2.determinant() = "<< endl << m2.determinant() << endl << endl;

m1 * m2 =
1 2 3
4 5 6
14 16 18

转置 m2.transpose() =
1 4 7
2 5 8
3 6 9

求和 m2.sum() =
45

求逆 m2.inverse() =
-inf inf -inf
inf -inf inf
-inf inf -inf

迹 m2.trace() =
15

数乘 10*m2 =
10 20 30
40 50 60
70 80 90

行列式 m2.determinant() =
0

3、代码

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
int main()
{
/*+++++++++++++++++++++++++++
一、向量矩阵基本操作
++++++++++++++++**/
//1、向量
Eigen::Vector3d v3d;
//向量
Eigen::VectorXd v(6);
//需要指定大小(6)
v3d << 1,2,3;
cout << "向量 v3d = "<< endl << v3d << endl << endl;
v << 1,2,3,4,5,6;
cout << "向量 v = "<< endl << v << endl << endl;
//2、矩阵基本操作
Eigen::Matrix<float,2,3> m_f23;
//行、列
Eigen::MatrixXd m_d24(2,4);
Eigen::Matrix3d m_d33;
m_f23 << 1,2,3,
4,5,6;
cout << "矩阵 m_f23 = "<< endl << m_f23 << endl << endl;
m_d24 << 1,2,3,4,
5,6,7,8;
cout << "矩阵 m_d24 = "<< endl << m_d24 << endl << endl;
m_d33 << 1,2,3,
4,5,6,
7,8,9;
cout << "矩阵 m_d33 = "<< endl << m_d33 << endl << endl;
//3、矩阵利用函数进行赋值
Eigen::Matrix4d m_d44 = Eigen::Matrix4d::Zero();
cout << "Zero 矩阵 m_d44 = " << endl << m_d44 << endl << endl;
m_d44 = Eigen::Matrix4d::Random();
cout << "矩阵 m_d44 = "<< endl << m_d44 << endl << endl;
Eigen::MatrixXd m_dnn(5,5);
m_dnn = Eigen::MatrixXd::Random(5, 5);
cout << "Random 矩阵 m_dnn = "<< endl << m_dnn << endl << endl;
//4、获取指定元素的值
for(int i = 0 ;i < 5; i++){
for(int j = 0; j < 5; j++){
cout << m_dnn(i,j) << "," ;
}
cout << endl << endl;
}
//5、获取矩阵的子矩阵（block）
Eigen::MatrixXf m_block(5,5);
m_block <<1,2,3,4,5,
6,7,8,9,10,
11,12,13,14,15,
16,17,18,19,20,
21,22,23,24,25;
Eigen::MatrixXf m_sun1 = m_block.block<2,2>(1,1);//从1行1列开始，获取2*2的子矩阵矩阵
cout << "block 矩阵 m_block.block<2,2>(1,1) = "<< endl << m_sun1 << endl << endl;
Eigen::MatrixXf m_sun2 = m_block.block(1,1,2,2);//同上
cout << "block 矩阵 m_block.block(1,1,2,2) = "<< endl << m_sun2 << endl << endl;
cout << "矩阵 m_block m_block.row(0) = "<< endl << m_block.row(0) << endl << endl;
cout << "矩阵 m_block m_block.col(0) = "<< endl << m_block.col(0) << endl << endl;
//6、矩阵组合操作
Eigen::Matrix3f R;
R << 1,2,3,4,5,6,7,8,9;
Eigen::Matrix<float,3,1> T;
T << 11,11,11;
Eigen::Matrix<float,1,4> B;
B << 22,22,22,22;
Eigen::Matrix<float,4,4> H;
H << R,T,B;
cout << "其次矩阵 H = "<< endl << H << endl << endl;
Eigen::Matrix<float,2,4> newH;
newH << H.block(0,0,1,4),B;
cout << "其次矩阵 newH = "<< endl << newH << endl << endl;
/*------------------
end
-----------------**/
/*+++++++++++++++++++++++++++
二、矩阵进行运算
++++++++++++++++**/
// 1、矩阵相乘
Eigen::Matrix3d m1,m2;
m1 << 1,0,0,
0,1,0,
0,0,2;
m2 << 1,2,3,
4,5,6,
7,8,9;
cout << "m1 * m2 = "<< endl << m1 * m2 << endl << endl;
//2、矩阵转置
cout << "转置
m2.transpose() = "<< endl << m2.transpose() << endl << endl;
//3、矩阵求和
cout << "求和
m2.sum() = "<< endl << m2.sum() << endl << endl;
//4、矩阵求逆
cout << "求逆
m2.inverse() = "<< endl << m2.inverse() << endl << endl;
//5、矩阵迹
cout << "迹
m2.trace() = "<< endl << m2.trace() << endl << endl;
//6、矩阵数乘
cout << "数乘
10*m2 = "<< endl << 10 * m2 << endl << endl;
//7、矩阵行列式
cout << "行列式
m2.determinant() = "<< endl << m2.determinant() << endl << endl;
return 0;
}

三、矩阵之间的转换

1、欧拉角转旋转矩阵

//这里theta也是弧度制
static Eigen::Matrix3d eul2rotm(Eigen::Vector3d &theta,string m_sSeq)
{
Eigen::Matrix3d rotX;
// 计算旋转矩阵的X分量
rotX <<
1,
0,
0,
0,
cos(theta[0]),
-sin(theta[0]),
0,
sin(theta[0]),
cos(theta[0]);
Eigen::Matrix3d rotY;
// 计算旋转矩阵的Y分量
rotY <<
cos(theta[1]),
0, sin(theta[1]),
0,
1,
0,
-sin(theta[1]),
0, cos(theta[1]);
Eigen::Matrix3d rotZ;
// 计算旋转矩阵的Z分量
rotZ <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]),
cos(theta[2]), 0,
0,
0,
1;
Eigen::Matrix3d R;
if (m_sSeq == "zyx")
{
R = rotX * rotY * rotZ;
}
else if (m_sSeq == "yzx")
{
R = rotX * rotZ * rotY;
}
else if (m_sSeq == "zxy")
{
R = rotY * rotX * rotZ;
}
else if (m_sSeq == "xzy")
{
R = rotY * rotZ * rotX;
}
else if (m_sSeq == "yxz")
{
R = rotZ * rotX * rotY;
}
else if (m_sSeq == "xyz")
{
R = rotZ * rotY * rotX;
}
return R;
}

2、旋转矩阵转欧拉角

static Eigen::Vector3d rotm2eul(Eigen::Matrix3d &R)
{
double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
bool singular = sy < 1e-6;
double x, y, z;
if (!singular)
{
x = atan2( R(2,1), R(2,2));
y = atan2(-R(2,0), sy);
z = atan2( R(1,0), R(0,0));
}
else
{
x = atan2(-R(1,2), R(1,1));
y = atan2(-R(2,0), sy);
z = 0;
}
return {x, y, z};
}