Quaternionの理解・調査を兼ねて、Eigen::Quaternionの挙動を整理しました。

Eigenとは

Eigenとは、C++用の行列計算ライブラリです。ヘッダファイルのみで構成されているためインストールが簡単、軽量、高速といった利点があります。

大まかな使い方については以下の記事が詳しいです。

Quaternionとは

Quaternion（四元数）とは、1つの実数と3つの虚数から構成されたものです。直感的には扱いにくいですが、回転行列やEuler角と同様に3次元の回転を表すことができます。特異点がないこと、合成や補間が容易で高速なこと等からよく用いられています。

Eigen::Quaternion

注意

以下のコード例は独自関数等を略しています。一番下に検証用プログラムを載せているので、気になる方はそちらもお願いします。

定義方法

#include <Eigen/Geometry>
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Quaterniond Quat;
typedef Eigen::AngleAxisd AngleAxisd;

// 直接指定
Quat q1(1.0, 0.0, 0.25, 0.5);

// 角度（ラジアン）とベクトルを渡す = Euler角
Quat q2(AngleAxisd(1, Vector3::UnitX())     // roll
        * AngleAxisd(0, Vector3::UnitY())    // pitch
        * AngleAxisd(0, Vector3::UnitZ()));  // yaw

// 2つのベクトルの間の角度を求める
Quat q3 = Quat::FromTwoVectors(Vector3::UnitX(), Vector3::UnitZ());

// 単位クオータニオン(実数が1で虚数が0)
Quat q4 = Quat::Identity();

// 回転行列から変換
Quat q5(Matrix3::Identity());

回転角の合成

クォータニオンによる回転角を合成するためには、クォータニオンの外積を求めればよい。

// これ以降Quaternionの初期値はEuler角を用いて指定
Quat q1 = rpy2q(deg2rad(30.0, 0.0, 0.0));
Quat q2 = rpy2q(deg2rad(20.0, 0.0, 0.0));
Quat q3 = q1 * q2;  // (50, 0, 0) [deg]

逆クォータニオンと共役クォータニオン

逆クォータニオン

Quat q1 = rpy2q(deg2rad(10.0, 30.0, -50.0));
Quat q1_inv = q1.inverse();  //  (17.2967, -26.7137, 51.4507) [deg]
Quat q1q = q1 * q1_inv;  // zero

共役クォータニオン

Quat q2 = rpy2q(deg2rad(10.0, 30.0, -50.0));
Quat q2_inv = q1.conjugate();  //  (17.2967, -26.7137, 51.4507) [deg]
Quat q2q = q2 * q2_inv;  // zero

クォータニオンが正規化されている時、逆クォータニオンと共役クォータニオンは等しい。

// 正規化
q1.normalize()
Quat q3 = q2.normalized()

三次元ベクトルの回転

ベクトルを原点を中心に回転させる時

Vector3 v1(1, 0, 0);
Quat q1 = rpy2q(deg2rad(0.0, 90.0, 0.0));
Vector3 v2 = q1 * v1;

ベクトルを原点を中心に回転させる時

Vector3 v3(1, 0, 0);
Quat q2 = rpy2q(deg2rad(0.0, 90.0, 0.0));
Vector3 v4 = q2.conjugate() * v3;

検証プログラム

gist0aa2dede3fdb29e7f356e358cc3e2410

kukkuのメモ帳

勉強したこと，思ったこと，あったことをつらつら書いていきます

Eigen::Quaternionの挙動を整理する