g2oのインストールと簡単なサンプルの実行

g2oとは?

  • SLAMによく使われるグラフ最適化ライブラリ

  • グラフ最適化

    • 非線形最適化とグラフ理論を組み合わせたもの
    • 最適化する変数が誤差項にいくつ存在するかを可視化できる
    • 孤立した頂点の排除や多数の辺を持つ頂点を優先するようにできる
    • 非線形最小二乗最適化問題をグラフモデルに表現できる
    • SLAMで使う上でCeresに比べて実装コストが低い

  • 例

    • 曲線フィッティング($y=ax2+bx+c$)
      • 頂点
        • $a, b, c$
      • 辺
        • 誤差項
    • BA
      • 頂点
        • 最適化変数
          • カメラポーズ
          • ランドマーク
      • 辺
        • 誤差項
          • カメラのtrajectory
          • カメラから観測したランドマークを結んだもの

  • g2oのインストール

  • 曲線フィッティングをサンプルとSLAMBOOKにそってほぼ写経したものをgithubに上げました

github.com

SLAMBOOKに分かりやすく説明が書かれている 頂点を$a, b, c$のベクトル、辺を各データの誤差としてグラフを考える つまり1つの頂点からその頂点自身に向けてデータ数だけ辺が伸びているグラフになる

実装では頂点はBaseVertex、辺はBaseUnaryEdgeを継承すればいい

頂点ではGNやLMで足していくときの演算の関数oplusimplを定義する これは単純に足し算すればいいだけでなくリー代数のときのような特殊な演算が必要なときがあるから

疑問なのはread()とwrite()がvirtualなんだけどSLAMBOOKとかだと関数の中身は書かずに、実行すると返り値がないとエラーが出てしまうこと これはg2o本家のサンプルを見るととりあえずfalseを返していたのでそれに倣ったら一応動いた まあとりあえずg2oを試すのが目的だから

OpenCV Matxの代入

Matの代入がアドレスで、値をコピーしたいときは.copy()を使用していたけどMatxには無かった

cv::Mat m1 = (cv::Mat_<double>(1, 3) << 4, 5, 6);
cv::Mat m2 = m1;
m1.at<double>(2) = 10;
std::cout << m1 << std::endl;
std::cout << m2 << std::endl;

cv::Matx13d mx1(4, 5, 6);
cv::Matx13d mx2 = mx1;
mx1(2) = 10;
std::cout << mx1 << std::endl;
std::cout << mx2 << std::endl;

結果

[4, 5, 10]
[4, 5, 10]
[4, 5, 10]
[4, 5, 6]