草庐IT

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

乾坤未定 2023-04-22 原文

一、简介

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};
}

欧拉Eigenltendl矩阵c++

有关Eigen库函数实现欧拉角与旋转矩阵转换的更多相关文章

  1. ruby - 在没有 sass 引擎的情况下使用 sass 颜色函数 - 2

    我想在一个没有Sass引擎的类中使用Sass颜色函数。我已经在项目中使用了sassgem，所以我认为搭载会像以下一样简单:classRectangleincludeSass::Script::FunctionsdefcolorSass::Script::Color.new([0x82,0x39,0x06])enddefrender#hamlengineexecutedwithcontextofself#sothatwithintemlateicouldcall#%stop{offset:'0%',stop:{color:lighten(color)}}endend更新:参见上面的#re

  2. ruby-on-rails - 在 ruby​​ 中使用 gsub 函数替换单词 - 2

    我正在尝试用ruby​​中的gsub函数替换字符串中的某些单词，但有时效果很好，在某些情况下会出现此错误？这种格式有什么问题吗NoMethodError(undefinedmethod`gsub!'fornil:NilClass):模型.rbclassTest"replacethisID1",WAY=>"replacethisID2andID3",DELTA=>"replacethisID4"}end另一个模型.rbclassCheck 最佳答案 啊，我找到了!gsub!是一个非常奇怪的方法。首先，它替换了字符串，所以它实际上修改了

  3. ruby - 如何根据特征实现 FactoryGirl 的条件行为 - 2

    我有一个用户工厂。我希望默认情况下确认用户。但是鉴于unconfirmed特征，我不希望它们被确认。虽然我有一个基于实现细节而不是抽象的工作实现，但我想知道如何正确地做到这一点。factory:userdoafter(:create)do|user,evaluator|#unwantedimplementationdetailshereunlessFactoryGirl.factories[:user].defined_traits.map(&:name).include?(:unconfirmed)user.confirm!endendtrait:unconfirmeddoenden

  4. ruby - 在 Ruby 中有条件地定义函数 - 2

    我有一些代码在几个不同的位置之一运行:作为具有调试输出的命令行工具，作为不接受任何输出的更大程序的一部分，以及在Rails环境中。有时我需要根据代码的位置对代码进行细微的更改，我意识到以下样式似乎可行:print"Testingnestedfunctionsdefined\n"CLI=trueifCLIdeftest_printprint"CommandLineVersion\n"endelsedeftest_printprint"ReleaseVersion\n"endendtest_print()这导致:TestingnestedfunctionsdefinedCommandLin

  5. 旋转矩阵的几何意义 - 2

    点向量坐标矩阵的几何意义介绍旋转矩阵的几何含义之前，先介绍一下点向量坐标矩阵的几何含义点：在一维空间下就是一个标量，如同一条直线上，以任意某一个位置为0点，以一定的尺度间隔为1，2，3...,相反方向为-1，-2，-3...；如此就形成了一维坐标系，这时候任何一个点都可以用一个数值表示，如点p1=5，即即从原点出发沿着x轴正方向移动5个尺度；点p2=-3，负方向移动3个尺度；     在一维坐标系上过原点做垂直于一维坐标系的直线，则形成了二维坐标系，此时描述一个点需要两个数值来表示点p3=（3，2），即从原点出发沿着x轴正方向移动3个尺度，在此基础上沿着y轴正方向移动两个尺度的位置就是点p3。

  6. ruby - 在 Ruby 中按名称传递函数 - 2

    如何在Ruby中按名称传递函数？(我使用Ruby才几个小时，所以我还在想办法。)nums=[1,2,3,4]#Thisworks,butismoreverbosethanI'dlikenums.eachdo|i|putsiend#InJS,Icouldjustdosomethinglike:#nums.forEach(console.log)#InF#,itwouldbesomethinglike:#List.iternums(printf"%A")#InRuby,IwishIcoulddosomethinglike:nums.eachputs在Ruby中能不能做到类似的简洁？我可以只

  7. 华为OD机试用Python实现 -【明明的随机数】 2023Q1A - 2

    华为OD机试题本篇题目：明明的随机数题目输入描述输出描述：示例1输入输出说明代码编写思路最近更新的博客华为od2023|什么是华为od，od薪资待遇，od机试题清单华为OD机试真题大全，用Python解华为机试题|机试宝典【华为OD机试】全流程解析+经验分享,题型分享,防作弊指南华为o

  8. Unity 3D 制作开关门动画,旋转门制作,推拉门制作,门把手动画制作 - 2

    Unity自动旋转动画1.开门需要门把手先动,门再动2.关门需要门先动,门把手再动3.中途播放过程中不可以再次进行操作觉得太复杂?查看我的文章开关门简易进阶版效果:如果这个门可以直接打开的话,就不需要放置"门把手"如果门把手还有钥匙需要旋转,那就可以把钥匙放在门把手的"门把手",理论上是可以无限套娃的可调整参数有:角度,反向,轴向,速度运行时点击Test进行测试自己写的代码比较垃圾,命名与结构比较拉,高手轻点喷,新手有类似的需求可以拿去做参考上代码usingSystem.Collections;usingSystem.Collections.Generic;usingUnityEngine;u

  9. 基于C#实现简易绘图工具【100010177】 - 2

    C#实现简易绘图工具一.引言实验目的:通过制作窗体应用程序(C#画图软件),熟悉基本的窗体设计过程以及控件设计,事件处理等,熟悉使用C#的winform窗体进行绘图的基本步骤,对于面向对象编程有更加深刻的体会.Tutorial任务设计一个具有基本功能的画图软件**·包括简单的新建文件,保存,重新绘图等功能**·实现一些基本图形的绘制,包括铅笔和基本形状等,学习橡皮工具的创建**·设计一个合理舒适的UI界面**注明:你可能需要先了解一些关于winform窗体应用程序绘图的基本知识,以及关于GDI+类和结构的知识二.实验环境Windows系统下的visualstudio2017C#窗体应用程序三.

  10. C51单片机——实现用独立按键控制LED亮灭（调用函数篇） - 2

    说在前面这部分我本来是合为一篇来写的，因为目的是一样的，都是通过独立按键来控制LED闪灭本质上是起到开关的作用，即调用函数和中断函数。但是写一篇太累了，我还是决定分为两篇写，这篇是调用函数篇。在本篇中你主要看到这些东西！！！1.调用函数的方法（主要讲语法和格式）2.独立按键如何控制LED亮灭3.程序中的一些细节（软件消抖等）1.调用函数的方法思路还是比较清晰地，就是通过按下按键来控制LED闪灭，即每按下一次，LED取反一次。重要的是，把按键与LED联系在一起。我打算用K1来作为开关，看了一下开发板原理图，K1连接的是单片机的P31口，当按下K1时，P31是与GND相连的，也就是说，当我按下去时

随机推荐