从旋转矩阵计算欧拉角

旋转矩阵和欧拉角之间的正向转换关系比较好推理，而逆向变换就显得不是那么容易了。这篇博客介绍由旋转矩阵计算欧拉角的方法，参考了一篇Paper：Computing Euler angles from a rotation matrix。Paper本身介绍的还是比较清楚的，这篇博客最后附了转换计算的代码，包括文章作者提供的Matlab版本和博主提供的C/C++版本，希望能有用。

计算过程

Matlab实现代码

虽然是Matlab的代码，不过也可以很轻松的转化成其它语言代码，其中主要使用了反三角函数相关运算

function eulerAngles = rotationMatrix2eulerAngles(R)

% eulerAngles = rotationMatrix2eulerAngles(R)
%
% This function returns the rotation angles in degrees about the x, y and z axis for a
% given rotation matrix
% 
% Copyright : This code is written by david zhao from SCUT,1257650237@qq,com. The code
%              may be used, modified and distributed for research purposes with
%              acknowledgement of the author and inclusion this copyright information.
%
% Disclaimer : This code is provided as is without any warrantly.

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<float>>就可以，完全不影响。

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;
}

OK，See You Next Chaper!