Eigen中四元数Quaterniond的初始

Quaterniond的初始

Eigen::Quaterniond q1(w, x, y, z);// 第一种方式

Eigen::Quaterniond q2(Vector4d(x, y, z, w));// 第二种方式

Eigen::Quaterniond q2(Matrix3d(R));// 第三种方式

以上两种方式是不同的，在Quaternion内部的保存中，虚部在前，实部在后，

如果以第一种方式构造四元数，则实部是w， 虚部是[x,y,z]；

对于第二种方式，则实部是w，虚部是[x,y,z]；

对于第三种方式，则是用3x3的旋转矩阵初始化四元数。

template<typename _Scalar , int _Options>
template<typename Derived >
Eigen::Quaternion< _Scalar, _Options >::Quaternion	(	const MatrixBase< Derived > & 	other	)	

Constructs and initializes a quaternion from either:

• a rotation matrix expression,
• a 4D vector expression representing quaternion coefficients.

四元数、旋转矩阵、旋转向量、欧拉角相互转化：

• ref:ceres-solver/include/ceres/rotation.h

 

#include <iostream>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#define PI (3.1415926535897932346f)
 
int main(int argc, char **argv) 
{
    using ::std::cout;
    using ::std::endl;
    double yaw = PI/3,pitching = PI/4,droll = PI/6;
    
    
      //EulerAngles to RotationMatrix
    ::Eigen::Vector3d ea0(yaw,pitching,droll);
    ::Eigen::Matrix3d R;
    R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())
        * ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())
        * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());
 
    cout << R << endl << endl;
 
    //RotationMatrix to Quaterniond
    ::Eigen::Quaterniond q; 
    q = R;    
    cout << q.x() << endl << endl;
    cout << q.y() << endl << endl;
    cout << q.z() << endl << endl;
    cout << q.w() << endl << endl;
    
    //Quaterniond to RotationMatrix
    ::Eigen::Matrix3d Rx = q.toRotationMatrix();
    cout << Rx << endl << endl;
    
    
    //RotationMatrix to EulerAngles
    ::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);     
    cout << ea1/PI*180 << endl << endl;
    
    
    std::cin.ignore();
    return 0;
}