RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_007日目_SLAM_MONO-VO(単眼カメラ視差推定) #C++

#◆ 前回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_006日目_Arduino+RaspberryPi+Bluetooth+シリアル通信+ROS連携　の続き

#◆ はじめに
前回はBluetoothによるロボット遠隔操作を試行した。
今回からLiDARやカメラを使ったSLAMに挑戦したい。
Qiitaやブログに関連記事を投稿してくださっている方は沢山いらっしゃるので読みあさってみたが・・・原理を細かく理解しようとした時点で負けが確定するシロモノだ。
言うなれば、何も考えずただボタンを押すだけの今時仕様から、一夜にしてサボテンやピエロを直視で目押ししてDDTしなければ確実に負けるほどの難易度に急上昇した、と言えば、自分が今置かれている状況がご理解頂けるだろうか。
※上記一文が意味不明な方々は華麗にスルーしてあげてください。

いっそのこと割りきって仕組みのことは一切考えない。
しかし・・・ワクワクしすぎて目移りする。
この際、片っ端からためしてやるぜ！ちくしょう！

#◆ 参考にさせていただいた記事、謝辞
nnn112358さん　Qiita

Crafty_as_a_Foxさん　Qiita

ROSとGazeboによる自律移動シミュレーション〜SLAMアルゴリズムの比較〜

NegativeMindさん　NegativeMindException

OpenCVでカメラ画像から自己位置認識 (Visual Odometry)

#◆ SLAMとは
これっぽっちも理解していないので、あちこちの情報を読みながら、試しつつ、迷子になりながら頭のなかを整理していく。
そもそも、

地図生成
自己位置推定
航路決定？

はそれぞれ別のルーチンで行う。
自分が今回やりたいことに対し、地図生成を自律航行ロボにリアルタイムでやらせること自体に意義は無く、出鼻から大きく勘違いしていたことが分かった。
当たり前の話だが、限られたリソースを有効活用するなら、地図を作ること、と、地図内を航行させること、は分けて考えるべきだと考えた。
探索エリアが決まっている場合は、あらかじめ探索エリア内のマップを生成しておくほうが良いような気がする。
未知のエリアを航行させるわけではないので良しとする。

ということで、第一段階は地図を作ること、に専念することとする。
で、調べた目ぼしい地図生成アルゴリズムは下記。

今回	他記事	アルゴリズム	特徴
	008日目	Gmapping	一番有名らしい、ROS対応、loop closure有り
	009日目	Google Cartographer	精度が高そう、ROS対応、loop closure有り
	010日目	Hector SLAM	精度がいまいちっぽい、loop closure無し
	011日目	Karto SLAM	単眼カメラでも動く、ROS対応、Visual-SLAM
	012日目	ORB-SLAM2	単眼カメラでも動く、Visual-SLAM
★		mono-vo	単眼カメラでも動く、Visual-SLAM
	013日目	VINS-Mono	単眼カメラでも動く、ROS対応、Visual-SLAM、ドローン
ー	ー	CNN-SLAM	単眼カメラでも動く、3D、アイデアのメモ情報しか見当たらなかったため検証対象外とする
ー	ー	RGBD-SLAM-V2	RGB-Dカメラ専用、検証対象外とする

#◆ アルゴリズムの選定に悩む

将来構想上での要件は、

無いものネダリなのは百も承知だが、RaspberryPiごときでも処理可能なほど軽量なアルゴリズムがあればうれしい
長時間の自律走行に耐えるためには電力効率を極限まで上げなくてはならない
格安LiDAR (RPLidar A1M8) は所持しているが他の要件を満たすためなら他の機材使用にシフトすることも厭わない
正確性はある程度捨てても構わないが迷子になることは避けたい
認識スピードはある程度欲しいがこだわらない

分けわかんないのはこの際しようがない。
とにかく、アルゴリズムごとに手当り次第、

1. とりあえず動かす
2. 自力で地図・オドメトリを生成してみる
3. 自作のデータで動かす

を繰り返してみる。
要件への適合性や使い勝手、パフォーマンスの結果を見てどれを選択するのかはあとから判断する。

#◆ mono-vo (OpenCV3.0ベースの単眼視差推定)
下記の情報を参照した。
実行の仕方が分からない〜！と、大騒ぎしている輩が多い。
Gitのチュートリアルが極めて不親切なので致し方ないが、C++のソース読みなよ。と思った。
結果的にはC++をまともに読んだことも書いたこともないトーシローでもだいたい勘で動かせた。

#◆ 実行環境構築
まずは味見がてら、Ubuntu16.04 上で実行環境を構築して動かしてみたい。
OpenCVはpipコマンドかソースビルドかdebパッケージであらかじめ導入済みの前提とする。
物珍しさで試してみたくなった方のために、サンプルデータのファイルサイズが22GBもあって、ダウンロードに４時間、解凍に１時間も要することを先にお伝えしておく。

$ cd ~
$ git clone https://github.com/PINTO0309/mono-vo.git
$ wget http://kitti.is.tue.mpg.de/kitti/data_odometry_gray.zip #<---データセット(グレースケール) 22GBもあるので空き容量注意!!ダウンロードだけで４時間。ZIP解凍に１時間。
$ wget http://kitti.is.tue.mpg.de/kitti/data_odometry_calib.zip #<---キャリブレーションファイル 600KB
$ wget http://kitti.is.tue.mpg.de/kitti/data_odometry_poses.zip #<---グラウンドトゥルースファイル 1.3MB
$ unzip data_odometry_gray.zip;unzip data_odometry_calib.zip;unzip data_odometry_poses.zip
$ rm data_odometry_gray.zip;rm data_odometry_calib.zip;rm data_odometry_poses.zip

homeフォルダ直下に下記のような階層が出来上がる。

可視化用C++プログラムをフォルダ階層に合わせて手直しする。

$ nano mono-vo/src/visodo.cpp

ファイルパス４行を自分の環境に適合するように修正する。

visodo.cpp

//ifstream myfile ("/home/avisingh/Datasets/KITTI_VO/00.txt");
ifstream myfile ("/home/<username>/dataset/poses/00.txt");

visodo.cpp

//sprintf(filename1, "/home/avisingh/Datasets/KITTI_VO/00/image_2/%06d.png", 0);
//sprintf(filename2, "/home/avisingh/Datasets/KITTI_VO/00/image_2/%06d.png", 1);
sprintf(filename1, "/home/<username>/dataset/sequences/00/image_0/%06d.png", 0);
sprintf(filename2, "/home/<username>/dataset/sequences/00/image_0/%06d.png", 1);

visodo.cpp

//sprintf(filename, "/home/avisingh/Datasets/KITTI_VO/00/image_2/%06d.png", numFrame);
sprintf(filename, "/home/<username>/dataset/sequences/00/image_0/%06d.png", numFrame);

プログラムをビルドする。

$ mkdir build
$ cd build
$ cmake ..
$ make

可視化プログラムを実行。

$ ./vo

うぉっ、すごい！！
動いた。
しかし、単眼カメラでこんなことができるとは！！

が、何かが違う。これじゃない。
野生の直感がそう叫んでいる。。。

ということで、自前のデータセット生成は先送りして次に行ってしまおうと思う。

#◆ 【参考】ビジュアルオドメトリ／SLAM開発キット
http://kitti.is.tue.mpg.de/kitti/devkit_odometry.zip

$ cd ~/mono-vo
$ wget http://kitti.is.tue.mpg.de/kitti/devkit_odometry.zip
$ unzip devkit_odometry.zip
$ rm devkit_odometry.zip
$ cd devkit/cpp
$ g++ -O3 -DNDEBUG -o evaluate_odometry evaluate_odometry.cpp matrix.cpp

#◆ 本日のまとめ

文系に数式は無理！吐き気がする！見た瞬間に「はわわぁ〜・・・」ってなる。
単眼カメラでは限界がある気がする。やっぱり LiDAR や IMU と組み合わせかな。
LiDAR / IMUとの合成版もサンプルが準備されているようだ。
でもやっぱり単眼カメラでもココまでできるのはビックリした。将来的な可能性を感じる。
DeepLearningと組み合わさるともっといいものが登場したりするのかな。

#◆ 次回予告
Gmapping にトライしてみようと思う。
なぜ最初に基本っぽいところから攻めなかったのか？と聞かれても返す言葉も無し。。。
オッサンの奮闘は更に続く。。。
FF7の 「更に闘う者達」 がずっと頭の中でリフレインしている。

#◆ 次回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_008日目_SLAM_GMapping_LiDAR(A1M8)　へ続く