三次元の変換行列

SLAMなどの三次元復元やカメラの位置・姿勢推定では、三次元の変換行列を取り扱う場面が多くある。剛体変換の場合、三次元の変換行列は4×4の行列で表現でき、以下のように左上の3×3成分が回転行列R、4列目の上から3つが平行移動成分tとなっている。

これらのパラメータをプログラム内部でどのように保持するのが良いか悩ましい場合がある。特に、Rやtが別々の変数で保持されている場合に、どの座標系からどの座標系への変換なのかが不明な場合がある。（オープンソースのSLAMで、Rとtが同じ変換の向きで無い場合があったりする）

何が最善か分からないが、OpenCVを使う場合は、Affine3を利用するのが色々と問題も少なく良いような気がする。以下に、Affine3fを使った三次元変換行列の一例を示す。

#include <iostream>

#include <opencv2/opencv.hpp>

int main(int argc, char** argv) {
  cv::Vec3f rvect1(0.0f, 0.0f, 0.0f);
  cv::Vec3f tvect1(1.0f, 1.0f, 1.0f);
  cv::Affine3f T1(rvect1, tvect1);

  std::cout << "rotation matrix" << std::endl;
  std::cout << T1.rotation() << std::endl;

  std::cout << "translation" << std::endl;
  std::cout << T1.translation() << std::endl;

  std::cout << "4x4 matrix" << std::endl;
  std::cout << T1.matrix << std::endl;

  cv::Vec3f rvect2(1.0f, 1.0f, 0.0f);
  cv::Vec3f tvect2(1.0f, 1.0f, 0.0f);
  cv::Affine3f T2(rvect2, tvect2);

  std::cout << "4x4 matrix" << std::endl;
  std::cout << T2.matrix << std::endl;

  // T1*T2
  cv::Affine3f T12 = T2.concatenate(T1);
  std::cout << "T1 * T2" << std::endl;
  std::cout << T12.matrix << std::endl;

  // または
  T12 = T1 * T2;
  std::cout << "T1 * T2" << std::endl;
  std::cout << T12.matrix << std::endl;

  return 0;
}

このようにAffine3fを使うと、回転ベクトル、併進ベクトルの値のセット、回転行列、併進ベクトル、4x4行列の取り出しが簡単に行える。また合成変換についても上記のように簡単に行える。同様の機能はEigenにもあるので、OpenCVやEigenを用いて変換行列を保持する場合にはAffine3を使った方が便利なように思う。