.-- --

スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。
スポンサー広告 comment(-) trackback(-)
.24 2013

Eigenで回転行列理解

やってしまいました...
本日寝坊してしまった、Uedaです。
目覚まし時計、もう一個買っておこう..

さて、今回はまたEigenにもどりまして3次元座標系での回転行列を作ってみました。

クォータニオンは次にとっておくとして、今回は各軸回転(yaw, pitch, roll)についてのプログラムを備忘録として残しておくことにします。

pitch → x軸中心の回転
yaw → y軸中心の回転
roll → z軸中心の回転

以下のコードは、
1. ランダムにpitch, yaw, rollの角度を算出
2. 算出した角度で回転行列を作る
3. 作った回転行列から、pitch, yaw, rollを抽出。元データとの確認。
4. 回転行列の行列としての性質として、ある座標を掛けて、掛けた座標を回転行列の転置行列を掛けると元通りになるというものがあります。その検証。

の順で、処理しています。


#include "Eigen/Core"
#include "Eigen/Dense"

...
#ifdef _rotate_matrix_

NSLog(@"回転行列のテスト");

double p = 0.5 - [self random] / 10.0;
double y = 0.5 - [self random] / 10.0;
double r = 0.5 - [self random] / 10.0;

Matrix3d R_pitch;
R_pitch(0, 0) = 1.0; R_pitch(0, 1) = 0.0; R_pitch(0, 2) = 0.0;
R_pitch(1, 0) = 0.0; R_pitch(1, 1) = cos(p); R_pitch(1, 2) = -sin(p);
R_pitch(2, 0) = 0.0; R_pitch(2, 1) = sin(p); R_pitch(2, 2) = cos(p);
Matrix3d R_yaw;
R_yaw(0, 0) = cos(y); R_yaw(0, 1) = 0.0; R_yaw(0, 2) = sin(y);
R_yaw(1, 0) = 0.0; R_yaw(1, 1) = 1.0; R_yaw(1, 2) = 0.0;
R_yaw(2, 0) = -sin(y); R_yaw(2, 1) = 0.0; R_yaw(2, 2) = cos(y);
Matrix3d R_roll;
R_roll(0, 0) = cos(r); R_roll(0, 1) = -sin(r); R_roll(0, 2) = 0.0;
R_roll(1, 0) = sin(r); R_roll(1, 1) = cos(r); R_roll(1, 2) = 0.0;
R_roll(2, 0) = 0.0; R_roll(2, 1) = 0.0; R_roll(2, 2) = 1.0;

MatrixXd R = R_roll * R_pitch * R_yaw;

double r11 = R(0, 0); double r12 = R(0, 1); double r13 = R(0, 2);
double r21 = R(1, 0); double r22 = R(1, 1); double r23 = R(1, 2);
double r31 = R(2, 0); double r32 = R(2, 1); double r33 = R(2, 2);

double yaw, roll, pitch;
double cp = sqrt(r12*r12 + r22*r22);

if (cp == 0.0)
{
pitch = r32/M_PI_2;
pitch = r32*M_PI_2;
yaw = 0.0;
roll = atan2(r21, r11);
}
else
{
pitch = atan2(r32, cp);
yaw = atan2(-r31, r33);
roll = atan2(-r12, r22);
}

NSLog(@"正解 %f, %f, %f", p, r, y);
NSLog(@"計算 %f, %f, %f", pitch, roll, yaw);

MatrixXd data(3, 1);
data(0, 0) = 0.5 - [self random] / 10.0;
data(1, 0) = 0.5 - [self random] / 10.0;
data(2, 0) = 0.5 - [self random] / 10.0;
MatrixXd data2 = R * data;
MatrixXd data3 = R.transpose() * data2;

NSLog(@"写像前 %f, %f, %f", data(0, 0), data(1, 0), data(2, 0));
NSLog(@"写像後 %f, %f, %f", data3(0, 0), data3(1, 0), data3(2, 0));

#endif
...


出力
回転行列のテスト
正解 0.319571, 0.331831, 0.415307
計算 0.319571, 0.331831, 0.415307
写像前 0.328536, 0.304225, 0.457576
写像後 0.328536, 0.304225, 0.457576

すばらしい!ぴったりです♪

参考記事
http://www.japan-iss.co.jp/wp-content/uploads/2010/02/RollPitchYaw.pdf
関連記事
スポンサーサイト

Comment

iriomote
参考になります、ありがとうございます。

僭越ながら、

pitch = r32/M_PI_2;

pitch = r32*M_PI_2;
でないでしょうか?

或いは
pitch = r32/M_2_PI;
2013.12.02 18:44
neoxneo
iriomote 様
> pitch = r32/M_PI_2;
> は
> pitch = r32*M_PI_2;
> でないでしょうか?
>
> 或いは
> pitch = r32/M_2_PI;

すみません!まさしくその通りでした。
修正させていただきます!

なお、EigenにはGeometry moduleというものもありますので、
そちらを利用してみるのも手かもしれませんね。
2013.12.02 19:13

Post comment

  • comment
  • secret
  • 管理者にだけ表示を許可する

Trackback

trackbackURL:http://appteam.blog114.fc2.com/tb.php/250-f6d84d50

ブログ内検索

関連リンク

製品情報

最新記事

カテゴリ

プロフィール

neoxneo



NEXT-SYSTEM iOS Developers Blog


  • UTO:
    カナダ版iPhone4Sは、マナーモードでシャッター音がならない…


  • Ehara:
    ...


  • Hayate:
    ...


  • Tasaki:
    Developer登録完了...したのはいいけど


  • Ueda:
    ...



リンク

上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。