読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

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

姿勢の計算

(メモ)
 
回転行列とは、座標軸のそれぞれの方向を表す単位ベクトル、そのものである。
x y z : 成分
------------
1 0 0 : x軸の方向ベクトル
0 1 0 : y軸の方向ベクトル
0 0 1 : z軸の方向ベクトル
の座標系をどのような方向に回転させたいか、を表したものが回転。
回転後の座標軸の方向を単位ベクトルで置けばよい。
任意の方向を取っていいわけではなく、それぞれ直交している。
なので、ロールピッチヨーとかクオータニオンとかを使って回転を考えるよりも、同次行列で表現しておいた方が圧倒的に理解が早い。
 
 
 
1.姿勢(回転)のオリジナルは3×3の行列で表される
2.実用上は常に、回転と併進の4x4の同次行列で扱う
3.オイラー角も、クオータニオンも、計算上の都合の便宜上、回転行列の表現方法を変えたものと捉える
4.姿勢の計算は、多くの場合クオータニオンで行う
5.オイラー角とロールピッチヨーの定義に混乱しない
12種類ある.どのオイラー角か明記しないと混乱する.
XYX,XZX,YXY,YZY,ZXZ,ZYZ
XYZ,XZY,YXZ,YZX,ZXY,ZYX
 
ロールピッチヨー
RotZ(yaw)RotY(pitch)RotX(roll)
 
6.回転行列ークオータニオンの変換
 
7.回転行列→オイラー角(取り扱い注意)
 
8.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.クオータニオンからオイラー角(RPYではない)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();
}