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;