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);
}

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 が通ったディレクトリにコピーする。

計算例

>>> 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 オイラー角のことです。計算効率は劣りますが、次のようにも書けます。

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;
}