回転行列⇔角度の変換 #C++

カメラキャリブレーションにおいて外部パラメータである回転行列(Rotation Matrix)を扱う時，
3 * 3の行列で表したい時と，各軸(x軸，y軸，z軸)に対する角度で表したい時がある．

この時の相互変換行列のメモ．間違っていても責めないでください．
（回転行列⇒角度の場合は一意に求まらない場合があるので，多少の注意は必要．）

ただし，今回は右手座標系において，各軸に対する変換は以下のように定めており，
回転行列R = Rz * Rx * Ryとする．（掛け合わせる順序が変わると式が変わる．）
x, y, zは各軸の回転角（ラジアン）とする．

x軸周りの回転行列

Rx = ( 1, 0, 0,
     0, cos(x), -sin(x),
     0, sin(x), cos(x) )

y軸周りの回転行列

Ry = ( cos(y), 0, sin(y),
     0, 1, 0,
     -sin(y), 0, cos(y) )

z軸周りの回転行列

Rz = ( cos(z), -sin(z), 0,
     sin(z), cos(z), 0,
     0, 0, 1 )

回転行列（Rz * Rx * Ry）

R = cos(y)cos(z) - sin(x)sin(y)sin(z), -cos(x)sin(z), sin(y)cos(z) + sin(x)cos(y)sin(z),
      cos(y)sin(z) + sin(x)sin(y)cos(z), cos(x)cos(z), sin(z)sin(y) - sin(z)cos(y)cos(z),
      -cos(x)sin(y), sin(x), cos(x)cos(y)

なので，
角度⇒回転行列

void computeMatrixFromAngles(
     cv::Mat & R,
     double x,
     double y,
     double z){

  R.row(0).col(0) = cos(y)*cos(z) - sin(x)*sin(y)*sin(z);
  R.row(0).col(1) = -cos(x)*sin(z);
  R.row(0).col(2) = sin(y)*cos(z) + sin(x)*cos(y)*sin(z);
  R.row(1).col(0) = cos(y)*sin(z) + sin(x)*sin(y)*cos(z);
  R.row(1).col(1) = cos(x)*cos(z);
  R.row(1).col(2) = sin(y)*sin(z) - sin(x)*cos(y)*cos(z);
  R.row(2).col(0) = - cos(x)*sin(y);
  R.row(2).col(1) = sin(x);
  R.row(2).col(2) = cos(x)*cos(y);
}

回転行列⇒角度

void computeAnglesFromMatrix(
                 cv::Mat R,
                 double & angle_x,
                 double & angle_y,
                 double & angle_z
                 ){

  double threshold = 0.001;

  if(abs(R.at<double>(2,1) - 1.0) < threshold){ // R(2,1) = sin(x) = 1の時
    angle_x = PI / 2;
    angle_y = 0;
    angle_z = atan2(R.at<double>(1,0), R.at<double>(0,0));
  }else if(abs(R.at<double>(2,1) + 1.0) < threshold){ // R(2,1) = sin(x) = -1の時
    angle_x = - PI / 2;
    angle_y = 0;
    angle_z = atan2(R.at<double>(1,0), R.at<double>(0,0));
  }else{
    angle_x = asin(R.at<double>(2,1));
    angle_y = atan2(-R.at<double>(2,0), R.at<double>(2,2));
    angle_z = atan2(-R.at<double>(0,1), R.at<double>(1,1));
  }
}

参考サイト
+ http://d.hatena.ne.jp/It_lives_vainly/20070829/1188384519
（↑のサイトは回転行列の表現がちょっと間違ってる？）