四元数、变换矩阵、欧拉角转换关系

最新推荐文章于 2024-04-22 20:45:04 发布

代码多少钱一两

最新推荐文章于 2024-04-22 20:45:04 发布

阅读量378

点赞数

CC 4.0 BY-SA版权

分类专栏： slam 文章标签：矩阵线性代数

本文链接：https://blog.youkuaiyun.com/handily_1/article/details/128023330

四元数to变换矩阵

   	Eigen::Quaterniond quaternion(w, x, y, z);
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix = quaternion.matrix();
    transformation.rotate(rotation_matrix);
    transformation.pretranslate(Eigen::Vector3d(tx, ty, tz));
    cout << transformation.matrix() << endl;

变换矩阵to四元数

transfor为已知的变换矩阵，从icp配准得到的就是

//获取变换矩阵，求四元数q和位移t_vector
    Eigen::Matrix4f transfor;
    transfor = icp.getFinalTransformation();
    Eigen::Matrix3f r_matrix = Eigen::Matrix3f::Identity();
    for(int i=0;i<3;i++)
    {
   
   
	for(int j=0;j<3;j++)
	{
   
   
	    r_matrix(i,j) = transfor(i,j);
	}
    }
    Eigen::Vector3f t_vector(transfor(0,3),transfor(1,3),transfor(2,3));
    Eigen::Quaternionf q;
    q = Eigen::Quaternionf ( r_matrix );

欧拉角转换to旋转矩阵

参考：https://zhuanlan.zhihu.com/p/144032401
https://blog.youkuaiyun.com/shyjhyp11/article/details/111701127
Eigen库:

    Eigen::Vector3d eulerAngle(1.0, 2.0, 3.0);//欧拉角
    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()))