【Eigen】四元数、欧拉角、四元数、旋转向量的初始化与相互转换

1.CmakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(Eigentest)
SET(CMAKE_BUILD_TYPE Release)


# Check C++11 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
   add_definitions(-DCOMPILEDWITHC11)
   message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
   add_definitions(-DCOMPILEDWITHC0X)
   message(STATUS "Using flag -std=c++0x.")
else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()


find_package(Eigen3 3 REQUIRED)
include_directories(
${EIGEN3_INCLUDE_DIR}
)

add_executable(eigen_test eigenTest.cpp )
target_link_libraries(eigen_test ${EIGEN3_LIBS})

2. 四元数、欧拉角、四元数、旋转向量的初始化与相互转换

/*
 * @Author: [email protected]
 * @Date: 2022-10-12 19:23:30
 * @LastEditors: 
 * @LastEditTime: 2022-10-13 09:16:19
 * @FilePath: /Eigen_test/eigenTest.cpp
 * @Description:
 */
#include 
#include 
#include 
int main(int argc, char const *argv[])
{
    {
        // 一：旋转向量
        // 1.1 初始化
        double alpha = M_PI;
        /* 旋转角为弧度制 右手坐标系*/
        Eigen::AngleAxisd yawAngle(alpha, Eigen::Vector3d::UnitZ());
        Eigen::AngleAxisd rotation_vector(-alpha, Eigen::Vector3d(0, 0, 1));

        /* 可视化*/
        std::cout << rotation_vector.angle() << std::endl;
        std::cout << rotation_vector.axis() << std::endl;
        std::cout << rotation_vector.matrix() << std::endl;

        // 1.2 旋转向量转旋转矩阵
        Eigen::Matrix3d rotation_matrix;
        rotation_matrix = rotation_vector.matrix();

        Eigen::Matrix3d rotation_matrix2;
        rotation_matrix2 = rotation_vector.toRotationMatrix();

        // 1.3 旋转向量转欧拉角
        /****************************************************************
            0，1，2 分别代表 roll(沿x轴旋转) pitch（沿y轴旋转） yall（沿z轴旋转）
            .eulerAngles(0,1,2) 代表四元数是按照 roll* pitch*yall 的顺序乘
            结果eulerAngle 中的顺序也是按照 roll pitch yall 的顺序进行存储
        *****************************************************************/
        Eigen::Vector3d eulerAngle = rotation_vector.matrix().eulerAngles(0, 1, 2);
        // 1.4 旋转向量转四元数
        Eigen::Quaterniond quaternion;
        quaternion = rotation_vector;
    }

    {
        // 二、旋转矩阵
        // 2.1 初始化旋转矩阵
        Eigen::Matrix3d rotation_matrix;
        /*按行展开x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22*/
        rotation_matrix << 1.0, 0., 0., 0., 1., 0., 0., 1.;
        /*可视化*/
        

        // 2.2 旋转矩阵转旋转向量
        Eigen::AngleAxisd rotation_vector(rotation_matrix);
        Eigen::AngleAxisd rotation_vector1;
        rotation_vector1=rotation_matrix;

        Eigen::AngleAxisd rotation_vector2;
        rotation_vector2.fromRotationMatrix(rotation_matrix);

        // 2.3 旋转矩阵转欧拉角
        Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);

        // 2.4 旋转矩阵转四元数
        Eigen::Quaterniond quaternion(rotation_matrix);
        
        Eigen::Quaterniond quaternion2;
        quaternion2=rotation_matrix;
    }

    {
        // 三、 欧拉角
        // 3.1 初始化 
        Eigen::Vector3d eulerAngle(0.1,0.2,0.3);//弧度制
              
        // 3.2 欧拉角转旋转向量
        Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitX()));
        Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
        Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitZ()));
        
        Eigen::AngleAxisd rotation_vector;
        rotation_vector=yawAngle*pitchAngle*rollAngle;
        
        // 3.3  欧拉角转旋转矩阵
        Eigen::Matrix3d rotation_matrix;
        rotation_matrix=yawAngle*pitchAngle*rollAngle;

        // 3.4 欧拉角转四元数
        Eigen::Quaterniond quaternion;
        quaternion=yawAngle*pitchAngle*rollAngle;
    }

    {
        // 四、四元数
        // 4.1 初始化四元数
        /*q.w q.x q.y q.z 的方式初始化*/
        Eigen::Quaterniond q(1,0,0,0);

        // 4.2 四元数转旋转向量
        Eigen::AngleAxisd rotation_vector(q);
        
        Eigen::AngleAxisd rotation_vector2;
        rotation_vector2=q;

        // 4.3 四元数转旋转矩阵
        Eigen::Matrix3d rotation_matrix;
        rotation_matrix=q.matrix();

        Eigen::Matrix3d rotation_matrix2;
        rotation_matrix2=q.toRotationMatrix();

        // 四元数转欧拉角
        Eigen::Vector3d eulerAngle=q.matrix().eulerAngles(0,1,2);
    }

    // 五、齐次转换
    {
        Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
        Eigen::Quaterniond q(1,0,0,0);
        Eigen::Vector3d t(1,2,3);
        T.rotate(q);// 上述四个旋转均可使用
        T.pretranslate(t);
        std::cout<<"齐次欧式变换：\n"<<T.matrix()<<std::endl;

    }


    return 0;
}