cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

NDTのD2D Registrationを試してみる

NDTについて

NDT(Normal Distribution Transform)は、大規模な点群データをざっくりと扱いたいときに使う特徴量で、空間を適当なGridで分割してGrid内の点群分布を正規分布に表したものです。点群が多くなっても高速に計算できる、応用範囲が広く、多くのアルゴリズムで精度が思いのほか高いなどのメリットがあります。
NDTを使って位置合わせを計算するアルゴリズムもいくつか提案されていて、NDTと点群(P2D: Point to Distribution)、NDT-MCL(パーティクルフィルタ)、NDTとNDT(D2D: Distribution to Distribution)などが知られています。ROSのSLAMの位置合わせアルゴリズムとしてよく使われていたり、国内では、名古屋大学のAutoware[1], 豊田中央研究所[2], 九州大学の倉爪研究室[3]などが力を入れているようです。

NDT-D2D Registration

NDT-P2D, NDT-MCLはなんだか無駄が多い気がして使う気にならなかったのですが、NDT-D2DはNDT同士を対応付けて位置姿勢をニュートン法で求めるというシンプルな方法をとるので良さそうに見えます。
元の論文は[4]で、実装例はスウェーデンのエーレブルー大学にあります。
github.com
Linux環境でROSが使える人は、ROSパッケージが出ていますので素直にこちらを参照してください。
ndt_registration - ROS Wiki

NDT-D2DをWindowsで試したい

Orebro大のソースコードは一見大きなライブラリに見えますが、NDT-D2Dの部分だけを部分的に取り出しても動かすことができるようです。
意味づけラベルとNDTを一緒に使うというzaganidisさんの提案のse_ndtのコードの内、oru_minimalが参考になります、が、OcTreeなども省いてしまっているようなので、これを参考にオリジナルからソースコードを切り出してみます。
Semantic-assisted 3D normal distributions transform for scan registration in environments with limited structure - IEEE Conference Publication
github.com

NDT-D2DをWindowsのopenFrameworksで試す

github.com
に切り出したソースコードを置きます。gettimeofdayなどLinux用関数を強制的に置き換えていますので、Windowsでしか動かない部分があると思います。変更を加えたソースコードの周りには//////(スラッシュ五個)を置いていますので、修正時の参考にしてください。
依存ライブラリとして、PCLとopenCVが必要です。
cvl-robot.hateblo.jp
を参考に

vcpkg install pcl:x64-windows
vcpkg install opencv:x64-windows

を通してください。vcpkg integrate installをしてしまうと、openFrameworksが面倒くさくなるのでしない方が良いです。インストールフォルダに手動でインクルードパスとライブラリパスを通してください。

include path: C:\vcpkg\installed\x64-windows\include
library path: C:\vcpkg\installed\x64-windows\lib

ライブラリ名を手動で列挙するのは苦行なので、こちらの便利なアプリを使うと良いでしょう。
neno-garden.com

実行結果

(多分ライブラリの実装上の制限で)スケールを合わせる必要がありますが、ちゃんと位置合わせ出来ています。
初期位置
f:id:cvl-robot:20181005223953p:plain
赤い点が位置合わせ結果
f:id:cvl-robot:20181005223959p:plain
ICPじゃ初期位置合わせを厳密にやらないと収束しそうもないデータでも(ぴったりじゃないけど)収束している。

赤い丸が位置合わせ結果
f:id:cvl-robot:20181005224012p:plain

[1] autoware.ai
[2] https://ipsj.ixsq.nii.ac.jp/ej/index.php?action=pages_view_main&active_action=repository_action_common_download&item_id=107049&item_no=1&attribute_id=1&file_no=1&page_id=13&block_id=8
[3] https://www.jstage.jst.go.jp/article/jrsj/31/9/31_31_896/_pdf
[4] http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.818.9757&rep=rep1&type=pdf