利用SolvePNP解出旋转向量,旋转向量通过罗德里格斯公式解出旋转矩阵,然后通过下面六种公式计算即可,欧拉角有十二种,六种是相对于自身参考系,六种是相对于惯性参考系,这里是相对于惯性参考系的
Point3f RecToEulerXZY(Mat &R){
double Z = asin(-1R.at(1,2));
double X = asin(R.at(3,2) / cos(Z));
double Y = asin(R.at(1,3) / cos(Z));
return Point3f(X/PI180,Y/PI180,Z/PI180);
}
Point3f RecToEulerXYZ(Mat &R){
double Y = asin(1R.at(1,3));
double X = acos(R.at(3,3) / cos(Y));
double Z = asin(R.at(1,2) / (-1cos(Y)));
return Point3f(X/PI180,Y/PI180,Z/PI180);
}
Point3f RecToEulerYXZ(Mat &R){
double X = asin(-1R.at(2,3));
double Z = acos(R.at(2,2) / cos(X));
double Y = asin(R.at(3,1) / (1cos(X)));
return Point3f(X/PI180,Y/PI180,Z/PI180);
}
Point3f RecToEulerYZX(Mat &R){
double Z = asin(1R.at(2,1));
double X = acos(R.at(2,2) / cos(Z));
double Y = asin(R.at(2,3) / (-1cos(X)));
return Point3f(X/PI180,Y/PI180,Z/PI180);
}
Point3f RecToEulerZYX(Mat &R){
double Y = asin(-1R.at(3,1));
double X = acos(R.at(3,3) / cos(Y));
double Z = acos(R.at(1,1) / (1cos(Y)));
return Point3f(X/PI180,Y/PI180,Z/PI180);
}
Point3f RecToEulerZXY(Mat &R){
double X = asin(1R.at(3,2));
double Y = acos(R.at(3,3) / cos(X));
double Z = asin(R.at(1,2) / (-1cos(X)));
return Point3f(X/PI180,Y/PI180,Z/PI*180);
}
从旋转向量到欧拉角的六种计算方法
于 2022-07-30 09:29:42 首次发布