Autowareの紹介と課題（1/2）

# はじめに
はじめまして！
画像データが増えすぎて書けなくなったので、急遽15日のアドベントカレンダーに参加しました。2回にわけます。

12月15日は、オープンソース自動運転ソフトウェア [**Autoware**](https://github.com/CPFL/Autoware)についての紹介をします。
Autowareは基本的に、PCさえあれば開発できます。
国産のオープンソースで日本人開発者も多くいます。日本語で質問ことできるは、気が楽くですし、すごくいい機会だと思うので、是非Autowareの開発に携わっていただければと思います（いつでもウェルカム状態です）。
興味があれば、メールを下さい
メール先などの情報：http://tier4.jp/en/students.php
個人的に、Autoware搭載ラジコンを開発しているので需要があればオープンハードウェアにします。
![u5199u771f 2016-12-05 23 10 32.jpg](https://qiita-image-store.s3.amazonaws.com/0/131558/d0c45674-62e8-d44e-7ffd-e8986f96eab3.jpeg)



twitter:[@yukky_saito](https://twitter.com/yukky_saito)
## Autowareとは
AutowareはLinuxとROSをベースとした自動運転システム用オープンソースソフトウェアです。レーザレーダ、カメラ、GNSSなどの環境センサを利用して、自車位置や周囲物体を認識しながら、カーナビから与えられたルート上を自律走行できます。

ただし、Autowareを利用すれば完全な自動運転が行えるわけではありません。自動運転自体まだまだ開発段階であり、どんなアルゴリズムが良いなどはわかっておらず、試行錯誤な状態です。そのためAutowareでは、自動運転開発プラットフォームとしてのスタンスを取っており、様々なアルゴリズムを取り入れ、選択できるようにしています。
**参考動画**

1. [動画](https://youtu.be/NWNpkBpdmAc)
1. [動画](https://youtu.be/LZTCDbcjIdw)
1. [動画](https://youtu.be/8dM7cwOFwaI)

##基本機能
Autowareには自律走行するための下記の基本機能を備えています。

- 3D Localization
- 3D Mapping
- Path Planning
- Path Following
- Accel/Brake/Steering Control
- Data Logging
- Car/Pedestrian/Object Detection
- Traffic Signal Detection
- Traffic Light Recognition
- Lane Detection
- Object Tracking
- Sensor Calibration
- Sensor Fusion
- Cloud-oriented Maps
- Connected Automation
- Smartphone Navigation
- Software Simulation
- Virtual Reality

# Autowareのセットアップ

## 環境・要求
### Spec Recommendation
公式のハードウェア要求は以下のようになっていますが、少々オーバースペックです。メモリに関してはマップ生成にメモリが食われるため普段の仕様であれば16GBあれば足りると思います。

- CPU cores: 8
- RAM size: 32GB
- Storage size: 30GB

### Requirements
ソフトウェア環境については、以下の環境を強くオススメします。

- Ubuntu 14.04
- ROS indigo
- OpenCV 2.4.10 or higher
- Qt 5.2.1 or higher
- CUDA

### Sensors
利用する機能にもよりけりですが、基本的に下記のセンサーが必要です。
3D LiDARはvelodyneが一番実績があります。

- 3D LiDAR
- Camera

## 環境セットアップ
ROSindigoをインストール済みとして話を進めます。
参考（[インストールスクリプト](https://github.com/NavIV/automation-scripts)があります）

下記のコマンドで必須パッケージをインストールします。

    sudo apt-get -y install ros-indigo-desktop-full ros-indigo-nmea-msgs ros-indigo-nmea-navsat-driver ros-indigo-sound-play ros-indigo-jsk-visualization
    sudo apt-get -y install libnlopt-dev freeglut3-dev qtbase5-dev libqt5opengl5-dev libssh2-1-dev libarmadillo-dev libpcap-dev gksu

ここからは、必須ではないですが、インストールしたほうが良いです。

GPUがある場合、CUDAを別途インストールしてください。現状では障害物検知の画像処理でCUDAを利用しています。インストールスクリプトを利用してもらってもいいですが、GPUに合うバージョンのCUDAじゃない可能性があるので注意してください

OpenCVはROS indigoのデフォルトのバージョンが低いため、別途インストールする必要があります。インストールスクリプトを利用したほうが良いと思います。OpenCV 2.4.10以上でないと画像での障害物トラッキングが動作しません。


また、Autoware内の一部の機能（SLAM）でOpenMPを使って並列化しているものがあり、並列化を有効にするには、下記のコマンドでPCLをバージョンアップする必要があります。
※ROS indigoではPCL1.7.1ですが、PCL1.7.2以上にする必要があります

    sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
    sudo apt-get update
    sudo apt-get dist-upgrade
    echo "export LIBRARY_PATH=/usr/lib/OpenNI2/Drivers:$LIBRARY_PATH" >> ~/.bashrc
    source ~/.bashrc

##Autowareのビルド
ビルドには数分かかります。（スワップ領域を含めメモリ8GBないと落ちる可能性あり？）

    cd $HOME
    git clone https://github.com/CPFL/Autoware.git
    cd ~/Autoware/ros/src
    source /opt/ros/indigo/setup.bash
    catkin_init_workspace
    cd ../
    ./catkin_make_release

#Autoware動作
ここから多少踏み込んだ話になってくるので、ROSの知識ゼロからは厳しいかもしれません。
Autowareでは、GUIでマウスのクリックで操作できるようになっています。
GUIソフトウェア（通称、**Runtime manager**）を起動するには、

    cd ~/Autoware/ros/
    ./run

で起動します。以降の章での話は、Runtime managerが起動した状態での操作の説明になります。
全体の流れは、

1. 準備
2. 自己位置推定
3. 障害物検知
4. 信号認識
5. グローバルパスプランニング
6. ローカルパスプランニング
7. 経路追従

になります。
追記（**重要**）：以降の動画で、Runtime managerからはじめにROSBAGを一瞬再生する操作がありますがシミュレーションクロックを有効にするためです。ROSBAGを利用する場合ははじめにこれをしないとtfがおかしくなります。上手く行かないときはtf関連の可能性が高いです。


##準備
準備として、

- 3Dマップ
- キャリブレーションファイル
 - カメラの内部パラメータ、LiDARとカメラの外部パラメータ（回転行列と並進ベクトルのアフィン変換行列）
- ベクターマップ（オプション）

が必要です。
Autowareには3Dマップ、キャリブレーションファイルを作成する機能が備わっています。

ちなみに、Githubのページにセンサデータを含んだROSBAG（プライバシの関係で画像データは省いています。プライバシの取扱に理解がある方はメールをいただければ提供できるかもしれません）を含め、上記のもの一式提供しています。Githubのデータを利用する場合は準備セクションは読み飛ばしていただいても構いません。
https://github.com/CPFL/Autoware#sample-data

※オープンのベクターマップの作成ツールは開発中です。現状は、アイサンテクノロジー株式会社に依頼して作成したいただいております。

### 3Dマップ作成
3Dマップ作成にはPointCloud2形式のpoints_rawトピックをパブリッシュするものがあればOKです。PointCloud2形式とかよくわからない人はvelodyneやhokuyo3Dといったセンサーがあれば大丈夫と思ってください。
Runtime managerのSensingタブの左側にAutoware対応しているセンサドライバ一覧があるのでチェックを入れればドライバノードが立ち上がります。
![sensing_tab.png](https://qiita-image-store.s3.amazonaws.com/0/131558/bd8c6052-28d0-d8e7-d7e5-7e4bd1830d66.png)

また、マップを作成するのには、リアルタイム処理するより、一度ROSBAGに落とし込んでマップを作成するほうが良いと思います。
PointCloud2形式のpoints_rawトピックの入ったROSBAGからマップを作成する手順は次の動画になります。

※ROSBAGを使わずセンサからのリアルタイムデータを使う場合は、動画0:14~0:20の作業を飛ばしてください
[![hsX4HX_XBM4](http://img.youtube.com/vi/hsX4HX_XBM4/0.jpg)](http://www.youtube.com/watch?v=hsX4HX_XBM4)
※動画1:37辺りの、Filter ResolutionはVoxel grid filterでダウンサンプリングするときのVoxelサイズ（単位：メートル）です。
※loop closureは未実装（2016/12/15現在）

### キャリブレーションファイル作成
今回は、カメラの内部パラメータ、LiDARとカメラの外部パラメータのキャリブレーションを想定します。
マニュアルと操作動画を貼っておきます
結構コツがあるので慣れないと難しいです。大体50~100枚程度繰り返さないと良い精度のキャリブレーションは行なえません。
マニュアル：https://github.com/CPFL/Autoware/blob/master/ros/src/sensing/fusion/packages/calibration_camera_lidar/CalibrationToolkit_Manual.pdf

[![pfBmfgHf6zg](http://img.youtube.com/vi/pfBmfgHf6zg/0.jpg)](http://www.youtube.com/watch?v=pfBmfgHf6zg)

手動で外部パラメータを微調節できれば良いのですが、現状その機能はありません（誰かつくってください）

### ベクターマップ作成
絶賛、開発中

## 自己位置推定
自動運転を行うのに最も基本となる機能で、自己位置推定の失敗=自律走行不可だと思ってください。
超重要技術です。
現在、自己位置推定を行うには大きく2種類あります。
NDT slamとvisual slamです。
今回のslamは簡単に言うと、自己位置推定に使われる技術で3Dマップとスキャンデータで取れたまわりの形状をマッチングしてピッタリ合うところが自分の位置だと認識するものです。
詳しくは、次のサイトを参考に

- http://myenigma.hatenablog.com/entry/20100803/1280762612
- https://staff.aist.go.jp/y-ichisugi/brain-archi/20140422YokozukaMasashi.pdf
- http://www.pccluster.org/ja/event/%E5%8A%A0%E8%97%A4%E5%85%88%E7%94%9F_%E7%99%BA%E8%A1%A8%E8%B3%87%E6%96%99.pdf

### NDT slam
NDT slamは3D mapをvoxelに区切り、正規分布で近似します。その結果、精度を下げること無く、計算コストをかなり削減できます。
![image](https://qiita-image-store.s3.amazonaws.com/0/131558/33165b59-e08b-38ab-fc80-4db3b21ca5b8.png)
引用: http://www.pccluster.org/ja/event/%E5%8A%A0%E8%97%A4%E5%85%88%E7%94%9F_%E7%99%BA%E8%A1%A8%E8%B3%87%E6%96%99.pdf

※Autowareでは、PCLで実装されているNDT slamを利用していますが、最近、名古屋大学の竹内先生が実装したNDT slam（ndt_matching_tku）も追加されました。

NDT slamを行うためにはまず、マップを読み込む必要があります。
マップの読み込み手順は次の動画になります。
[![EJa4PHnjdRY](http://img.youtube.com/vi/EJa4PHnjdRY/0.jpg)](http://www.youtube.com/watch?v=EJa4PHnjdRY)
※動画0:44のtfはframe_idのworldとmapの座標関係です。

マップ読み込み後、スキャンデータとマッチングさせるのですが、スキャンデータの全ての点群を使うと計算コストが大きすぎます。
その為、前処理としてダウンサンプリングする必要があります。
ダウンサンプリングにはVoxel grid filterを利用し、voxelサイズは1.0～2.0mにするのが良いです。
理由として、LiDARの性質上、近くに障害物（車など）が来た場合、多くの点群が障害物にあたってしまいます。すると、障害物の影響がかなり大きくなり、自己位置推定に失敗しやすくなります。Voxel grid filterを利用すると空間をvoxelに区切り、voxel内では点群は1つのみになります。結果として、障害物に点群が引っ張られることがなくなりロバスト性を大きく向上させることにも繋がります（むしろこっちがメイン？）。

これらを踏まえて、自己位置推定をするまでの動画を貼り付けておきます。
[![ODlxMzGTJzw](http://img.youtube.com/vi/ODlxMzGTJzw/0.jpg)](http://www.youtube.com/watch?v=ODlxMzGTJzw)
※追記：動画0:20辺りのtfはbase_link（車体）とvelodyneの位置関係になります。Vehicle modelはRvizで車体を表示させるためのノード立ち上げです。

動画の途中で次のようなウィンドウがあります。検討しないといけないパラメータは、
Initial pose or GNSS、Predict Pose、Use OpenMPです。
![ndt_matching.png](https://qiita-image-store.s3.amazonaws.com/0/131558/99e0da56-dcaf-fa50-3e40-d86e39bb0799.png)
Initial pose or GNSSは初期位置の座標を指定してあげるか、GNSS（RTK-GPS）から取るかの指定です。基本的には、Initial poseを適当な数値で与え、Rvizの2D pose estimateで与えてあげるのが良いです。自己位置推定が失敗した場合も2D pose estimateで与えてあげればそこから自己位置推定を開始します。
Predict Poseは毎回スキャンごとにslamを行う際、開始位置を前回の自己位置推定結果から行うか、線形補間をして予測したところから行うかの選択です。経験的に線形補間を行うと回転時に弱い気ですが、車両速度が早い場合に力を発揮している気がします。
Use OpenMPは並列化して実行するかどうかです。基本的にはチェックしてください。


### Visual slam
Visual slamは画像ベースで行うslamです。
有名所に、LSD slamやORB slamなどがあります。
AutowareではORB slamを利用しています。最近開発されたばかりなので詳しく利用方法はわかっていませんが、デモ動画を貼っておきます。
分かり次第、追記します。
[![2x_KvXACAgc](http://img.youtube.com/vi/2x_KvXACAgc/0.jpg)](http://www.youtube.com/watch?v=2x_KvXACAgc)


## 障害物検知
障害物検知には現在大きく以下の2種類あります。

- LiDARとカメラのセンサーフュージョン
- LiDARベースの障害物検知

それぞれについて説明していきます。

### LiDARとカメラのセンサーフュージョン
構成は次のようになっています。
まだまだ、改善の余地があります。（3次元でトラッキングしたほうが良さげなど）
![detection.png](https://qiita-image-store.s3.amazonaws.com/0/131558/5a0804eb-ea24-4b47-470b-8a28a522de19.png)

これらのノードはComputingタブから立ち上げます。
![Screenshot from 2016-12-15 22-27-54.png](https://qiita-image-store.s3.amazonaws.com/0/131558/c336bb95-ac20-8278-ae5f-acac58da75b8.png)
左側のDetectionをおもに操作します。
構成図との対応関係を示しながら説明します

- 画像処理：dpm_ocv・dpm_ttic・rcnn_msr・ssd_unc

ssd_uncが圧倒的な性能を誇っているのでssd_uncを利用することをオススメします。
SSDのインストール方法：https://github.com/CPFL/Autoware/blob/master/ros/src/computing/perception/detection/packages/cv_tracker/nodes/ssd/README.md
動画：
[![EjamMJjkjBA](http://img.youtube.com/vi/EjamMJjkjBA/0.jpg)](http://www.youtube.com/watch?v=EjamMJjkjBA)

- 座標・投影変換：Points Image・Virtual Image・Scan Image

これらはSensingタブにあります。これらの利用にはキャリブレーションファイルをCaliblation Publisherより選択し、起動する必要があります。
Points ImageかVirtual Imageを選択することをオススメします。
※Virtual scanの参考論文：http://ieeexplore.ieee.org/document/7535461/
![sensing_tab.png](https://qiita-image-store.s3.amazonaws.com/0/131558/bd8c6052-28d0-d8e7-d7e5-7e4bd1830d66.png)

- トラッキング：klt_track・kf_track
- 距離推定：range_fusion

距離推定後の動画
[![E8t2X2ZWNg4](http://img.youtube.com/vi/E8t2X2ZWNg4/0.jpg)](http://www.youtube.com/watch?v=E8t2X2ZWNg4)

- 逆投影：obj_reproj
- クラスタリング：euclidean_cluster
- クラスタ割当：obj_fusion

最終的には、bounding_boxesというトピックでBoungingBoxArray形式でパブリッシュされています（もしかしたら開発中で、obj_poseという名前でMakerArrayかも？）
※obj_fusionが形状推定した結果を流さず、重心位置のみ投げている可能性があります（未確認）

### LiDARベースの障害物検知
点群から地面を削除し、クラスタリングを行い、指定範囲枠にある点群クラスタを障害物とする手法です。
今後、点群クラスタをSVMで学習し、歩行者・車などの見分けを行い、パーティクルフィルタでトラッキングします（開発中）。
おそらく、動画を見る限りUber、Googleもこの手法を使ってます（たぶん）。
参考（Uber）：
[![YKQ-6YnrKNc](http://img.youtube.com/vi/YKQ-6YnrKNc/0.jpg)](http://www.youtube.com/watch?v=YKQ-6YnrKNc)
参考（Google）：
[![tiwVMrTLUWg](http://img.youtube.com/vi/tiwVMrTLUWg/0.jpg)](http://www.youtube.com/watch?v=tiwVMrTLUWg)

まだ開発段階ですが、地面を省きクラスタリングを行い、指定範囲枠にある点群クラスタを障害物とするところまでは出来ているので動画を上げておきます。
[![Tma2DKMxt4Y](http://img.youtube.com/vi/Tma2DKMxt4Y/0.jpg)](http://www.youtube.com/watch?v=Tma2DKMxt4Y)
ただし、SVMで学習器を使う以上、velodyne hdl-32専用とかhdl-64専用とかになり汎用的に使えない可能性があります。

## 信号認識
信号検知はベクターマップ（点群じゃない白線や信号などを含んだベクトル形式の地図）を利用します。
ベクターマップで正確な信号の位置がわかるため、画像上のどの辺りに信号があるのかが判断できます。
画像上の信号が映る範囲のみを探索し信号の色を検知します。
かなり精度が良いです。
使い方は次の動画を参考にしてください。
[![KmOdBms9r2w](http://img.youtube.com/vi/KmOdBms9r2w/0.jpg)](http://www.youtube.com/watch?v=KmOdBms9r2w)

# 続きについて
画像等の関係で2MB上限に引っかかりました。
続きは[23日](http://qiita.com/yukkysaito/items/98a39dc4b842d7135ce2
)に！
では