Edited at

回転行列⇔角度の変換

More than 5 years have passed since last update.

カメラキャリブレーションにおいて外部パラメータである回転行列(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

(↑のサイトは回転行列の表現がちょっと間違ってる?)