jjzjj

Eigen(2):使用eigen进行四元数与旋转矩阵转换(c++实现)

biter0088 2024-05-03 原文

1 四元数转旋转矩阵

        使用了normalized()函数和toRotationMatrix()函数

//下面的变量名称自拟      
    Eigen::Quaterniond q_odom_curr_tmp;//声明一个Eigen类的四元数
    //此处进行赋值，使用其他语句以及合理的常数也可
    q_odom_curr_tmp.x() = imuVec[imuVec.size()-1].orientation.x;
    q_odom_curr_tmp.y() = imuVec[imuVec.size()-1].orientation.y;
    q_odom_curr_tmp.z() = imuVec[imuVec.size()-1].orientation.z;
    q_odom_curr_tmp.w() =imuVec[imuVec.size()-1].orientation.w;
    Eigen::Matrix3d R_odom_curr_tmp;//声明一个Eigen类的3*3的旋转矩阵
    //四元数转为旋转矩阵--先归一化再转为旋转矩阵
    R_odom_curr_tmp= q_odom_curr_tmp.normalized().toRotationMatrix();

        打印输出四元数：

    std::cout<<"q_odom_curr_now:"<< q_odom_curr_now.x() <<q_odom_curr_now.y()<<q_odom_curr_now.z()<<q_odom_curr_now.w()<<std::endl;

2 旋转矩阵转四元数

        下面使用了inverse()--求逆函数，Eigen::Quaterniond()--求四元数，normalize()--归一化

//注：下面的赋值方法及变量名称自己按情况定
Eigen::Matrix3d R_odom_curr_now;//3*3旋转矩阵
R_odom_curr_now=R_odom_curr0.inverse()*R_odom_curr_tmp;//对旋转矩阵进行赋值
q_odom_curr_now=Eigen::Quaterniond(R_odom_curr_now);//旋转矩阵转为四元数
q_odom_curr_now.normalize();//转为四元数之后，需要进行归一化

3 四元数转欧拉角

        下面是函数的主体代码，用到了ros的tf::Quaternion和tf::Matrix3x3，Eigen可用可不用，主要传递给函数的参数能够代替四元数即可，返回值为角度值

#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#define M_PI 3.1415926;

double getyaw(Eigen::Quaterniond qua){
  tf::Quaternion q(qua.x(),
    qua.y(),
    qua.z(),
    qua.w());
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  return yaw*180.0/M_PI;
}

4 变换矩阵、旋转矩阵/四元数、平移向量

四元数+平移向量构造变换矩阵

      Eigen::Quaterniond q_odom_curr0;//四元数
      Eigen::Isometry3d T_1=Eigen::Isometry3d::Identity(); //变换矩阵
      T_1.rotate ( q_odom_curr0.toRotationMatrix() );  
      T_1.pretranslate ( Eigen::Vector3d ( 0,0,0 ) );

 变换矩阵转旋转向量/四元数+平移向量

      Eigen::Matrix3d rotation_matrix;//旋转矩阵
      rotation_matrix = T_1.rotation();
      t_odom_curr_now = T_1.translation();//t_odom_curr_now 平移向量
      q_odom_curr_now = Eigen::Quaterniond ( rotation_matrix );//转为四元数

5 四元数转欧拉角度--欧拉角转四元数

见：无人驾驶学习（二）：定位与导航_biter0088的博客-CSDN博客_无人驾驶定位

 有c++代码

 

参考链接：

讲解四元数、旋转矩阵、欧拉角的转换，很不错：

https://zhuanlan.zhihu.com/p/259999988

讲解了Eigen与矩阵相关的函数和类，也很不错

https://blog.csdn.net/weixin_31707171/article/details/112363668

eigen矩阵索引：

https://blog.csdn.net/xiaoyucyt/article/details/87866445

四元数、旋转矩阵、平移向量、变换矩阵等：

https://blog.csdn.net/HERO_CJN/article/details/82426372

https://blog.csdn.net/qq_40081208/article/details/102937581

https://www.cnblogs.com/jerry323/p/9097264.html

@meng

c++Eigencurrodomhttps矩阵线性代数

有关Eigen(2):使用eigen进行四元数与旋转矩阵转换(c++实现)的更多相关文章

  1. ruby - 如何使用 Nokogiri 的 xpath 和 at_xpath 方法 - 2

    我正在学习如何使用Nokogiri，根据这段代码我遇到了一些问题:require'rubygems'require'mechanize'post_agent=WWW::Mechanize.newpost_page=post_agent.get('http://www.vbulletin.org/forum/showthread.php?t=230708')puts"\nabsolutepathwithtbodygivesnil"putspost_page.parser.xpath('/html/body/div/div/div/div/div/table/tbody/tr/td/div

  2. ruby - 使用 RubyZip 生成 ZIP 文件时设置压缩级别 - 2

    我有一个Ruby程序，它使用rubyzip压缩XML文件的目录树。gem。我的问题是文件开始变得很重，我想提高压缩级别，因为压缩时间不是问题。我在rubyzipdocumentation中找不到一种为创建的ZIP文件指定压缩级别的方法。有人知道如何更改此设置吗？是否有另一个允许指定压缩级别的Ruby库？ 最佳答案 这是我通过查看ruby​​zip内部创建的代码。level=Zlib::BEST_COMPRESSIONZip::ZipOutputStream.open(zip_file)do|zip|Dir.glob("**/*")d

  3. ruby - 为什么我可以在 Ruby 中使用 Object#send 访问私有(private)/ protected 方法？ - 2

    类classAprivatedeffooputs:fooendpublicdefbarputs:barendprivatedefzimputs:zimendprotecteddefdibputs:dibendendA的实例a=A.new测试a.foorescueputs:faila.barrescueputs:faila.zimrescueputs:faila.dibrescueputs:faila.gazrescueputs:fail测试输出failbarfailfailfail.发送测试[:foo,:bar,:zim,:dib,:gaz].each{|m|a.send(m)resc

  4. ruby-on-rails - 使用 Ruby on Rails 进行自动化测试 - 最佳实践 - 2

    很好奇，就使用ruby​​onrails自动化单元测试而言，你们正在做什么？您是否创建了一个脚本来在cron中运行rake作业并将结果邮寄给您？git中的预提交Hook？只是手动调用？我完全理解测试，但想知道在错误发生之前捕获错误的最佳实践是什么。让我们理所当然地认为测试本身是完美无缺的，并且可以正常工作。下一步是什么以确保他们在正确的时间将可能有害的结果传达给您？ 最佳答案 不确定您到底想听什么，但是有几个级别的自动代码库控制:在处理某项功能时，您可以使用类似autotest的内容获得关于哪些有效，哪些无效的即时反馈。要确保您的提

  5. ruby - 在 Ruby 中使用匿名模块 - 2

    假设我做了一个模块如下:m=Module.newdoclassCendend三个问题:除了对m的引用之外，还有什么方法可以访问C和m中的其他内容？我可以在创建匿名模块后为其命名吗(就像我输入“module...”一样)？如何在使用完匿名模块后将其删除，使其定义的常量不再存在？ 最佳答案 三个答案:是的，使用ObjectSpace.此代码使c引用你的类(class)C不引用m:c=nilObjectSpace.each_object{|obj|c=objif(Class===objandobj.name=~/::C$/)}当然这取决于

  6. ruby - 使用 ruby​​ 和 savon 的 SOAP 服务 - 2

    我正在尝试使用ruby​​和Savon来使用网络服务。测试服务为http://www.webservicex.net/WS/WSDetails.aspx?WSID=9&CATID=2require'rubygems'require'savon'client=Savon::Client.new"http://www.webservicex.net/stockquote.asmx?WSDL"client.get_quotedo|soap|soap.body={:symbol=>"AAPL"}end返回SOAP异常。检查soap信封，在我看来soap请求没有正确的命名空间。任何人都可以建议我

  7. python - 如何使用 Ruby 或 Python 创建一系列高音调和低音调的蜂鸣声？ - 2

    关闭。这个问题是opinion-based.它目前不接受答案。想要改进这个问题？更新问题，以便editingthispost可以用事实和引用来回答它.关闭4年前。Improvethisquestion我想在固定时间创建一系列低音和高音调的哔哔声。例如:在150毫秒时发出高音调的蜂鸣声在151毫秒时发出低音调的蜂鸣声200毫秒时发出低音调的蜂鸣声250毫秒的高音调蜂鸣声有没有办法在Ruby或Python中做到这一点？我真的不在乎输出编码是什么(.wav、.mp3、.ogg等等)，但我确实想创建一个输出文件。

  8. ruby-on-rails - 按天对 Mongoid 对象进行分组 - 2

    在控制台中反复尝试之后，我想到了这种方法，可以按发生日期对类似activerecord的(Mongoid)对象进行分组。我不确定这是完成此任务的最佳方法，但它确实有效。有没有人有更好的建议，或者这是一个很好的方法？#eventsisanarrayofactiverecord-likeobjectsthatincludeatimeattributeevents.map{|event|#converteventsarrayintoanarrayofhasheswiththedayofthemonthandtheevent{:number=>event.time.day,:event=>ev

  9. ruby-on-rails - 'compass watch' 是如何工作的/它是如何与 rails 一起使用的 - 2

    我在我的项目目录中完成了compasscreate.和compassinitrails。几个问题:我已将我的.sass文件放在public/stylesheets中。这是放置它们的正确位置吗？当我运行compasswatch时，它不会自动编译这些.sass文件。我必须手动指定文件:compasswatchpublic/stylesheets/myfile.sass等。如何让它自动运行？文件ie.css、print.css和screen.css已放在stylesheets/compiled。如何在编译后不让它们重新出现的情况下删除它们？我自己编译的.sass文件编译成compiled/t

  10. ruby - 使用 ruby​​ 将 HTML 转换为纯文本并维护结构/格式 - 2

    我想将html转换为纯文本。不过，我不想只删除标签，我想智能地保留尽可能多的格式。为插入换行符标签，检测段落并格式化它们等。输入非常简单，通常是格式良好的html(不是整个文档，只是一堆内容，通常没有anchor或图像)。我可以将几个正则表达式放在一起，让我达到80%，但我认为可能有一些现有的解决方案更智能。 最佳答案 首先，不要尝试为此使用正则表达式。很有可能你会想出一个脆弱/脆弱的解决方案，它会随着HTML的变化而崩溃，或者很难管理和维护。您可以使用Nokogiri快速解析HTML并提取文本:require'nokogiri'h

随机推荐