matchingによるマッピングとマップマッチングによる自己位置推定[SLAM]C++PCLROSSLAMROS2はじめにこんにちは。アドベントカレンダーの12日目です。遅れて申し訳ありません。次回は@youtalkさんの「QoSプロファイルの実践と課題」です。matchingによるマップマッチングによる自己位置推定のコードがあったので、それを基に勉強がてらマッピングとマップマッチングによる自己位置推定のコードを書いて動かしました!ちなみに、ROS2で2D-SLAMはROBOTIS社のロボットturtlebot3のROS2チュートリアルで実機・simulationともに簡単にできます。cartographerを使っています。これによる3D-SLAMの移植はされていません。http://emanual.robotis.com/docs/en/platform/turtlebot3/overview/また、gmappingも移植している人がいて、その移植版を使っている人(リンク先Qiita記事)もいるようです。https://github.com/Project-MANAS/slam_gmapping他にもslam_toolbox等があります。matchingについて詳しくは以下を参照ください。2018年5月27日https://www.slideshare.net/YukiKitsukawa/ndt-13dpfn-2018527説明は一番上のgraizegrazieさんさんのブログ記事から引用します。スキャンマッチングの説明。NDT[1]とは、2つの点群のマッチングをさせる手法の1つです。点群マッチングとは、ある点群(参照点群と呼ぶ)に対し、ある点群(入力点群と呼ぶ)がどれだけ重なっているか、重ねるためには点群をどの様に移動させるべきかを判断するための手法です。NDTの説明これに対してNDTは、ボクセルやグリッドを使って点群が存在する空間を等間隔に分割するし、ボクセル又はグリッド毎に求めた正規分布を使って点群の表現、マッチングを行うため、計算量を削減することができます。pclのndtのソースは以下です。ソースに参考文献も書いてあります。https://github.com/PointCloudLibrary/pcl/blob/master/registration/include/pcl/registration/impl/ndt.hppソースプログラムは以下のソースを参考に書きました。https://github.com/OUXT-Polaris/pcl_apps書いたコードはGithubにあります。マッピングhttps://github.com/rsasaki0109/pcl_apps/tree/master/pcl_apps/src/mappingマッチング(こっちは基のコードを微修正しただけ)https://github.com/rsasaki0109/pcl_apps/tree/master/pcl_apps/src/matching実験マッピング設定ndtのresolutionは5.0(屋外環境での値の目安は5~10mくらい)、https://gitlab.com/autowarefoundation/autoware.ai/core_perception/blob/master/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp#L116入力点群のvoxel_gridのサイズは以下を参考に0.2にしました。https://gitlab.com/autowarefoundation/autoware.ai/core_perception/blob/master/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp#L121実行手順rosbagデータはAutowareのROS1のdemoデータを使います。ROS2でROS1のbagデータを再生するにはrosbag2に加え、rosbag2_bag_v2をインストールする必要があります。SLAMと言ったらvelodyneが有名ですが、velodyneのROS2ドライバーは現状以下の様らしいです。runでパラメータを渡せないので面倒です。eloquentだとできるようになります)位置姿勢初期値を指定します。--oncerosbag再生。結果mappingした結果がこちらです。大丈夫そうですね!マップマッチングによる自己位置推定マップマッチングは、自己位置推定パッケージmcl_3dlのデモデータを使って自己位置推定します。以下を参考にndtのresolutionは0.2,入力点群のvoxel_gridのサイズは0.1にしました。ndt_matching.launch.py位置姿勢初期値を指定します。実行すると、以下のようになります。(ちょっと微妙な気が)・・・まあまあですね!今回はとりあえずここまでとします。upLoginユーザーは見つかりませんでした。