slam学习——四元素和旋转矩阵的一些知识

slam学习——四元素和旋转矩阵的一些知识

这个是知识星球里面的第二题

已知相机的位姿用四元数表示为q=[0.35,0.2,0.3,0.1],顺序为x,y,z,w，请编程实现：

输出四元数对应的旋转矩阵、旋转矩阵的转置，旋转矩阵的逆矩阵，旋转矩阵乘以自身的转置，验证旋转矩阵的正交性。

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

int main(){
    Eigen::Quaterniond quat1 = Eigen::Quaterniond(0.1,0.35,0.2,0.3);
    quat1.normalize();
    Eigen::Matrix3d rotation_matrix = quat1.matrix();
    std::cout << "旋转矩阵：r \n" << rotation_matrix << std::endl;
    std::cout << "旋转矩阵的转置： rt \n" << rotation_matrix.transpose()<<std::endl;
    std::cout << "旋转矩阵的逆 r.inv \n" << rotation_matrix.inverse() << std::endl;
    std::cout << "旋转矩阵乘旋转矩阵的转置: r*rt \n" << rotation_matrix*rotation_matrix.transpose()<<std::endl;
}

最终的结果如下：