Graph SLAM(実装編)

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

<table border="0">
<tr>
<td><img width="300" height="150" src="https://qiita-image-store.s3.amazonaws.com/0/239786/ab1893aa-4a31-93ba-154f-4c5ae070619d.jpeg"></td>
<td><img width="300" height="150" src="https://qiita-image-store.s3.amazonaws.com/0/239786/54b9aaaf-3420-cf71-b8a3-2cd2874f95fc.jpeg"></td>
<td><img width="300" height="150" src="https://qiita-image-store.s3.amazonaws.com/0/239786/2baf4be6-8b1e-c989-b244-c48ed2717eb6.jpeg"></td>
</tr>
</table>


# 環境
- 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パッケージをダウンロードします。

```bash:catkin_workspace
$ git clone https://github.com/Sadaku1993/graph_slam
```

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

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

```bash:graph_slam
$ ./setup.bash
```

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

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

g2oには以下のパッケージなどが必要です。
[g2oのセットアップ](https://github.com/Sadaku1993/g2o)のところにも書いてあるので、確認してみてください。

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

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

```bash: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のパッケージをダウンロードする必要があります。

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

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

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

## Bagデータ
[明治大学ロボット工学研究室が公開しているbagデータ](https://drive.google.com/open?id=1VEy_iJZKEGcNDDKsK-YrqgKxwy6qsric)をダウンロードします。
この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として保存してください。

```bash
graph_slam/
  data/
    bagfiles/
      infant_outdoor.bag
```

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

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

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

保存するデータは
1. Nodeの絶対座標 (初期位置を基準とする)
2. Nodeで取得した点群

```bash
$ roslaunch graph_slam data_save.launch
```

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

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

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

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

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

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

```bash
$ 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とした場合、作成された３次元地図は以下に示す階層に保存されます。

```bash
graph_slam/
  data/
    Map/
      odometry.pcd
```

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

```bash:graph_slam/data/map
$ pcl_viewer odometry.pcd
```

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

<p align="center">
<img width=400 alt="odometry.jpg" src="https://qiita-image-store.s3.amazonaws.com/0/239786/1c01fec9-9065-b9d5-6b3b-c90e0a8b191a.jpeg">
</p>

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

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

```bash
$ roslaunch graph_slam node_edge.launch
```

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

```bash
graph_slam/
  data/
    csv/
      gicp.csv
```

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

```bash
$ 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を用いて三次元地図を可視化します。

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

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

<img width="400" alt="gicp.jpg" src="https://qiita-image-store.s3.amazonaws.com/0/239786/8275f557-6ed6-087b-f338-b7a5b16ec5ea.jpeg">

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

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

```bash
$ roslaunch graph_slam slam3d.launch
```

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

```bash
graph_slam/
  data/
    csv/
      0.csv
      1.csv
      :
      :
```
再訪判定と最適化を行った回数分csvファイルが作成されます。

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

```bash
$ 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としています。

```bash:graph_slam/data/map
$ pcl_viewer 4.pcd
```

<img width="400" alt="g2o.jpg" src="https://qiita-image-store.s3.amazonaws.com/0/239786/ab1893aa-4a31-93ba-154f-4c5ae070619d.jpeg">


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

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