cvl-robot's diary

研究ノート　メモメモ https://github.com/dotchang/

姿勢の計算

（メモ）
 
回転行列とは、座標軸のそれぞれの方向を表す単位ベクトル、そのものである。
x y z　：　成分
------------
1 0 0　：　x軸の方向ベクトル
0 1 0　：　y軸の方向ベクトル
0 0 1　：　z軸の方向ベクトル
の座標系をどのような方向に回転させたいか、を表したものが回転。
回転後の座標軸の方向を単位ベクトルで置けばよい。
任意の方向を取っていいわけではなく、それぞれ直交している。
なので、ロールピッチヨーとかクオータニオンとかを使って回転を考えるよりも、同次行列で表現しておいた方が圧倒的に理解が早い。
 
 
 
１．姿勢（回転）のオリジナルは3×3の行列で表される
２．実用上は常に、回転と併進の4ｘ4の同次行列で扱う
３．オイラー角も、クオータニオンも、計算上の都合の便宜上、回転行列の表現方法を変えたものと捉える
４．姿勢の計算は、多くの場合クオータニオンで行う
５．オイラー角とロールピッチヨーの定義に混乱しない
12種類ある．どのオイラー角か明記しないと混乱する．
XYX，XZX，YXY，YZY，ZXZ，ZYZ
XYZ，XZY，YXZ，YZX，ZXY，ZYX
 
ロールピッチヨー
RotZ(yaw)RotY(pitch)RotX(roll)
 
６．回転行列ークオータニオンの変換
 
７．回転行列→オイラー角（取り扱い注意）
 
８．openframeworksでの実装は？
ofMatrix4x4クラスとofQuaternionクラスを使う
rotate(), setRotate()など
 
 
//  ------------------------------------------------------------
//  各軸の正規化されたベクトルを定義
ofVec3f Znormal(0,0,1);
ofVec3f Xnormal(1,0,0);
ofVec3f Ynormal(0,1,0);
 
//  各x,y,z軸毎の回転をquaternionとして作成
ofQuaternion qr (roll, Znormal);
ofQuaternion qp (pitch, Xnormal);
ofQuaternion qy (yaw, Ynormal);
ofQuaternion qt;    //  total quaternion
     
qt = qr * qp * qy;
 
b.クオータニオンからオイラー角（ＲＰＹではない）Y→Z→X
ofQuaternion::getEuler()
標準の物は使い物にならない
 
c.下記のクラスを使って行列→オイラー角
 
 
d.QuaternionからMatrix
void transformQuaternionToRotMat(ofMatrix4x4 & m, ofQuaternion q)
{
m(0,0) = 1.0f - 2.0f * q.y() * q.y() - 2.0f * q.z() * q.z();
m(0,1) = 2.0f * q.x() * q.y() + 2.0f * q.w() * q.z();
m(0,2) = 2.0f * q.x() * q.z() - 2.0f * q.w() * q.y();

m(1,0) = 2.0f * q.x() * q.y() - 2.0f * q.w() * q.z();
m(1,1) = 1.0f - 2.0f * q.x() * q.x() - 2.0f * q.z() * q.z();
m(1,2) = 2.0f * q.y() * q.z() + 2.0f * q.w() * q.x();

m(2,0) = 2.0f * q.x() * q.z() + 2.0f * q.w() * q.y();
m(2,1) = 2.0f * q.y() * q.z() - 2.0f * q.w() * q.x();
m(2,2) = 1.0f - 2.0f * q.x() * q.x() - 2.0f * q.y() * q.y();
}