ODE/回転行列から角度を求める - ずくなしのーと

回転行列から角度を求める

回転行列から角度を求めたい時がたまにあります。
例えば、加速センサーとジャイロセンサーをシミュレートしたい場合などです。

いざ計算しようと思った時、さてどうやったっけ？となるのでメモを残します。

回転行列から角度もを求める関数は次の通り。

void computeAnglesFromMatrix(
      const dReal* R,
      dReal & angle_x,
      dReal & angle_y,
      dReal & angle_z
  ){
    double threshold = 0.001;
 
    if(fabs(R[2+1*4] - 1.0) < threshold){ // R(2,1) = sin(x) = 1の時
      angle_x = M_PI / 2;
      angle_y = 0;
      angle_z = atan2(R[1+0*4], R[0+0*4]);
    }else if(fabs(R[2+1*4] + 1.0) < threshold){ // R(2,1) = sin(x) = -1の時
      angle_x = - M_PI / 2;
      angle_y = 0;
      angle_z = atan2(R[1+0*4], R[0+0*4]);
    }else{
      angle_x = asin(R[2+1*4]);
      angle_y = atan2(-R[2+0*4], R[2+2*4]);
      angle_z = atan2(-R[0+1*4], R[1+1*4]);
    }
  }
 

ここで、引数Rは、dBodyGetRotationもしくはdGeomGetRotationから取得した回転行列を想定しています。
ODEの場合、3x3ではなく4x3であるとこに注意です。

上記は、絶対値での角度が求められます。

ただ、加速度センサーやジャイロセンサーを

void get_angle(
      dReal & roll,  // x
      dReal & pitch, // y
      dReal & yaw    // z
  ) {
    dReal angle_x, angle_y, angle_z;
    const dReal *r = dBodyGetRotation(m_frame_box->get_body());
    computeAnglesFromMatrix( r, angle_x, angle_y, angle_z );
 
    roll  = - angle_x * sin( angle_z ) - angle_y * cos( angle_z );
    pitch = - angle_x * cos( angle_z ) + angle_y * sin( angle_z );
    yaw   = angle_z;
 
    roll  *= 180.0 / M_PI;
    pitch *= 180.0 / M_PI;
    yaw   *= 180.0 / M_PI;
  }
 

これで、complementary filterを通した時の値っぽくなる？！かなと思います。
ロー、ピッチ、ヨーの向きやプラスマイナスがどっちとかは、必要に応じで変更必要かもです。

間違っていたら、ごめんなさい。

参考

• http://qiita.com/q_tarou/items/46e5045068742dfb2fa6