【C++/Eigen】Eigenで2次元の同次変換行列を扱う

はじめに

C++の数値計算ライブラリであるEigenを使用して主に2次元の同次変換行列を扱う方法を紹介します。
参考までに、Eigenとは何か、どんなメリットがあるかはChatGPTに聞いたところ以下のような返答が返ってきました。

Eigenは特に数値計算や機械学習、ロボティクス、グラフィックスなどの分野で幅広く使用されており、その高いパフォーマンスと柔軟性が評価されています。
Eigenを使用するメリットはいくつかあります。以下にその主なポイントを挙げてみましょう：

• 高性能な行列演算: Eigenは高度に最適化された行列演算ライブラリであり、BLAS（Basic Linear Algebra Subprograms）やLAPACK（Linear Algebra PACKage）のようなライブラリと統合されています。これにより、行列演算が非常に高速で効率的に行えます。
• 抽象化と一般性: Eigenは様々な次元の行列やベクトルに対応しており、静的サイズや動的サイズの行列を扱うことができます。また、様々な行列演算や分解を提供しています。
• コンパイル時最適化: Eigenはテンプレートを使用しており、多くの行列演算がコンパイル時に解決されます。これにより、実行時におけるオーバーヘッドを最小限に抑えつつ、最適なコードを生成することができます。
• ヘッダーオンリー: Eigenはヘッダーオンリーライブラリであり、インストールが簡単です。プロジェクトに導入するのが非常に容易で、他のライブラリと組み合わせやすいです。
• ライセンス: EigenはMPL2（Mozilla Public License 2.0）の下で提供されており、商用プロジェクトでも自由に使用できます。
• 豊富な機能: Eigenは行列演算に特化しているだけでなく、クォータニオン、回転行列、変換行列、多項式など、様々な数学的概念や機能をサポートしています。

また、同次変換行列についての説明とPythonのサンプルコードは以下の記事が参考になりました。

環境

項目	バージョン
OS	Ubuntu 22.04
Eigen	3.4.0

Eigenのバージョンは以下で確認しました。

$ grep "#define EIGEN_[^_]*_VERSION" /usr/include/eigen3/Eigen/src/Core/util/Macros.h
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 4
#define EIGEN_MINOR_VERSION 0

同次変換行列の適用

まずコードの例と実行結果を以下に示します。以下のようなコードで2次元の座標系の平行移動、回転行列、同次変換行列の適用を実施することができます。
複雑な行列の計算はなく、すべて積算で実行できていることがわかります。

eigen_sample.cpp

#include <iostream>
#include <Eigen/Dense>
#include <math.h>

int main() {
    // 2次元の初期位置
    Eigen::Vector2d pose(1, 2);

    // 平行移動行列
    Eigen::Translation2d translation(1, 1);
    double yaw = 45.0;
    // 回転行列
    Eigen::Rotation2Dd rotation(yaw / 180 * M_PI);

    std::cout << "初期位置: \n" << pose << std::endl;
    std::cout << "平行移動行列: \n" << translation.translation() << std::endl;

    // 平行移動
    Eigen::Vector2d translated_pose = translation * pose;
    std::cout << "平行移動後の位置: \n" << translated_pose << "\n" << std::endl;

    std::cout << "回転行列: \n" << rotation.matrix() << std::endl;
    // 回転
    Eigen::Vector2d rotated_pose = rotation * pose;
    std::cout << "回転後の位置: \n" << rotated_pose << "\n" << std::endl;

    // 同次変換行列
    Eigen::Transform<double, 2, Eigen::Isometry> transform = translation * rotation;
    std::cout << "同次変換行列: \n" << transform.matrix() << "\n" << std::endl;

    // 同次変換行列の適用
    Eigen::Vector2d transformed_pose = transform * pose;
    std::cout << "適用後の位置: \n" << transformed_pose << std::endl << std::endl;

    return 0;
}

ビルドは以下のコマンドで実行します。

g++ eigen_sample.cpp -I /usr/include/eigen3/ -o eigen_sample_out

実行結果

初期位置:
1
2
平行移動行列:
1
1
平行移動後の位置:
2
3

回転行列:
 0.707107 -0.707107
 0.707107  0.707107
回転後の位置:
-0.707107
  2.12132

同次変換行列:
 0.707107 -0.707107         1
 0.707107  0.707107         1
        0         0         1

適用後の位置:
0.292893
 3.12132

コード解説

初期位置

初期位置は、Eigen::Vector2dで(X, Y)の値を指定しています。

// 2次元の初期位置
Eigen::Vector2d pose(1, 2);

平行移動

平行移動行列をEigen::Translation2dで表現し、初期位置を右側からかけることで平行移動を適用します。

// 平行移動行列
Eigen::Translation2d translation(1, 1);
// 平行移動
Eigen::Vector2d translated_pose = translation * pose;

数式で表すと以下の処理を実施しています。

\begin{pmatrix}
x^{\prime} \\
y^{\prime}\\
1\\
\end{pmatrix}
= 
\begin{pmatrix}
1 & 0 & t_x \\
0 & 1 & t_y \\
0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
x \\
y \\
1 \\
\end{pmatrix}

回転

回転行列をEigen::Rotation2Ddで表現し、初期位置を右側からかけることで回転を適用します。

double yaw = 45.0;
// 回転行列
Eigen::Rotation2Dd rotation(yaw / 180 * M_PI);
// 回転
Eigen::Vector2d rotated_pose = rotation * pose;

数式で表すと以下の処理を実施しています。

\begin{pmatrix}
x^{\prime} \\
y^{\prime}\\
1\\
\end{pmatrix}
= 
\begin{pmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
x \\
y \\
1 \\
\end{pmatrix}

同次変換行列の適用

同次変換行列をEigen::Transform<double, 2, Eigen::Isometry>で表現します。
引数については以下が参考になります。X上でのコメントありがとうございました！

同次変換行列の生成は簡単で、上記の平行移動行列と回転行列をかけると生成できます。

// 同次変換行列
Eigen::Transform<double, 2, Eigen::Isometry> transform = translation * rotation;
std::cout << "同次変換行列: \n" << transform.matrix() << "\n" << std::endl;

2次元の場合、同次変換行列は以下になります。

T =
\left(
\begin{array}{}
\cos \theta & - \sin \theta & t_x \\
\sin \theta &   \cos \theta & t_y \\
0           & 0             & 1 
\end{array}
\right)

同次変換行列の適用も、同次変換行列に右側から初期位置をかけることで実現できます。

// 同次変換行列の適用
Eigen::Vector2d transformed_pose = transform * pose;

数式で表すと以下の処理を実施しています。

\begin{align}
\begin{pmatrix}
x^{\prime} \\
y^{\prime}\\
1\\
\end{pmatrix}
&=
\left(
\begin{array}{}
\cos \theta & - \sin \theta & t_x \\
\sin \theta &   \cos \theta & t_y \\
0           & 0             & 1 
\end{array}
\right)
\begin{pmatrix}
x \\
y \\
1 \\
\end{pmatrix} \\
\end{align}
 \tag{3}

おわりに

ロボットの位置姿勢を扱うにあたってEigenを使用して計算をしようとしたところ分からなかったところをまとめました。参考になれば幸いです。また、なにか間違っているところ等ありましたら教えてください。