前回
前回は第6章のオドメトリによる地図構築の部分まで行い、それから第7章以降にある内容を写経していったのだが、第7章の部分だけを実装というのはやや難しく、結局全部実装することになってしまった。（継承とポインタによって実行後に構成を切り替えているため、必要な部分だけ切り出して写すというのが面倒くさかった）
全部行ったので最後のループ閉じ込みまでちゃんと実装されたものが出力できた。作成された画像は以下の通り。
きちんとループが閉じられて整合性が取れていることがわかる。まぁコードを写していっただけなのでこれができるのは当然。
現状まだ理解ができていないところとして
1. 共分散の計算と伝播
おそらくオドメトリ側かスキャンマッチング側かによらず、1つのポーズに対して1つの共分散が付随して、それが伝播していくようなものなのだと思っているが、理解が足りなくてこれの具体的な構成を自分だけでは再現できない。
自分が実利用するときには、おそらく3次元6DoFの姿勢を対象とするというのも問題で、回転成分をクォータニオンで表現する場合に共分散行列がどうなるのかさっぱり想像できていない。共分散行列自体はラジアン角とかで表現されるもので、都度それを変換するということになるのだろうか。
2. 効率的なマップのデータ構造
地図を保持する上でどのようなデータ構造が良いのかちゃんとわかっていない。最後のループ閉じ込みで、突然部分地図に分割して持っておくと良いという記述がでてきたけれど、あまりその根拠に納得もできなかった。ある程度の位置にあたりをつけて探索するなら全体地図への探索でもそこまで非効率的でもない気はする。実践的にはもっと巨大な地図になるので、メモリに載りきらないので徐々に入れ替えていくということだろうか。
点群をグリッドに分けるのはそうだろうけど、そこからグリッドごとに点群のvectorを持つのか、代表点だけで良いのか、グリッドの大きさはどれくらいが良いのかなど、そのあたりも掴みきれていない。
スキャン点群のResampleもどこまで必要性が高いのかピンと来なかった。まぁあったほうが良いと言えばそうなのかもしれない。
3. ポーズグラフ最適化
ポーズグラフ最適化のところについて、実装の方ではp2oというライブラリに任せているという感じだったので、そちらも読んでみないとわからなそう。理屈としては二乗誤差を最小化していきたいということなのだとは思うが、それがプログラム的にどう実装されるのか。線形代数の肝のところはさらにEigenのライブラリ内部になるのかもしれないが。
今後
もう一度本を読み直して、今度は自分なりのコーディングスタイルで0から書き直してみたい。いろいろな機能を組み替えられるようにするところまではしなくて良いので、最初から全部盛りの実装で。コンピュータ将棋選手権も近いのでいつできるかはわからないが……。
