この記事では以下のような三次元地図をROSを用いて構築していきます。

環境

Ubuntu16.04
ROS(Kinetic)
g2o
PCL(1.8)

セットアップ

Ubuntu(16.04)、ROS(Kinetic)、PCL(1.8)はセットアップ済みとします。
PCLは1.7以上であれば問題なく動作すると思います。
g2oは最新のものではないため、私のgit-hubからcloneしてコンパイルを行います。
最新のもので扱いたい場合は適宜変更してください。

ダウンロード

graph_slam

catkin workspace内にgraph_slamパッケージをダウンロードします。

catkin_workspace

$ git clone https://github.com/Sadaku1993/graph_slam

graph_slamのパッケージ内でg2oをライブラリとして使用するため、
g2oのセットアップの後にこのパッケージをコンパイルします。

g2o

次にg2oをダウンロードし、コンパイルを行います。
まず、graph_slamパッケージ内にあるsetup.bashを起動します。

graph_slam

$ ./setup.bash

ThridParty, dataなどのフォルダが作成されます。
以下のような構成になっているか確認してください。

ThirdParty内にてg2oのセットアップを行います。

g2oには以下のパッケージなどが必要です。
g2oのセットアップのところにも書いてあるので、確認してみてください。

cmake
libeigen3-dev
libsuitesparse-dev
qtdeclarative5-dev
qt5-qmake
libqglviewer-dev

ダウンロードが完了したらg2oをダウンロードし、コンパイルします。

graph_slam/ThirdParty

$ git clone https://github.com/Sadaku1993/g2o
$ cd g2o
$ mkdir build
$ cd build
$ cmake .. -DBUILD_CSPARSE=ON
$ make
$ sudo make install

g2oのコンパイルが成功したらgraph_slamのパッケージをコンパイルしてください。

Velodyne

今回使用するbagデータはVelodyneを使用しているため、
velodyneのパッケージをダウンロードする必要があります。

catkin_workspace

$ git clone https://github.com/ros-drivers/velodyne
$ roscd && catkin_make

Normal Estimation

Node-Edge情報を算出するのに、今回はGICPを使用するため、点群に法線情報を付与する必要があります。
velodyne用の法線推定packageをダウンロードします。

catkin_workspace

$ git clone https://github.com/amslabtech/perfect_velodyne
$ roscd && catkin_make

Bagデータ

明治大学ロボット工学研究室が公開しているbagデータをダウンロードします。
このbagデータは明治大学生田キャンパスD館にて取得されたものです。
データは

Odometry (geometry_msgs/Odoemtry)
Imu (sensor_msgs/Imu)
PointCloud (velodyne_msgs/VelocyneScan)
にて構成されています。

なお点群はVelodyne HDL-32eを使用しています。
ダウンロードしたbagデータはgraph_slamパッケージ内にあるdata/bagfiles内にinfant_outdoor.bagとして保存してください。

graph_slam/
  data/
    bagfiles/
      infant_outdoor.bag

実装

今回はGraph SLAMにより明治大学生田キャンパスD館の三次元地図を作成します。
4ステップに分けて行います。

Gyro Odometryを元にデータを保存
GICPによりNode-Edgeの情報を取得
Graph OptimizationとLoop ClosingによりNodeとEdgeの情報を修正
修正したNode-Edge情報から三次元地図を作成

データを保存

Imuとwheel encoderからgyro odometryを算出し、ある程度移動を確認できたらその地点を新たなノードとして保存していきます。

保存するデータは

Nodeの絶対座標 (初期位置を基準とする)
Nodeで取得した点群

$ roslaunch graph_slam data_save.launch

データは以下の階層に保存されます。

graph_slam/
  data/
    csv/
      odometry.csv
    pcd/
      0.pcd
      1.pcd
        :

CSVには各ノードの座標、PCDには各ノードで取得した点群が保存されています。
点群データ内には人などの障害物があるため、クラスタリングを用いて除去します。
(すいません、ここのパラメータが適当なため検出精度が低いです。)

$ roslaunch graph_slam remove_cluster.launch

データは以下の階層に保存されます。

graph_slam/
  data/
    remove/
      0.pcd
      1.pcd
        :
        :

ここでOdometryを利用して３次元地図を作成してみましょう。
CSV使用するCSVファイルと保存するPCDファイル名を指定してください。

$ roslaunch graph_slam integrator.launch
  :
  :
CSV file list 
  odometry.csv

Please serect csv file : odometry.csv
Please enter map file : odometry.pcd

保存するPCDファイル名をodometry.pcdとした場合、作成された３次元地図は以下に示す階層に保存されます。

graph_slam/
  data/
    Map/
      odometry.pcd

pcl_viewerを利用してOdometryを用いた場合の３次元地図の結果を見てみましょう。

graph_slam/data/map

$ pcl_viewer odometry.pcd

とても崩れていることがわかります。
オドメトリにはスリップ誤差などが積算しているからです。
ここからSLAMにより、最適なグラフ構造を算出していきます。

GICPによりNode-Edge情報を取得

オドメトリにより作成した３次元地図はとても崩れていたことがわかったと思います。せっかく点群を取得しているので各ノードで取得した点群を用いてオドメトリよりも正確な相対関係を算出することができます。Node間の相対関係をGICPを用いて算出します。

GICPは２つの点群間の相対位置を算出することが可能です。
しかし、ある程度２つの点群間の相対関係がわかっている必要があります。
その相対関係を算出するためにOdometryを利用しました。

$ roslaunch graph_slam node_edge.launch

GICPにより算出したNode-Edge情報はgicp.csvとして保存されます。

graph_slam/
  data/
    csv/
      gicp.csv

GICPにより算出したNode-Edge情報から、３次元地図を作成してみます。
参照するCSVはgicp.csv、三次元地図はgicp.pcdとします。

$ roslaunch graph_slam integrator.launch
	:
	:
CSV file list 
  odometry.csv
  gicp.csv

Please serect csv file : gicp.csv
Please enter map file : gicp.pcd

pcl_viewerを用いて三次元地図を可視化します。

$ roscd graph_slam/data/map
$ pcl_viewer gicp.csvf

Odometryと比べると、かなり良くなっていることがわかります。
しかし、スタート地点とゴール地点の鉛直方向(z方向)がずれてしまっていると思います。

Graph Optimization & Loop Closing

GICPでも最適な三次元地図を作ることができないことがわかりました。

次に考えることはグラフ最適化です。
Graph SLAMにおいて最も重要な部分となります。
ORB SLAMなど様々な手法においてGraph最適化が利用されています。
ここでは実装がメインなので説明は省略しますが、以前訪れたことがあるであろうノードとの相対関係を算出し(再訪判定 Loop Closing) 、グラフ最適化を行うことで最適な３次元地図の構築を行います。

$ roslaunch graph_slam slam3d.launch

最適化されたNode-Edge情報は0.csv、1.csv…として以下の階層に保存されます。

graph_slam/
  data/
    csv/
      0.csv
      1.csv
      :
      :

再訪判定と最適化を行った回数分csvファイルが作成されます。

最適化されたNode-Edge情報から３次元地図を作成してみましょう。
利用したいcsvファイル名と保存するファイル名を指定してください。

$ roslaunch graph_slam integrator.launch
	:
	:
CSV file list 
  odometry.csv
  gicp.csv
  0.csv
  1.csv
  :

Please serect csv file : 4.csv
Please enter map file : 4.pcd

３次元地図は選択したcsvファイルが4.csv、保存するMap名を4.pcdとしています。

graph_slam/data/map

$ pcl_viewer 4.pcd

まとめ

この記事は研究室の後輩のためにまとめたものですが、自動運転やSLAMを始めたばかりの人でも、思ったよりも簡単に３次元地図が作れたり、SLAMができるんだなと感じていただければ幸いです。

もっとSLAMのことを知りたい人はPython Roboticsを参考にしてください。
研究室の先輩で自律移動の分野で知らない人はいないほどの神様的な存在です。
SLAM以外にもPath Planningなど様々な領域をまとめてくださっています。