Eigen3 AngleAxis::angle()を使いこなす！ラジアンから度への変換と実践プログラミング

Eigen::AngleAxis::angle() const の説明

• 使用例
  
• const 修飾子
  このメソッドはオブジェクトの状態を変更しないことを示します。つまり、AngleAxis オブジェクトの値を変更せずに角度を取得できます。
• 返り値
  Scalar 型（通常は float または double）の回転角度。
• 機能
  Eigen::AngleAxis オブジェクトが表す回転の角度（ラジアン単位）を返します。

#include <iostream>
#include <Eigen/Geometry>

int main() {
  Eigen::Vector3d axis(1, 0, 0); // 回転軸 (x軸)
  double angle = M_PI / 2;       // 回転角度 (90度)

  Eigen::AngleAxisd angleAxis(angle, axis);

  double retrievedAngle = angleAxis.angle();

  std::cout << "回転角度: " << retrievedAngle << " ラジアン" << std::endl; //出力 回転角度: 1.5708 ラジアン
  return 0;
}

Eigen::AngleAxis::angle() const は、Eigen3ライブラリのEigen::AngleAxisクラスのメンバ関数であり、AngleAxisオブジェクトが表現する3次元空間での回転の角度をラジアン単位で取得するために使用されます。この関数は、オブジェクトの状態を変更せずに、回転角度の値のみを返します。

具体的には、AngleAxisオブジェクトは回転軸と回転角度を保持しており、angle()関数を呼び出すことで、その回転角度をdouble型（またはfloat型）の値として取得できます。この値は、回転の大きさを表し、ラジアンで表現されます。

例えば、あるオブジェクトがx軸周りに90度の回転を表している場合、angle()関数を呼び出すと、返り値はπ/2（約1.5708）ラジアンとなります。

この関数は、回転の角度を直接的に取得したい場合に便利です。例えば、回転の大きさを評価したり、他の回転表現との変換を行う際に利用できます。

---

1. Eigen::AngleAxis オブジェクトが初期化されていない
   
   • エラー
     未初期化の AngleAxis オブジェクトに対して angle() を呼び出すと、予期しない値やクラッシュが発生する可能性があります。
   • トラブルシューティング
     AngleAxis オブジェクトを使用する前に、必ず適切な回転軸と角度で初期化してください。
     • 例: Eigen::AngleAxisd angleAxis(angle, axis);
2. 回転軸が正規化されていない
   
   • エラー
     AngleAxis オブジェクトを作成する際に、回転軸ベクトルが正規化されていないと、回転の表現が正しくなくなる可能性があります。特に、angle()関数で取得される角度が期待される値と異なる場合があります。
   • トラブルシューティング
     回転軸ベクトルを正規化してから AngleAxis オブジェクトを作成してください。
     • 例: axis.normalize(); Eigen::AngleAxisd angleAxis(angle, axis);
3. 角度の単位の誤解
   
   • エラー
     angle() 関数は回転角度をラジアンで返します。度数で角度を扱っている場合、値が期待と異なることがあります。
   • トラブルシューティング
     角度を度数で扱っている場合は、ラジアンに変換してから AngleAxis オブジェクトを作成し、angle() の返り値を度数に変換して使用してください。
     • 例:

       #include <cmath>
       double degree = 90.0;
       double radian = degree * M_PI / 180.0; // 度からラジアンへ変換
       Eigen::AngleAxisd angleAxis(radian, axis);
       double retrievedRadian = angleAxis.angle();
       double retrievedDegree = retrievedRadian * 180.0 / M_PI; // ラジアンから度へ変換
       

4. AngleAxis オブジェクトの意図しない変更
   
   • エラー
     AngleAxis オブジェクトの値を意図せず変更してしまうと、その後の angle() の結果が予期しないものになることがあります。
   • トラブルシューティング
     AngleAxis オブジェクトを操作する際には、その変更が意図したものであることを確認し、必要に応じてコピーを作成して操作してください。
5. コンパイルエラー
   
   • エラー
     Eigen3ライブラリが正しくインストールされていない、またはコンパイルオプションが正しく設定されていない場合、コンパイルエラーが発生する可能性があります。
   • トラブルシューティング
     Eigen3ライブラリのインストールとコンパイルオプションを確認してください。CMakeを使用している場合、find_package(Eigen3 REQUIRED) が正しく機能しているか確認してください。
6. 浮動小数点数の精度
   
   • エラー
     浮動小数点数の計算では、精度誤差が発生する可能性があります。特に、複雑な回転や連続的な回転を扱う場合、誤差が累積することがあります。
   • トラブルシューティング
     誤差を考慮して、許容範囲内で値を比較してください。必要に応じて、より高い精度（double など）を使用してください。
7. 回転表現の変換
   
   • エラー
     AngleAxis から他の回転表現（クォータニオン、回転行列など）に変換する際に、変換式を間違えると、angle() の結果が期待と異なることがあります。
   • トラブルシューティング
     変換式を正しく理解し、Eigen3ライブラリの提供する変換関数を使用してください。例えば、Eigen::Quaterniond や Eigen::Matrix3d への変換があります。
8. デバッグ
   
   • トラブルシューティング
     値が期待と異なる場合、デバッガを使用して AngleAxis オブジェクトの内部状態を確認してください。回転軸と角度の値が正しく設定されているか確認してください。

---

例1: 基本的な角度取得

この例では、AngleAxis オブジェクトを作成し、angle() メソッドを使用して回転角度を取得します。

#include <iostream>
#include <Eigen/Geometry>

int main() {
  Eigen::Vector3d axis(0, 0, 1); // Z軸周りの回転
  double angle = M_PI / 4;       // 45度の回転 (ラジアン)

  Eigen::AngleAxisd angleAxis(angle, axis);

  double retrievedAngle = angleAxis.angle();

  std::cout << "回転角度: " << retrievedAngle << " ラジアン" << std::endl; //出力 回転角度: 0.785398 ラジアン
  return 0;
}

例2: 角度を度数に変換して表示

この例では、angle() で取得したラジアン単位の角度を度数に変換して表示します。

#include <iostream>
#include <Eigen/Geometry>
#include <cmath>

int main() {
  Eigen::Vector3d axis(1, 1, 0);
  double angle = M_PI / 3; // 60度の回転

  Eigen::AngleAxisd angleAxis(angle, axis);

  double retrievedAngleRad = angleAxis.angle();
  double retrievedAngleDeg = retrievedAngleRad * 180.0 / M_PI; // ラジアンから度へ変換

  std::cout << "回転角度: " << retrievedAngleDeg << " 度" << std::endl; //出力 回転角度: 60 度
  return 0;
}

例3: 回転軸を正規化してから AngleAxis を作成し、角度を取得

この例では、回転軸ベクトルを正規化してから AngleAxis オブジェクトを作成し、angle() を使用して角度を取得します。

#include <iostream>
#include <Eigen/Geometry>

int main() {
  Eigen::Vector3d axis(1, 2, 3);
  axis.normalize(); // 回転軸を正規化
  double angle = M_PI / 6; // 30度の回転

  Eigen::AngleAxisd angleAxis(angle, axis);

  double retrievedAngle = angleAxis.angle();

  std::cout << "回転角度: " << retrievedAngle << " ラジアン" << std::endl; //出力 回転角度: 0.523599 ラジアン
  return 0;
}

例4: AngleAxis からクォータニオンに変換し、角度を比較

この例では、AngleAxis オブジェクトをクォータニオンに変換し、クォータニオンから角度を計算して AngleAxis の角度と比較します。

#include <iostream>
#include <Eigen/Geometry>
#include <cmath>

int main() {
  Eigen::Vector3d axis(0, 1, 0);
  double angle = M_PI / 2; // 90度の回転

  Eigen::AngleAxisd angleAxis(angle, axis);
  Eigen::Quaterniond quaternion(angleAxis); // AngleAxisからクォータニオンへ変換

  double angleFromQuaternion = 2.0 * std::acos(quaternion.w()); // クォータニオンから角度を計算

  std::cout << "AngleAxisの角度: " << angleAxis.angle() << " ラジアン" << std::endl;
  std::cout << "クォータニオンから計算した角度: " << angleFromQuaternion << " ラジアン" << std::endl; //両方の値が同じになる。
  return 0;
}

#include <iostream>
#include <Eigen/Geometry>
#include <vector>

int main() {
  std::vector<Eigen::AngleAxisd> angleAxisList;
  angleAxisList.push_back(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(1, 0, 0)));
  angleAxisList.push_back(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 1, 0)));
  angleAxisList.push_back(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1)));

  for (const auto& aa : angleAxisList) {
    std::cout << "回転角度: " << aa.angle() << " ラジアン" << std::endl;
  }
  return 0;
}

---

クォータニオンからの角度計算

AngleAxis オブジェクトをクォータニオンに変換し、クォータニオンから角度を計算する方法です。

#include <iostream>
#include <Eigen/Geometry>
#include <cmath>

int main() {
  Eigen::AngleAxisd angleAxis(M_PI / 3, Eigen::Vector3d(1, 0, 0));
  Eigen::Quaterniond quaternion(angleAxis);

  double angleFromQuaternion = 2.0 * std::acos(quaternion.w()); // クォータニオンから角度を計算

  std::cout << "AngleAxisの角度: " << angleAxis.angle() << " ラジアン" << std::endl;
  std::cout << "クォータニオンから計算した角度: " << angleFromQuaternion << " ラジアン" << std::endl;
  return 0;
}

• 注意点
  quaternion.w() はクォータニオンの実数部であり、acos() の結果を2倍にする必要があります。
• 利点
  クォータニオンは回転を効率的に表現でき、AngleAxis との相互変換が容易です。クォータニオンを使用する他の処理と組み合わせる場合に便利です。

回転行列からの角度計算

AngleAxis オブジェクトを回転行列に変換し、回転行列から角度を計算する方法です。

#include <iostream>
#include <Eigen/Geometry>
#include <cmath>

int main() {
  Eigen::AngleAxisd angleAxis(M_PI / 4, Eigen::Vector3d(0, 1, 0));
  Eigen::Matrix3d rotationMatrix = angleAxis.toRotationMatrix();

  double angleFromRotationMatrix = std::acos((rotationMatrix.trace() - 1.0) / 2.0); // 回転行列から角度を計算

  std::cout << "AngleAxisの角度: " << angleAxis.angle() << " ラジアン" << std::endl;
  std::cout << "回転行列から計算した角度: " << angleFromRotationMatrix << " ラジアン" << std::endl;
  return 0;
}

• 注意点
  回転行列のトレース（対角成分の和）を使用して角度を計算します。精度誤差に注意が必要です。また、回転角度が180度に近い場合、計算が不安定になることがあります。
• 利点
  回転行列は幾何学的な回転を直接的に表現します。回転行列を使用する他の処理と組み合わせる場合に便利です。

回転軸と回転角度を別々に保持

AngleAxis オブジェクトを使用せずに、回転軸と回転角度を別々の変数として保持する方法です。

#include <iostream>
#include <Eigen/Geometry>

int main() {
  Eigen::Vector3d axis(0, 0, 1);
  double angle = M_PI / 6;

  // Eigen::AngleAxisd angleAxis(angle, axis); // AngleAxisを使わない

  std::cout << "回転角度: " << angle << " ラジアン" << std::endl;
  return 0;
}

• 注意点
  回転に関する他の操作（クォータニオンや回転行列への変換など）を自分で実装する必要があります。
• 利点
  AngleAxis オブジェクトのオーバーヘッドを避け、単純な回転を扱う場合に効率的です。

回転軸ベクトルのノルムの確認

回転軸のノルムが1であることを確認します。

#include <iostream>
#include <Eigen/Geometry>

int main(){
    Eigen::Vector3d axis(0,1,0);
    double angle = M_PI/2;
    Eigen::AngleAxisd angleAxis(angle, axis);
    std::cout << "回転軸のノルム: " << angleAxis.axis().norm() << std::endl;
    return 0;
}

• 注意点
  必ずしもエラーを検知できるわけではありません。
• 利点
  回転軸の正規化を確認することで、計算誤差の確認や、予期しない回転が起きていないかを確認することができます。