Eigen::AngleAxis 演算子* (Quaternion) の使い方と注意点【Eigen3】

この演算子は、Eigen::AngleAxis オブジェクトと Eigen::Quaternion オブジェクトの合成（結合）を行います。具体的には、現在の AngleAxis が表す回転の後に、引数 other の Quaternion が表す回転を適用した結果を表す新しい Quaternion オブジェクトを返します。

以下に、それぞれの要素について詳しく説明します。

• 戻り値: この演算子は、2つの回転を合成した結果を表す新しい Eigen::Quaternion オブジェクトを返します。
• const (関数の末尾): これは、この演算子が AngleAxis オブジェクト自身の状態を変更しないことを意味します。つまり、この演算を呼び出した AngleAxis オブジェクトは、演算後も元の状態を保ちます。
• (const QuaternionType &other): これは、演算子の引数です。
  • const QuaternionType &: Eigen::Quaternion 型の定数参照です。つまり、この関数内で other の値が変更されることはありません。また、参照渡しなので、大きなクォータニオンオブジェクトをコピーする際のオーバーヘッドを避けることができます。QuaternionType は通常 Eigen::Quaternion<Scalar> のような型になります。
  • other: もう一つの回転を表す Quaternion オブジェクトです。
• operator*: これは、乗算演算子をオーバーロードしたものです。この特定のオーバーロードは、AngleAxis オブジェクトと Quaternion オブジェクトの間で定義されています。
• Eigen::AngleAxis: これは、回転を回転軸（単位ベクトル）と回転角で表現するEigenのクラスです。

動作のイメージ

もし angleAxis1 がある回転を表す Eigen::AngleAxis オブジェクトで、quaternion2 が別の回転を表す Eigen::Quaternion オブジェクトである場合、

Eigen::Quaternion quaternion_result = angleAxis1 * quaternion2;

このコードは、まず angleAxis1 が表す回転を適用し、その後に quaternion2 が表す回転を適用した結果の回転を quaternion_result という新しい Eigen::Quaternion オブジェクトに格納します。

重要な点

• 効率性: Eigenライブラリは、これらの演算を効率的に行うように実装されています。
• 戻り値の型: 演算の結果は Quaternion オブジェクトであることに注意してください。AngleAxis オブジェクトと Quaternion オブジェクトを直接合成した結果を AngleAxis として得る演算子は（通常は）提供されていません。必要であれば、結果の Quaternion を Eigen::AngleAxis に変換することができます。
• 回転の順序: 合成された回転は、最初に AngleAxis オブジェクトの回転が適用され、次に Quaternion オブジェクトの回転が適用されるという順序になります。行列の積と同様に、回転の順序は一般的に重要です。

使用例

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

int main() {
  // 角度と軸で回転を定義
  Eigen::AngleAxisd angleAxis1(Eigen::AngleAxisd::Identity());
  angleAxis1.angle() = M_PI / 4.0; // 45度
  angleAxis1.axis() = Eigen::Vector3d::UnitZ(); // Z軸周りの回転

  // クォータニオンで別の回転を定義
  Eigen::Quaterniond quaternion2(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX())); // X軸周りの90度回転

  // AngleAxis と Quaternion を合成
  Eigen::Quaterniond combined_rotation = angleAxis1 * quaternion2;

  std::cout << "AngleAxis 1:\n" << angleAxis1.matrix() << std::endl;
  std::cout << "\nQuaternion 2:\n" << quaternion2.matrix() << std::endl;
  std::cout << "\nCombined Rotation (Quaternion):\n" << combined_rotation.matrix() << std::endl;

  return 0;
}

この例では、Z軸周りの45度の回転を表す AngleAxis と、X軸周りの90度の回転を表す Quaternion を合成し、その結果を Quaternion として出力しています。

---

型の不一致 (Type Mismatch)

• 解決策
  
  • 渡す引数が Eigen::Quaternion 型であることを確認する。
  • テンプレート引数（通常は double や float）が、AngleAxis オブジェクトが内部で使用している型と一致する Quaternion オブジェクトを使用する。必要であれば、.cast<NewScalarType>() などで型を変換する。
• 原因
  
  • Eigen::Quaternion ではなく、例えば Eigen::Matrix3d (回転行列) や Eigen::Vector3d (ベクトル) などを渡している。
  • テンプレート引数（スカラー型）が異なる Quaternion オブジェクトを渡している（例: Eigen::Quaternion<float> に対して Eigen::Quaternion<double> を渡す）。
• エラー
  コンパイラエラーが発生し、演算子の引数の型が期待される const Eigen::QuaternionType& と異なることが指摘される。

回転の順序の誤解 (Misunderstanding the Order of Rotation)

• 解決策
  意図した回転の順序を再確認し、演算子の適用順序や使用する演算子（* や inverse() など）を適切に選択する。AngleAxis を直接 Quaternion の右オペランドとして乗算することはできません。toRotationMatrix() で回転行列に変換してから Quaternion と乗算するか、Quaternion 同士の乗算を使用します。
• 原因
  angleAxis * quaternion は、「最初に angleAxis の回転を適用し、その後に quaternion の回転を適用する」という意味です。もし逆の順序で回転を適用したい場合は、quaternion * angleAxis.toRotationMatrix() のように、Quaternion の乗算演算子を使用する必要があります。
• 問題
  期待した回転と異なる結果が得られる。

AngleAxis オブジェクトの初期化忘れまたは不正な初期化 (Uninitialized or Incorrectly Initialized AngleAxis Object)

• 解決策
  
  • AngleAxis オブジェクトを使用する前に、.angle() と .axis() メソッドを使って回転角と回転軸を明示的に設定する。
  • 回転軸は .normalized() メソッドを使って単位ベクトルに正規化されていることを確認する。
  • Eigen::AngleAxisd::Identity() などを使って恒等回転で初期化することも有効です。
• 原因
  
  • AngleAxis オブジェクトの回転角や回転軸が適切に設定されていない（例: デフォルトコンストラクタのまま使用している、回転軸が単位ベクトルでない）。
  • 無効な回転軸（長さがゼロのベクトルなど）を設定している。
• 問題
  予期しない回転結果が得られる。

数値誤差 (Numerical Errors)

• 解決策
  
  • 厳密な比較を行うのではなく、ある程度の許容範囲（イプシロン）内で比較する。
  • 必要に応じて、より高精度のスカラー型（例: float から double へ変更）を使用することを検討する。ただし、パフォーマンスとのトレードオフも考慮する必要があります。
• 原因
  浮動小数点演算の性質上、わずかな数値誤差は避けられません。特に、複雑な回転合成を繰り返すと誤差が累積する可能性があります。
• 問題
  厳密な値と比較して、わずかな誤差が生じる。

ライブラリのリンクエラー (Linking Errors)

• 解決策
  プロジェクトのビルド設定を確認し、必要なライブラリが正しくリンクされているか確認する。Eigen自体が原因である可能性は低いですが、他のライブラリとの連携に問題があるかもしれません。
• 原因
  Eigenライブラリはヘッダーオンリーであるため、通常はリンクの必要はありませんが、もし他のライブラリと組み合わせて使用している場合や、特殊なコンパイルオプションを使用している場合にリンクの問題が発生することがあります。
• エラー
  コンパイルは通るが、実行時にライブラリが見つからないなどのエラーが発生する。

• 回転の表現方法を理解する
  AngleAxis、Quaternion、回転行列など、回転の異なる表現方法の特性と相互の変換方法を理解することが重要です。
• 簡単な例から始める
  複雑な問題を一度に解決しようとせず、まずは簡単な合成操作が正しく動作することを確認してから、徐々に複雑なケースを試していく。
• Eigenのドキュメントを参照する
  Eigenの公式ドキュメントには、各クラスや演算子の詳細な説明や使用例が記載されています。
• 単位テストを書く
  簡単な既知の回転を合成するテストケースを作成し、期待される結果と比較することで、問題の原因を特定しやすくなります。
• デバッグ出力を活用する
  AngleAxis オブジェクトや Quaternion オブジェクトの内容（角度、軸、クォータニオンの各成分）を std::cout などで出力して、期待通りの値になっているか確認する。

---

例1: Z軸周りの回転とX軸周りの回転の合成

この例では、Z軸周りの回転を表す AngleAxis オブジェクトと、X軸周りの回転を表す Quaternion オブジェクトを作成し、それらを合成して最終的な回転を Quaternion として得ます。

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

int main() {
  // 1. AngleAxis オブジェクトの作成: Z軸周りに45度回転
  Eigen::AngleAxisd angleAxis1(Eigen::AngleAxisd::Identity());
  angleAxis1.angle() = M_PI / 4.0; // 45度 (ラジアン)
  angleAxis1.axis() = Eigen::Vector3d::UnitZ(); // Z軸 (0, 0, 1)

  std::cout << "AngleAxis 1 (Z軸周り 45度):\n" << angleAxis1.matrix() << std::endl;

  // 2. Quaternion オブジェクトの作成: X軸周りに90度回転 (AngleAxis から変換)
  Eigen::AngleAxisd angleAxis2(M_PI / 2.0, Eigen::Vector3d::UnitX()); // X軸周り 90度
  Eigen::Quaterniond quaternion2 = angleAxis2; // AngleAxis から Quaternion へ暗黙的に変換

  std::cout << "\nQuaternion 2 (X軸周り 90度):\n" << quaternion2.matrix() << std::endl;

  // 3. AngleAxis と Quaternion の合成 (angleAxis1 の後に quaternion2 の回転を適用)
  Eigen::Quaterniond combinedRotation = angleAxis1 * quaternion2;

  std::cout << "\n合成された回転 (Quaternion):\n" << combinedRotation.matrix() << std::endl;

  // 合成された回転を AngleAxis に変換して表示
  Eigen::AngleAxisd combinedAngleAxis(combinedRotation);
  std::cout << "\n合成された回転 (AngleAxis):\n角度: " << combinedAngleAxis.angle() * 180.0 / M_PI << " 度\n軸:\n" << combinedAngleAxis.axis() << std::endl;

  return 0;
}

説明

1. Eigen::AngleAxisd angleAxis1(...): Z軸周りに45度の回転を表す AngleAxisd オブジェクトを作成します。Eigen::Vector3d::UnitZ() は (0, 0, 1) の単位ベクトルを表します。
2. Eigen::Quaterniond quaternion2 = angleAxis2;: X軸周りに90度の回転を表す AngleAxisd オブジェクト angleAxis2 を作成し、それを暗黙的に Eigen::Quaterniond オブジェクト quaternion2 に変換します。
3. Eigen::Quaterniond combinedRotation = angleAxis1 * quaternion2;: angleAxis1 が表す回転の後に quaternion2 が表す回転を適用した結果を、新しい Eigen::Quaterniond オブジェクト combinedRotation に格納します。
4. Eigen::AngleAxisd combinedAngleAxis(combinedRotation);: 合成された Quaternion を AngleAxis に変換して、回転角と回転軸を表示します。

例2: 複数の回転の連続的な合成

この例では、複数の AngleAxis オブジェクトと Quaternion オブジェクトを組み合わせて、連続的な回転を合成します。

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

int main() {
  // 最初の回転: X軸周りに30度 (AngleAxis)
  Eigen::AngleAxisd rot1(M_PI / 6.0, Eigen::Vector3d::UnitX());

  // 2番目の回転: Y軸周りに60度 (Quaternion)
  Eigen::AngleAxisd rot2_aa(M_PI / 3.0, Eigen::Vector3d::UnitY());
  Eigen::Quaterniond rot2_q = rot2_aa;

  // 3番目の回転: Z軸周りに90度 (AngleAxis)
  Eigen::AngleAxisd rot3(M_PI / 2.0, Eigen::Vector3d::UnitZ());

  // 回転の合成: rot1 の後 rot2_q、さらにその後に rot3 を適用
  Eigen::Quaterniond combinedRotation = rot1 * rot2_q;
  combinedRotation = combinedRotation * rot3; // Quaternion 同士の乗算

  std::cout << "最初の回転 (AngleAxis):\n" << rot1.matrix() << std::endl;
  std::cout << "\n2番目の回転 (Quaternion):\n" << rot2_q.matrix() << std::endl;
  std::cout << "\n3番目の回転 (AngleAxis):\n" << rot3.matrix() << std::endl;
  std::cout << "\n最終的な合成された回転 (Quaternion):\n" << combinedRotation.matrix() << std::endl;

  return 0;
}

説明

1. 複数の回転を AngleAxis または Quaternion で定義します。
2. angleAxis1 * quaternion2 のように、AngleAxis と Quaternion を乗算することで、最初の回転の後に2番目の回転を適用した結果を得ます。
3. さらに回転を合成する場合は、得られた Quaternion に対して別の AngleAxis (または Quaternion) を乗算します。ただし、Quaternion の右オペランドに AngleAxis を直接使用することはできないため、rot3 は最終的に Quaternion に変換されてから乗算される必要があります（上記の例では暗黙的に AngleAxis から Quaternion への変換が行われています）。より明示的に書く場合は combinedRotation = combinedRotation * Eigen::Quaterniond(rot3); となります。

例3: ベクトルの回転

合成された回転を使って、ベクトルを回転させる例です。

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

int main() {
  // Z軸周りに30度の AngleAxis
  Eigen::AngleAxisd angleAxis(M_PI / 6.0, Eigen::Vector3d::UnitZ());

  // X軸周りに45度の Quaternion
  Eigen::AngleAxisd angleAxisQ(M_PI / 4.0, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond quaternion = angleAxisQ;

  // AngleAxis と Quaternion を合成
  Eigen::Quaterniond combinedRotation = angleAxis * quaternion;

  // 回転させるベクトル
  Eigen::Vector3d initialVector(1.0, 0.0, 0.0);
  std::cout << "初期ベクトル:\n" << initialVector << std::endl;

  // 合成された回転をベクトルに適用
  Eigen::Vector3d rotatedVector = combinedRotation * initialVector;
  std::cout << "\n回転後のベクトル:\n" << rotatedVector << std::endl;

  return 0;
}

1. AngleAxis と Quaternion を合成して、最終的な回転を表す combinedRotation を得ます。
2. 回転させたい initialVector を定義します。
3. 合成された Quaternion オブジェクトにベクトルを乗算することで、そのベクトルを合成された回転に従って回転させた結果 rotatedVector を得ます。

---

Quaternion 同士の乗算

AngleAxis オブジェクトを直接 Quaternion と乗算する代わりに、まず AngleAxis オブジェクトを Quaternion に変換し、その後で Quaternion 同士を乗算する方法があります。

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

int main() {
  // AngleAxis オブジェクト
  Eigen::AngleAxisd angleAxis(M_PI / 4.0, Eigen::Vector3d::UnitZ());

  // Quaternion オブジェクト
  Eigen::AngleAxisd angleAxisQ(M_PI / 2.0, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond quaternion = angleAxisQ;

  // AngleAxis を Quaternion に変換
  Eigen::Quaterniond quaternionFromAngleAxis = angleAxis;

  // Quaternion 同士を乗算 (angleAxis の回転の後に quaternion の回転を適用)
  Eigen::Quaterniond combinedRotation = quaternionFromAngleAxis * quaternion;

  std::cout << "合成された回転 (Quaternion):\n" << combinedRotation.matrix() << std::endl;

  return 0;
}

説明

• Quaternion 同士の乗算 quaternionFromAngleAxis * quaternion は、「最初に quaternionFromAngleAxis の回転を適用し、その後に quaternion の回転を適用する」という意味になります。これは AngleAxis::operator* と同じ順序です。
• Eigen::Quaterniond quaternionFromAngleAxis = angleAxis; のように、AngleAxis オブジェクトは Quaternion 型の変数に代入することで暗黙的に変換されます。明示的に変換する場合は Eigen::Quaterniond(angleAxis) を使用することもできます。

利点

• より一貫した記法で回転合成を行えます。常に Quaternion 同士の乗算を使用するため、演算の順序を理解しやすくなります。

回転行列 (Rotation Matrix) を介した合成

AngleAxis オブジェクトと Quaternion オブジェクトをそれぞれ回転行列に変換し、それらを乗算することで回転を合成する方法です。最後に、必要に応じて結果を Quaternion や AngleAxis に戻すことができます。

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

int main() {
  // AngleAxis オブジェクト
  Eigen::AngleAxisd angleAxis(M_PI / 4.0, Eigen::Vector3d::UnitZ());

  // Quaternion オブジェクト
  Eigen::AngleAxisd angleAxisQ(M_PI / 2.0, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond quaternion = angleAxisQ;

  // AngleAxis を回転行列に変換
  Eigen::Matrix3d rotationMatrix1 = angleAxis.toRotationMatrix();

  // Quaternion を回転行列に変換
  Eigen::Matrix3d rotationMatrix2 = quaternion.toRotationMatrix();

  // 回転行列同士を乗算 (angleAxis の回転の後に quaternion の回転を適用)
  Eigen::Matrix3d combinedRotationMatrix = rotationMatrix2 * rotationMatrix1; // 注意: 適用順序に注意！

  std::cout << "合成された回転行列:\n" << combinedRotationMatrix << std::endl;

  // 結果を Quaternion に変換
  Eigen::Quaterniond combinedRotationQuaternion(combinedRotationMatrix);
  std::cout << "\n合成された回転 (Quaternion):\n" << combinedRotationQuaternion.coeffs().transpose() << std::endl;

  return 0;
}

説明

• 結果の回転行列から Eigen::Quaterniond(combinedRotationMatrix) を用いて Quaternion オブジェクトを構築できます。
• 回転行列の乗算 rotationMatrix2 * rotationMatrix1 は、最初に rotationMatrix1 の回転を適用し、その後に rotationMatrix2 の回転を適用するという意味になります。これは AngleAxis::operator* と同じ順序です。行列の乗算の順序に注意してください。
• angleAxis.toRotationMatrix() と quaternion.toRotationMatrix() を使用して、それぞれの回転表現を 3x3 の回転行列に変換します。

利点

• 回転行列に関する既存の知識やアルゴリズムを応用しやすい。
• 回転の合成を線形代数の枠組みで理解しやすい。

欠点

• Quaternion の方が数値的に安定している場合がある（特に多くの回転を連続して合成する場合）。
• Quaternion よりもメモリ使用量が多くなる可能性があります（3x3 行列 vs 4成分ベクトル）。

累積的な回転の適用

複数の回転を連続して適用する場合、一つずつ Quaternion に変換して累積的に乗算していく方法も考えられます。

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

int main() {
  // 最初の回転 (AngleAxis)
  Eigen::AngleAxisd rot1(M_PI / 6.0, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond q1 = rot1;

  // 2番目の回転 (Quaternion)
  Eigen::AngleAxisd rot2_aa(M_PI / 3.0, Eigen::Vector3d::UnitY());
  Eigen::Quaterniond q2 = rot2_aa;

  // 3番目の回転 (AngleAxis)
  Eigen::AngleAxisd rot3(M_PI / 2.0, Eigen::Vector3d::UnitZ());
  Eigen::Quaterniond q3 = rot3;

  // 累積的に Quaternion を乗算
  Eigen::Quaterniond combinedRotation = q1 * q2 * q3; // rot1 -> rot2 -> rot3 の順

  std::cout << "最終的な合成された回転 (Quaternion):\n" << combinedRotation.matrix() << std::endl;

  return 0;
}

説明

• 合成したい順序で Quaternion オブジェクトを左から順に乗算していきます。q1 * q2 は「最初に q1 の回転を適用し、その後に q2 の回転を適用する」という意味になります。
• 各回転を AngleAxis または Quaternion で定義し、必要に応じて Quaternion に変換します。

利点

• Quaternion の効率性と数値安定性を維持できます。
• 複数の回転を扱う場合に、合成の順序が明確になります。

• 累積的な Quaternion の乗算は、複数の回転を順に適用していく場合に自然な記述方法となります。
• 回転行列を介した方法は、回転行列に関する知識を活用したい場合や、他のライブラリとの互換性が必要な場合に有用です。ただし、行列の乗算順序には注意が必要です。
• 多くの場合、Quaternion 同士の乗算 (AngleAxis を最初に Quaternion に変換する) が、コードの可読性、効率性、数値安定性のバランスが良い選択肢となります。