こちらのサイトで公開されていたICPアルゴリズムのPythonのサンプルを動かしてみました.
https://myenigma.hatenablog.com/entry/20140617/1402971928
サンプルは元の点群と,回転並進を加えた点群とのマッチングをとるものでしたが,
自分の作りたいものに合わせてちょっと改変.
点群に誤差を加えて,点群の順番を入れ替えて,それぞれの点群でマッチングしない点を作っった後に,
マッチングが取れる点だけを探して使って,2つの点群のマッチングするプログラム.
以下のスクリーンの写真だと最初に赤と青を5ポイント誤差を加えて生成,順番の入れ替え.
赤と青の1ポイントずつを大きく飛ばしてマッチング取れない点を作り,
マッチングが取れる残り3ポイントの並びを検出して,3ポイントだけをICPアルゴリズムでマッチングを取って,回転と並進の結果を得る,というものです.
次は,レーザーレンジで孤立的な障害物を抽出して点群作成と,
マッチングによりマップ作成を作ります.
その動画→
ICPマッチング,例外除去あり