特征:将旋转矩阵转换为四元数，然后返回完全不同的矩阵 [英] Eigen: convert rotation matrix to quaternion then back getting completely different matrices

问题描述

谁能帮我解决 Eigen 问题?我试图将四元数转换为矩阵，然后返回并得到完全不同的矩阵.在理解这个问题之前，我无法相信四元数.代码如下:

Can anyone help me out with Eigen? I tried to convert quaternion to matrix and then back and got completely different matrices. I am not able to trust quaternion before understanding this issue. Here is the code:

#include <Eigen/Geometry>
#include <iostream>

void Print_Quaternion(Eigen::Quaterniond &q){
    std::cout<<"["<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<"]"<<std::endl;
}
void Verify_Orthogonal_Matrix(Eigen::Matrix3d &m)
{
    std::cout<<"|c0|="<<m.col(0).norm()<<",|c1|="<<m.col(1).norm()<<",|c2|="<<m.col(2).norm()<<std::endl;
    std::cout<<"c0c1="<<m.col(0).dot(m.col(1))<<",c1c2="<<m.col(1).dot(m.col(2))<<",c0c2="<<m.col(0).dot(m.col(2))<<std::endl;
}
int main()
{
    Eigen::Matrix3d m; m<<0.991601,0.102421,-0.078975,0.125398,-0.611876,0.78095,-0.0316631,0.784294,0.619581;

    std::cout<<"Input matrix:"<<std::endl<<m<<std::endl;
    std::cout<<"Verify_Orthogonal_Matrix:"<<std::endl;
    Verify_Orthogonal_Matrix(m);
    std::cout<<"Convert to quaternion q:"<<std::endl;
    Eigen::Quaterniond q(m);
    Print_Quaternion(q);
    std::cout<<"Convert back to rotation matrix m1="<<std::endl;
    Eigen::Matrix3d m1=q.normalized().toRotationMatrix();
    std::cout<<m1<<std::endl;
    std::cout<<"Verify_Orthogonal_Matrix:"<<std::endl;
    Verify_Orthogonal_Matrix(m1);
    std::cout<<"Convert again to quaternion q1="<<std::endl;
    Eigen::Quaterniond q1(m1);
    Print_Quaternion(q1);
}

这是我得到的结果:

Input matrix:
  0.991601   0.102421  -0.078975
  0.125398  -0.611876    0.78095
-0.0316631   0.784294   0.619581
Verify_Orthogonal_Matrix:
|c0|=1,|c1|=1,|c2|=1
c0c1=-4.39978e-07,c1c2=4.00139e-07,c0c2=2.39639e-08
Convert to quaternion q:
[0.706984 0.00118249 -0.0167302 0.00812501]
Convert back to rotation matrix m1=
   0.998617  -0.0230481   -0.047257
  0.0228899     0.99973 -0.00388638
  0.0473339   0.0027993    0.998875
Verify_Orthogonal_Matrix:
|c0|=1,|c1|=1,|c2|=1
c0c1=1.73472e-18,c1c2=-4.33681e-19,c0c2=6.93889e-18
Convert again to quaternion q1=
[0.999653 0.001672 -0.0236559 0.0114885]

我在这里做错了吗?我觉得这应该是一个众所周知的问题，但我卡在这里了.有人可以帮我吗?

Did I do something wrong here? I feel that this should be a well-known problem but I got stuck here. Can someone help me out?

推荐答案

输入矩阵不是旋转矩阵，它包含镜像.它的行列式 == -1，但旋转应该有 +1.检查正交化代码，并查看最后一列的符号

Input matrix is not a rotation matrix, it contains mirroring. Its determinant == -1, but rotation should have +1. check the code for orthogonalization, and look and the signs of last col

m.col(0).normalize();
m.col(1).normalize();
m.col(2) = m.col(0).cross(m.col(1));
m.col(2).normalize();
m.col(0) = m.col(1).cross(m.col(2));
m.col(0).normalize();

std::cout << "orthogonal matrix:" << std::endl << m << std::endl;




Input matrix:
  0.991601   0.102421  -0.078975
  0.125398  -0.611876    0.78095
-0.0316631   0.784294   0.619581
orthogonal matrix:
  0.991601   0.102421   0.078975
  0.125398  -0.611876   -0.78095
-0.0316628   0.784294  -0.619581

这篇关于特征:将旋转矩阵转换为四元数，然后返回完全不同的矩阵的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！