pcl::getTranslationAndEulerAngles精度缺失问题

pcl::getTranslationAndEulerAngles的功能是根据仿射矩阵计算x,y,z,roll,pitch,yaw

但发现这种计算的rpy有一定的精度问题，于是进行了实验。

一个是从一个四元数，根据eulerAngles计算rpy，再根据rpy与下述公式计算回四元数，发现误差极小。

    Eigen::Quaternionf q;
    q = Eigen::Quaternionf ( r_matrix );
    std::cout<<"q0 = "<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<std::endl;
    Eigen::Matrix3f rx = q.toRotationMatrix();
    Eigen::Vector3f ea = rx.eulerAngles(2,1,0);
    std::cout<<"rpy1 = "<<ea[2]<<" "<<ea[1]<<" "<<ea[0]<<std::endl;
    
    double cy = cos(double(ea[0]) * 0.5);
    double sy = sin(double(ea[0]) * 0.5);
    double cp = cos(double(ea[1]) * 0.5);
    double sp = sin(double(ea[1]) * 0.5);
    double cr = cos(double(ea[2]) * 0.5);
    double sr = sin(double(ea[2]) * 0.5);
 
    double q_n[4];
    q_n[0] = cy * cp * cr + sy * sp * sr;
    q_n[1] = cy * cp * sr - sy * sp * cr;
    q_n[2] = sy * cp * sr + cy * sp * cr;
    q_n[3] = sy * cp * cr - cy * sp * sr;
    std::cout<<"q_n = "<<q_n[0]<<" "<<q_n[1]<<" "<<q_n[2]<<" "<<q_n[3]<<std::endl;

 基本可以认定全过程无误。

之后根据常见的计算轴角公式，再累乘轴角的方式计算四元数。

    Eigen::Quaterniond q1;
    Eigen::AngleAxisd rollAngle1(double(ea[2]), Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd yawAngle1(double(ea[0]), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle1(double(ea[1]), Eigen::Vector3d::UnitY());
    q1 =  yawAngle1 * pitchAngle1*rollAngle1;
    std::cout<<"q1 = "<<q1.w()<<" "<<q1.x()<<" "<<q1.y()<<" "<<q1.z()<<std::endl;

说明这种方法精度也正常。

但使用pcl::getTranslationAndEulerAngles函数时，发现rpy和q都不精确，但累乘轴角的方式计算四元数的方式没问题，所以pcl::getTranslationAndEulerAngles函数存在一定的精度问题，从输出的rpy上也可以看出p.z()上有较大的误差。

    double x,y,z,roll,pitch,yaw;
    pcl::getTranslationAndEulerAngles(tCorrect_s, x, y, z, roll, pitch, yaw);
    Eigen::AngleAxisd rollAngle(double(roll), Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd yawAngle(double(yaw), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(double(pitch), Eigen::Vector3d::UnitY());
    std::cout<<"rpy2 "<<roll<<" "<<pitch<<" "<<yaw<<std::endl;
    Eigen::Quaterniond q2;
    q2 = yawAngle * pitchAngle*rollAngle;
    std::cout<<"q2 "<<q2.w()<<" "<<q2.x()<<" "<<q2.y()<<" "<<q2.z()<<std::endl;