3次元の剛体変換（Exponential Map）

3次元の回転（Exponential Map）ではSO(3)による回転表現について、簡単な説明とC++のライブラリSophusを用いた実装例について紹介した。今回は、併進を加えた3次元の剛体変換SE(3)について、簡単に紹介する。

SE(3)のExponential Mapは6次元のベクトル(u ω)^Tを用いて以下のように表される。ここで、ωはSO(3)の時と同様に回転の軸角表現となっている。

逆変換のLogarithm Mapは、まずSO(3)のLogarithm Mapを用いてωを算出し、その後、このωを用いて以下のように算出できる。

また、以下のような2つの変換行列T₁，T₂の補間を行いたい場合は、次のように中間の剛体変換を算出することができる。

ここで、wは重みで0～１の値をとる。この補間方法を用いることで、2つのカメラ視点の中間視点などを算出できるようになる。
以下に、Sophusを用いたSE(3)の実装例を示す。

#include <iostream>
#include "sophus/geometry.hpp"
int main() {
  // 単位行列
  Sophus::SE3d T0;
  std::cout << "T0:\n" << T0.matrix() << std::endl;

  // パラメータベクトルse3（後ろ3つが回転so3と対応）
  Sophus::Vector6d uw1;
  uw1 << 0.0, 0.0, 0.0, Sophus::Constants::pi() / 3, 0.0, 0.0;
  // x軸周りに60度回転させる変換行列
  Sophus::SE3d T1 = Sophus::SE3d::exp(uw1);
  std::cout << "Transform matrix" << std::endl;
  std::cout << T1.matrix() << std::endl;
  std::cout << std::endl;

  //変換行列2
  Sophus::Vector6d uw2;
  uw2 << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
  Sophus::SE3d T2 = Sophus::SE3d::exp(uw2);
  std::cout << "T2:\n" << T2.matrix() << std::endl;
  std::cout << std::endl;

  // T1*T2
  std::cout << "T1*T2" << std::endl;
  std::cout << (T1 * T2).matrix() << std::endl;
  std::cout << std::endl;

  // 変換行列T1，T2の補間
  std::cout << "Interpolation" << std::endl;
  double interval = 1.0 / 10.0;
  for (double i = 0; i <= 1.0; i += interval) {
    Sophus::SE3d T = Sophus::SE3d::exp(i * (T2 * T1.inverse()).log()) * T1;
    std::cout << "step: " << i << "\n" << T.matrix() << std::endl;
  }

  return 0;
}