環境
- ROS1やROS2のpythonで動かしていました．
- Anacondaの使用時は少し上手く動かないかもしれません．
目的
超シンプルなICPアルゴリズムといえど，一旦は自分で書き起こして，点同士対応付けやマッチングの様子をビジュアル化をしたいもの．そこで，pythonで1スキャン分のICPスキャンマッチング(2D)を再現してみた．本アルゴリズムは「SLAM入門」を参考にして作りました．
Repository
git clone https://github.com/KOKIAOKI/simple_icp.git
概要
-
点群は，csv形式で任意のx,yの座標を用意します．(点群データは最終的にはnumpy配列にして計算していくので，ご自分に合わせて変えてみてください)
-
初期位置を手動で与えることができます．やり直すこともできます．最適化手法のパラメータとなる点群の代表点は，点群座標の平均値です．
-
最急降下法，ニュートン法，共役勾配法(CG法)のいずれかを用いて点群と点群同士をマッチングさせることができます．最近傍探索にはkd-treeを用いています．
-
出力には，マッチングするまでのアニメーション(対応付けも可視化)と軌跡を表示します．
-
軌跡グラフ上では，収束時点のスキャン点群位置を用いて，総当たり的に点群同士の平均距離の分布をヒートマップ状に出力します．
使い方
READMEを参考にしてください．
中身について
- ICPの誤差関数
スキャン点群$\boldsymbol{p_i}(i=1,2...N)$と参照点群（地図側）$ \boldsymbol{q_j}(j=1,2...M)$で誤差関数$E$を定義します。スキャン点群と参照点群の対応付けは、kd-treeを用いて$\boldsymbol{p_i}$に対する最近傍の点$ \boldsymbol{q_j}$とします。$R$は回転行列、$ \boldsymbol{t}$は並進ベクトルです。意味合い的にはスキャン点群と対応付けられた参照点群の距離の二乗平均です。これを最小化します。最小化には下記で述べる３種のいずれかの最適化手法を用い、局所的な最適解を求めるたびに最近傍探索を実施します。収束判定はclassのinitの中でハードコーディングしているので適当にいじってください。
E( \boldsymbol{x}) =\frac{1}{N} \sum_{i=1}^{N}\|(R \boldsymbol{p_i}+ \boldsymbol{t}) - \boldsymbol{q_j}\|^{2}
以下は時間が合ったら記述します。
最適化手法
「これなら分かる最適化数学」や「しっかり学ぶ数理最適化」という書籍が非常にわかりやすいです。
-
最急降下法
-
ニュートン法
-
共役勾配法(CG法)
-
勾配F
-
ヘシアンH
おわりに
コードの中身は誰にも見てもらっていないので間違えてたり効率の悪い計算をしていたら申し訳ないです。良ければ改善点などを教えていただけると嬉しいです。スクリプト内は日本語でメモしていますので最適化部分の実装やICPの理解の補助となれば幸いです。
今後はNDT1スキャン分の簡単な可視化もしたいなと思っています。
参考文献
- SLAM入門 ロボットの自己位置推定と地図構築の技術、友納正裕、オーム社、2018年
- これなら分かる最適化数学 基礎原理から計算手法まで、金谷健一、共立出版株式会社、2005年
- しっかり学ぶ数理最適化 モデルからアルゴリズムまで、梅谷俊治、講談社、2020年