web-dev-qa-db-ja.com

オイラーからクォータニオン/エイゲンを使用したクォータニオンからオイラー

オイラー角をクォータニオンに変換して「YXZ」に戻すことができる機能を実装しようとしています。これはEigenを使用した従来の方法です。後でこれを使用して、ユーザーがオイラー角を与え、クォータニオンとして回転し、ユーザーのために元に戻すことができるようにする必要があります。実際、私は数学が本当に苦手ですが、最善を尽くしました。この行列が正しいかどうかはわかりません。コードは機能しますが、私の結果はうまくいかないでしょう。私が間違った方向に進むところはありますか？これは私の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はオフになっています。

c++eigenquaternionseuler-angles
5
Little-God

オイラーからクォータニオンへ：

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 から取得

20
Ross

これが1つのアプローチです（テストされていません）：

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];
2
Shital Shah

使うとき

自動オイラー= q.toRotationMatrix（）。eulerAngles（0、1、2）

常に完全に機能するわけではなく、オイラー角は常に一定のビートを持っています（実際の値と計算された値の偏差は±πです）。たとえば、rqtpicture 。でヨー角を読み取って表示します。

これについてはわかりませんが、ros tf :: getYaw（） も「QuaterniontoEuler」を達成できることがわかりました（ヨー角が必要なため）。

0
JKTesla