pybind11 で Eigen/Geometry を呼び出してロール・ピッチ・ヨー角と回転行列を計算する。

pybind11 では numpy の行列と Eigen の行列を相互参照してくれます。numpy には３次元幾何用の関数が不足しているので、C++ の Eigen/Geometry を呼び出して、ロール・ピッチ・ヨー角と回転行列を計算するモジュールを作成しました。pybind11 の使い方の例として記事にしておきます。

# 環境
* CMake がインストールされ、コマンドラインから `cmake` が起動できること。
* pybind11 がインストールされ、環境変数 `pybind11_DIR` の設定等によって、`find_package(pybind11)` が成功すること。
* Eigen がインストールされ、環境変数 `Eigen3_DIR` の設定等によって、`find_package(Eigen3)` が成功すること。

# モジュールの作成

適当な作業用ディレクトリに以下の内容で geometry.cpp と CMakeLists.txt を作成する。
```geometry.cpp
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Dense>
#include <Eigen/Geometry>

using vec3 = Eigen::Vector3d;

// I/O Matrices are RowMajor for numpy
using vec3R = Eigen::Matrix<double, 1, 3, Eigen::RowMajor>;
using mat3R = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>;

mat3R AngleAxis(double angle, Eigen::Ref<const vec3R> axis) {
    mat3R R;
    R = Eigen::AngleAxisd(angle, Eigen::Map<const vec3>(axis.data()));
    return R;
}

vec3R RollPitchYawOf(Eigen::Ref<const mat3R> R) {
    vec3 e = R.eulerAngles(2, 1, 0);
    return vec3R(e[2], e[1], e[0]);
}

mat3R FromRollPitchYaw(Eigen::Ref<const vec3R> rpy) {
    Eigen::Quaterniond z(cos(rpy[2]/2), 0, 0, sin(rpy[2]/2));
    Eigen::Quaterniond y(cos(rpy[1]/2), 0, sin(rpy[1]/2), 0);
    Eigen::Quaterniond x(cos(rpy[0]/2), sin(rpy[0]/2), 0, 0);
    mat3R R;
    R = (z*y*x).toRotationMatrix();
    return R;
}

using namespace pybind11::literals;

PYBIND11_MODULE(geometry, m) {
    m.def("AngleAxis", &AngleAxis, "angle"_a, "axis"_a);
    m.def("RollPitchYawOf", &RollPitchYawOf, "R"_a);
    m.def("FromRollPitchYaw", &FromRollPitchYaw, "rpy"_a);
}
```
```CMake:CMakeLists.txt
cmake_minimum_required(VERSION 3.15)

set(modname "geometry")

project(${modname} LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)

find_package(Python COMPONENTS Interpreter Development)
find_package(pybind11 REQUIRED)

find_package(Eigen3 REQUIRED)
message(STATUS "Found Eigen-" ${EIGEN3_VERSION_STRING})

pybind11_add_module(${modname})
target_sources(${modname} PRIVATE ${modname}.cpp)
target_link_libraries(${modname} PRIVATE Eigen3::Eigen)

# Copy the target module to the source directory.
add_custom_command(TARGET ${modname} POST_BUILD COMMAND ${CMAKE_COMMAND} -E
  copy $<TARGET_FILE:${modname}> ${CMAKE_SOURCE_DIR}
)
```
端末（Windows の場合は Developer Command Prompt for VS 2022）から
```
$ mkdir build
$ cd build
$ cmake ..
$ cmake --build . --config Release
```
とし、出来上がった ```geometry.~~~.pyd``` （`~~~` の部分は環境に依存。ubuntu では `geometry.~~~.so`）を Python を実行するディレクトリもしくは ```PYTHONPATH``` が通ったディレクトリにコピーする。

# 計算例
```python
>>> import numpy as np
>>> import geometry
>>> u = np.array([0, 0, 1])
>>> geometry.AngleAxis(np.pi/2, u)
array([[ 6.123234e-17, -1.000000e+00,  0.000000e+00],
       [ 1.000000e+00,  6.123234e-17,  0.000000e+00],
       [ 0.000000e+00,  0.000000e+00,  1.000000e+00]])
>>> rpy = np.array([-np.pi/2, np.pi/6, np.pi/3])
>>> print(rpy)
[-1.57079633  0.52359878  1.04719755]
>>> R = geometry.FromRollPitchYaw(rpy)
>>> print(R)
[[ 0.4330127 -0.25      -0.8660254]
 [ 0.75      -0.4330127  0.5      ]
 [-0.5       -0.8660254  0.       ]]
>>> print(geometry.RollPitchYawOf(R))
[-1.57079633  0.52359878  1.04719755]
```
* `AngleAxis(angle, axis)` は、単位ベクトル `axis` の周りでの `angle` の回転操作を表す回転行列（３次の直交行列）を計算します。
* `FromRollPitchYaw` は３次元ベクトルの要素を順にロール角、ピッチ角、ヨー角とみなして回転行列を計算します。
* `RollPitchYawOf` は与えられた回転行列に対応する、ロール角、ピッチ角、ヨー角を計算し、ベクトルに入れて返します。
* ここでいうロール・ピッチ・ヨー角は Z-Y-X オイラー角のことです。計算効率は劣りますが、次のようにも書けます。
```C++
mat3R FromRollPitchYaw2(Eigen::Ref<const vec3R> rpy) {
    mat3R R;
    R = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ())
      * Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX());
    return R;
}
```