大域的目標
KellerらのRGB-D SLAM[1]が実装したい!と思い立ったので実装していく,というモチベーションの記録.ちょっとずつ実装している.今回が5回目.モチベーションに関する詳細は初回の記事を参照のこと.
前回(以下,参照)は,画像ピラミッドを使ったICPアルゴリズムを実装した.
今回の目標
キーフレームに対する位置姿勢推定を行う.
実装内容
やることはとても単純.前回に実装したICPアルゴリズムは前フレームと現フレームを入力として,前フレームに対する現フレームの位置姿勢を推定していた.なので,今回は,前フレームをあるフレームに固定して,現フレームだけ更新する.
ただし,固定したあるフレームと現フレームの見た目は,移動量や回転量が大きくなるほど異なるため,前に推定した位置姿勢を初期値としてICPアルゴリズムを実行する.
実装結果
以下の,Gifアニメーションは,右側が現フレームのカラー画像,左側が現フレームの奥行画像をキーフレーム(初期フレーム)に投影した時のデータ(この可視化方法については,過去の記事を参照のこと).
並進した場合.視野内にキーフレームが写っているので,継続してトラッキングできている.
回転した場合.視野内にキーフレームが写らなくなるので,おっとと~!あぁ,もう無理... という感じでトラッキングが破綻する.
所感
ということで,ある程度フレームがキーフレームを捉えなくなったと判断したら新たにキーフレームを登録する必要がある,ということになる.この時点でも立派なSLAMに思えるが,キーフレームを新たに登録する場合,これまで参照していたキーフレームを新たなキーフレームに投影してから,キーフレームとなる現フレームのデータとマージしなくてはいけないと思われる.
でも,キーフレームの追加の前に,キーフレーム(マップ)の3次元位置や法線の更新を実装したいなぁ.