从旋转矩阵计算欧拉角

旋转矩阵和欧拉角之间的正向转换关系比较好推理，而逆向变换就显得不是那么容易了。这篇博客介绍由旋转矩阵计算欧拉角的方法，参考了一篇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;
}

文章来源：公众号：机器人视觉