草庐IT

c++ - 使用特征值的欧拉到四元数/四元数到欧拉

coder 2024-02-06 原文

我正在尝试实现一项功能，该功能可以使用 Eigen 将欧拉角转换为四元数并返回“YXZ”约定。稍后这应该用于让用户给你欧拉角并作为四元数旋转并为用户转换回。事实上，我的数学很差，但我尽力了。我不知道这个矩阵是否正确或任何东西。代码有效，但我想我的结果很糟糕。知道我在哪里走错了吗？这是我的 Quat.cpp 的样子:

#include "Quat.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>

using namespace Eigen;

Vector3f Quat::MyRotation(const Vector3f YPR)
{
    Matrix3f matYaw(3, 3), matRoll(3, 3), matPitch(3, 3), matRotation(3, 3);
    const auto yaw = YPR[2]*M_PI / 180;
    const auto pitch = YPR[0]*M_PI / 180;
    const auto roll = YPR[1]*M_PI / 180;

    matYaw << cos(yaw), sin(yaw), 0.0f,
        -sin(yaw), cos(yaw), 0.0f,  //z
        0.0f, 0.0f, 1.0f;

    matPitch << cos(pitch), 0.0f, -sin(pitch),
        0.0f, 1.0f, 0.0f,   // X
        sin(pitch), 0.0f, cos(pitch);

    matRoll << 1.0f, 0.0f, 0.0f,
        0.0f, cos(roll), sin(roll),   // Y
        0.0f, -sin(roll), cos(roll);

    matRotation = matYaw*matPitch*matRoll;

    Quaternionf quatFromRot(matRotation);

    quatFromRot.normalize(); //Do i need to do this?

    return Quat::toYawPitchRoll(quatFromRot);
}

Vector3f Quat::toYawPitchRoll(const Eigen::Quaternionf& q)
{
    Vector3f retVector;

    const auto x = q.y();
    const auto y = q.z();
    const auto z = q.x();
    const auto w = q.w();

    retVector[2] = atan2(2.0 * (y * z + w * x), w * w - x * x - y * y + z * z);
    retVector[1] = asin(-2.0 * (x * z - w * y));
    retVector[0] = atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);

#if 1
    retVector[0] = (retVector[0] * (180 / M_PI));
    retVector[1] = (retVector[1] * (180 / M_PI))*-1;
    retVector[2] = retVector[2] * (180 / M_PI);
#endif
    return retVector;
}

输入:x = 55.0，y = 80.0，z = 12.0 四元数:w:0.872274，x:-0.140211，y:0.447012，z:-0.140211 返回值:x:-55.5925，y:-6.84901，z:-21.8771 忽略前缀，X 值似乎是正确的，但 Y 和 z 是关闭的。

最佳答案

从欧拉到四元数:

using namespace Eigen;
//Roll pitch and yaw in Radians
float roll = 1.5707, pitch = 0, yaw = 0.707;    
Quaternionf q;
q = AngleAxisf(roll, Vector3f::UnitX())
    * AngleAxisf(pitch, Vector3f::UnitY())
    * AngleAxisf(yaw, Vector3f::UnitZ());
std::cout << "Quaternion" << std::endl << q.coeffs() << std::endl;

从四元数到欧拉:

auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler << std::endl;

取自https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html

关于c++ - 使用特征值的欧拉到四元数/四元数到欧拉，我们在Stack Overflow上找到一个类似的问题： https://stackoverflow.com/questions/31589901/

欧拉ampretVectorltc++eigenquaternionseuler-angles

有关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 - '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

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

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

  10. ruby - 在 64 位 Snow Leopard 上使用 rvm、postgres 9.0、ruby 1.9.2-p136 安装 pg gem 时出现问题 - 2

    我想为Heroku构建一个Rails3应用程序。他们使用Postgres作为他们的数据库，所以我通过MacPorts安装了postgres9.0。现在我需要一个postgresgem并且共识是出于性能原因你想要pggem。但是我对我得到的错误感到非常困惑当我尝试在rvm下通过geminstall安装pg时。我已经非常明确地指定了所有postgres目录的位置可以找到但仍然无法完成安装:$envARCHFLAGS='-archx86_64'geminstallpg--\--with-pg-config=/opt/local/var/db/postgresql90/defaultdb/po

随机推荐