旋转矩阵和欧拉角之间的正向转换关系比较好推理,而逆向变换就显得不是那么容易了。这篇博客介绍由旋转矩阵计算欧拉角的方法,参考了一篇Paper:Computing Euler angles from a rotation matrix。Paper本身介绍的还是比较清楚的,这篇博客最后附了转换计算的代码,包括文章作者提供的Matlab版本和博主提供的C/C++版本,希望能有用。
Matlab实现代码
虽然是Matlab的代码,不过也可以很轻松的转化成其它语言代码,其中主要使用了反三角函数相关运算
function eulerAngles = rotationMatrix2eulerAngles(R)
if abs(R(3,1)) ~= 1
theta1 = -asin(R(3,1));
theta2 = pi - theta1;
psi1 = atan2(R(3,2)/cos(theta1), R(3,3)/cos(theta1));
psi2 = atan2(R(3,2)/cos(theta2), R(3,3)/cos(theta2));
pfi1 = atan2(R(2,1)/cos(theta1), R(1,1)/cos(theta1));
pfi2 = atan2(R(2,1)/cos(theta2), R(1,1)/cos(theta2));
theta = theta1; % could be any one of the two
psi = psi1;
pfi = pfi1;
else
phi = 0;
delta = atan2(R(1,2), R(1,3));
if R(3,1) == -1
theta = pi/2;
psi = phi + delta;
else
theta = -pi/2;
psi = -phi + delta;
end
end
%psi is along x-axis...........theta is along y-axis........pfi is along z
%axis
% eulerAngles = [psi theta pfi]; %for rad;
eulerAngles = [psi*180/pi theta*180/pi pfi*180/pi]; %for degree;
C++代码实现
需要math.h,矩阵R的类型,博主用的Eigen库,如果你不用这个直接换成二维数组vector<vector>就可以,完全不影响。
std::vector<float> computeEularAngles(Eigen::Matrix4f& R, bool israd){
std::vector<float> result(3, 0);
const float pi = 3.14159265397932384626433;
float theta = 0, psi = 0, pfi = 0;
if (abs(R(2, 0)) < 1 - FLT_MIN || abs(R(2, 0)) > 1 + FLT_MIN){ // abs(R(2, 0)) != 1
float theta1 = -asin(R(2, 0));
float theta2 = pi - theta1;
float psi1 = atan2(R(2,1)/cos(theta1), R(2,2)/cos(theta1));
float psi2 = atan2(R(2,0)/cos(theta2), R(2,2)/cos(theta2));
float pfi1 = atan2(R(1,0)/cos(theta1), R(0,0)/cos(theta1));
float pfi2 = atan2(R(1,0)/cos(theta2), R(0,0)/cos(theta2));
theta = theta1;
psi = psi1;
pfi = pfi1;
} else{
float phi = 0;
float delta = atan2(R(0,1), R(0,2));
if (R(2, 0) > -1 - FLT_MIN && R(2, 0) < -1 + FLT_MIN){ // R(2,0) == -1
theta = pi / 2;
psi = phi + delta;
} else{
theta = -pi / 2;
psi = -phi + delta;
}
}
// psi is along x-axis, theta is along y-axis, pfi is along z axis
if (israd){ // for rad
result[0] = psi;
result[1] = theta;
result[2] = pfi;
} else{
result[0] = psi * 180 / pi;
result[1] = theta * 180 / pi;
result[2] = pfi * 180 / pi;
}
return result;
}
文章来源:公众号:机器人视觉
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)