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を用いた実装例は、以下のようになる。
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; }
コメント
コメントを投稿