3次元相似変換(Exponential Map)の実装

Visual SLAMでは、スケールの不定性があるため、スケールドリフトと呼ばれる問題が生じ、Loop Closureなどで蓄積誤差を解消するにあたって3次元の剛体変換ではうまく補正できない場合がある。このような場合に、3次元相似変換Sim3を用いて補正を行う方法が提案されている。

Scale Drift-Aware Large Scale Monocular SLAM
http://roboticsproceedings.org/rss06/p10.pdf

Sim3のExponential Mapは上記論文や以下の記事を参照してください。(書くのが面倒なので。。。)

Sim(3) optimization の exponential map を計算してみた

3次元の相似変換では以下のように回転成分にスケール要素が掛かった形になる。

ここで、expSim(3)、expSO(3)はそれぞれSim(3)、SO(3)のExponential Mapを表す。Wの中身については、論文や上記記事を参照してください。

Sophusを用いた実装例は、以下のようになる。

#include <iostream>

#include "sophus/sim3.hpp"
#include "sophus/geometry.hpp"

int main() {
  // 単位行列
  Sophus::Sim3d T0;
  std::cout << "T0:" << std::endl;
  std::cout << T0.matrix() << std::endl;

  // 7次元のパラメータベクトル
  Sophus::Vector7d uws;
  uws << 10.0, 0.0, 0.0, Sophus::Constants::pi(), 0.0, 0.0, 0.0;
  Sophus::Sim3d T1 = Sophus::Sim3d::exp(uws);

  // スケーリング
  double scale = 2.0;
  T1.setScale(scale);

  std::cout << "T1:" << std::endl;
  std::cout << T1.matrix() << std::endl;
  return 0;
}

コメント

このブログの人気の投稿

COLMAPでキャリブレーション済みのデータを使う

5点アルゴリズムによるカメラ位置・姿勢の推定

2D-3D対応からのカメラ位置・姿勢の推定