お盆休みを終えて．．．．

前回のブログ更新からちょうど一カ月．．．
さぼっていたわけでなく，割と作業をしてたんですが，，，，，行き詰まりました．

ROSのパッケージをなるべく使いまわすべく，下記を目標にやってました．

1. ステレオの結果をつかって， Local Map を生成．この Local Map に対して，仮想的なレーザースキャンを生成する．

2. 視差画像を鳥瞰図変換して，このLocalMapに対して仮想的なレーザースキャンを生成する．

が，やっぱり既存の amcl, gslam mapping をほとんど変更なしで使うのは無理そうです．
ステレオカメラと2Dレーザースキャナの長所と短所が完全に正反対で，ステレオカメラをレーザースキャンに変更した時点でステレオの長所を全部殺してしまってるみたいです．
ひょっとしたら gslam mapping のパラメータを変更して微調整をすればうまくいくかもしれないのですが，どうも筋が悪そうな気がしてたまらなくなってきたので別の攻め方で行くことにしました．

ステレオカメラ：
長所：画像情報なので，情報リッチ．3次元情報が取得できる．
短所：検知物体がカメラから離れるにしたがって，距離精度が距離の4乗（2乗？）に反比例して下がる．視野角が狭い（レーザスキャナに比べると）．

2Dレーザスキャナ：
長所：検知物体からの距離が大きくなっても，ステレオほど急激に精度が劣化しない．視野角が広い．
短所：2Dスキャンなので，情報が限られている．一定の高さ以上の物体しかみつけられない．

下記，ちょっと自分の勉強もかねて整理したので備忘録代わりに残しておきます．

上記，ステレオの原理を簡単に図示したんですが，「左カメラ」「右カメラ」って書いてある下の長方形がカメラのイメージャー（画像素子）です．
実際には焦点の後ろに撮像素子があるのですが，前に書いても説明は成り立つので簡単のためにそう書いてます．

ステレオカメラで距離を取得するまでのステップを簡単に書くと，，

1. それぞれのカメラ（左カメラ，右カメラ）でほぼ同じ景色を撮影．

左カメラと右カメラで景色を取るわけですが，二つのカメラの距離が微妙に違う（横方向に20cm位）ので，景色の見え方が微妙に違います．

2. 左カメラで撮影した映像が，右カメラで見るとどの位置に来るかマッチングする．

例えば，湘南の海岸に行って映像を撮影したとして，左カメラで写した映像では左上から数えて横800pix，縦400pixのところにきれいなビキニのおねーちゃんが写っていたとして，
右カメラで撮った映像で同じきれいなおねーちゃんを探していくと，横600pix，縦400pixのとことかにあるわけです．
男がやると，このビキニのおねーちゃんの位置探しは絶対間違えないと思いますが，実際には題材はビキニのおねーちゃんではないことと（残念！），
めっちゃ早く（一秒間に10枚の映像ペア）対応するところを見つけないといけないので，この「左画像と右画像の対応点探し」をするアルゴリズムが必要になります．
これが世に言われる「ステレオマッチング」という問題で，この対応点をいかに正確に早く探せるかという研究が結構さかんに行われていました．
僕の場合は，「SGBM」というアルゴリズムを使って対応点を探してます．

3. 2でマッチングを取ることによって，左カメラ・右カメラに移っている同じ被写体の画像位置がわかるので，あとは上記の三角形の相似から距離を求める．

2のステップで対応点を見つけたら，あとは上記のスライドに書いてある三角形の相似の関係から，対応点までの距離がわかります．
ちなみに，スライドで書いてあるそれぞれの数字は，，，

B：左カメラ・右カメラの距離
cx, cy : カメラの主点座標
disp : 左カメラ・右カメラで写っているx座標の位置の差

です．で，ここで disp を使って距離を求めているわけですが，disp はピクセル座標の差なので，基本的に整数になります．
ビキニのおねーちゃんがあまりに遠くにいてしまって，左右の映像で写っている位置が同じになってしまうと disp = 0 になってしまってもはや距離を求めるすべがなくなってしまいます．
また，おねーちゃんの距離がカメラから離れるにしたがって，映像のなかのおねーちゃんのサイズも小さくなってしまうので，disp もどんどん小さくなってしまいます．
つまり（ちょっと強引ですが），，，，被写体が離れるにしたがって，Zの値があんまり正確ではなくなってしまいます．

で，カメラから離れるにしたがって，1mの正方形がどのくらい小さくなってしまうのかを考えてみたんですが，

20m離れた地点で 1m x 1m の正方形の面積が 34pix x 34 pix になりました．で，これがカメラからの距離によって被写体が小さく映ってしまう影響で，ステレオの場合はもう一つ
カメラから離れるにしたがって，距離の精度が下がってしまうという影響があります．下記，視差と距離の関係ですが，視差が大きい時（カメラの近くに被写体がいる）場合，1pixマッチングが
ずれても誤差は数センチなんですが，視差が小さいとき（カメラの遠くに被写体がいる）場合，1pixのマッチング誤差が数mの誤差を生んでしまいます．
（下の図の例でいうと，視差 54pix のところを間違えて 53pix と判断しても誤差は 2cm ですが，視差 4pix のところを 3pix と判断すると誤差は 7m 以上になります．）

ということで，ステレオにあった進め方をする必要があり．．．いま考え中です．．．ちょっと書くのが疲れたので，また新しいポストを上げます．

ROS Nodelet, Noise Remover, Gmapping....

おはようございます．一週間更新が滞ってしまいましたが，いろいろとやってました．
先週やったこととしては，，，，

1. ROS の Nodelet 化

2. SGBM の視差マップノイズ除去部品の作成

3. depthimage_to_laserscan と gmapping を使った地図の作成

1. ROS の Nodelet 化

　まず，ROS の Nodelet 化ですが，今まで一つ一つの部品は ROSの Node として作成していたのですが，Nodeをバンバン作ってプロセスとして立ち上げると，特に画像処理回りはノード間の通信量が大きくなりすぎてしまってPCがいっぱいいっぱいになってしまうので，今までプロセスとして立ち上げていたNodeをスレッドとして立ち上げてメモリ空間を共有するようにしました．（と書くと，僕が難しいことを成し遂げたみたいですが，Nodeletを使ったので自分は大したことはしてませんが（笑））
　ただ，Nodeletを使うのが初めてだったのでちょっとはまりました．

2. SGBM の視差マップノイズ除去部品の作成

　SGBMの視差マップですが，前回のエントリで書いた通り視差画像にはどうしてもノイズが入ってしまうので，これを除去するための部品を作りました．ノイズ除去するうえで用いた仮定は，，，
「視差は画面下から上に向かうにつれて，必ず小さくなっていく．」
「地表を最低面とする」
　結局，環境の三次元構造をとらえる必要はなく，地面からのモノの生え際がわかればいいのでこれでもいいかと．結果としては，下記のようになりました．写真を見てもわかるとおり，木の構造が地面から生える一本の柱みたいになってます．

ノイズ除去前の視差画像

ノイズ除去後の視差画像

3. depthimage_to_laserscan と gmapping を使った地図の作成

　で，最もやりたかった gmapping を使った Map の作成ですが，結果的には全然ダメでした（笑）やっぱりレーザースキャナとステレオ視差だとモデルが全然異なるので，もっと工夫しないと成立しないですね．自分が使っているステレオカメラの視野角がかなり広いので，10m位離れてしまうと距離の精度が50-100cm位になってしまって，全然きれいなマップができませんでした．

　うーん．やっぱりなかなかストレートにはいかないですね．．．ただ，SLAMのコードを自作するのはつらいので，何とかステレオからレーザースキャナの出力を生成する方法を考えてみます．今思いついているものとしては，

1. ステレオの結果をつかって， Local Map を生成．この Local Map に対して，仮想的なレーザースキャンを生成する．

2. 視差画像を鳥瞰図変換して，このLocalMapに対して仮想的なレーザースキャンを生成する．

　ということで，今週は LocalMap2LaserScan の作成に取り組みます．ということで，仕事行ってきます！

システム構成とお盆休みまでのTODO

もう七月も折り返し地点ですね．．時間ばっかりガンガン過ぎていきます．．．

目標にしていた試走会一回目の確認走行突破，残念ながらクリアできませんでしたが，どうやって次の試走会までに愛犬を調教すべきか考えていました．
いろいろ論文をみたりアイデアが出てくると，どうしてもアルゴリズムを作ってみたくなるんですが，自分のスキルだと論文からアルゴリズムを実装して使えるようになるまでにおそらく半年近くかかってしまうので，どう考えても間に合わないんですよね．．．ということで，やっぱり初心にもどって，最大限ROSのパッケージを使う方向で行きたいと思います．ROSのNavigation Stackをそのまま使ったとして，あと何を作らないといけないか，書き出してみました．

上の図で，灰色の箱はROSのパッケージとしてすでに提供されているもの．緑の箱は自分が作ったもの（ラッパー程度しか作ってないものも含む．）．黄色の箱はこれから作らいないといけないものです．
で，方針としてはステレオカメラの出力をLaserScanに変換して，SlamGMapping と AMCLパッケージを使います．視差情報からそこそこのレベルで LaserScanが生成できればいいのですが，おそらくノイズ対策をうまくしなければ使えないと思うので，下記の部品を作ります．

Noise Remover

SGBMの出力ですが，どうしてもノイズが多いのでこれをSuppressします．

Local Map 2 Laser Scan

この部品で，視差情報をLaserScanに変換します．視差情報からLaserScanを生成してくれる部品はすでに存在するのですが，

github.com

一度この部品を使ってマップ生成・自己位置推定をやってみます．これで精度が出ればいいんですが，おそらく難しいと思うので，そしたらちょっと考えます．

Visual Odometry

ヨーレートセンサーの積分だとどうしてもドリフトの影響が避けられないので，モノカメラを使ってヨー角を算出します．Visual Odometryで計算したヨー角とヨーレートセンサーの積分値を組み合わせて，できる限り正確なオドメトリを作ります．

Way Point Publisher

ターゲット座標を都度都度出力します．システムは発行された座標を目指して進行し，ターゲット座標に一定以上近づいたら，Way Point Publisherが新たな座標を発行します．

で，天王山のお盆休みまであと一か月を切りましたが，お盆休みまでに上記の作業を終わらせたいです．となると，，，

7/17 - 7/23 : Noise Remover の作成，depthimage_to_laserscan，slamgmapping を使ってマップ生成．
7/24 - 7/30 : Local Map 2 Laser Scanの検討・作成．
7/31 - 8/6 : Visual Odometryの作成．
8/7 - 8/10 : Waypoint Publisherの作成

うーん．こんなに順調にはいかないだろうな．．

お買い物

つくばチャレンジに参加して今年で３年目になるんですが，毎回つくばに行くたびに思っていたことがありました．

「電源がない！」

屋外でロボットを走らせるというコンテストなのですが，当然ながらロボットの調整をするバックヤードも屋外で，電気を確保する手段がありませんでした．
で，どうしていたかというと，ロボットを抱えて最寄りの駐車場に往復してエンジンかけてシガーから電気を取って．．．．ということを繰り返していたんですが，時間はかかるわ，暑いわで，，，，思い切って購入することにしました．そうです．発電機です（笑）ついでに，この機会にテントとか机とかも買うことにしました．地面に座って作業してると蟻も寄ってくるし，火蟻かもしんないし．．．

大体ほかの参加チームは大学の研究室なので，発電機とかテントとか必要なものって代々引き継がれているものがあるんですよね．個人参加だと当然このあたりのものがないので，買いそろえないといけなくて，出費が止まりません．

インバータ発電機とガソリン一斗缶

工進 インバーター発電機 (定格出力0.9kVA) GV-9i

まさか，プライベートでガソリンスタンドに行って一斗缶にガソリン入れてもらうことになるとは．．．

ガレージ・ゼロ ガソリン携行缶 5L [赤/横型/亜鉛メッキ鋼板/消防法適合品](GZKK01)

テントとおもり

FIELDOOR 組立て簡単!! 2.0ｘ2.0m 軽量アルミタープテント グリーン G03 (高耐水加工＆シルバーコーティング/UVカットコーティング/高さ6段階調整)

エリッゼ(ELLISSE) マルチウェイト（ウェイトバッグ）10L 4個セット バンドテープ付 持ち運びに便利なバッグ付 ワンタッチタープの重しに最適！

机と椅子

クイックキャンプ 折りたたみ テーブル 120×60cm アウトドア イベント用 ホワイト AL2FT-120

山善(YAMAZEN) 折りたたみチェア シルバー/ブラックYZX-08SB

土曜日に届いたんですが，やっぱり買ったもの届くとテンション上がりますね．思わず部屋の中でテントを広げてしまいました．

ということで，ロボット以外はだいぶ整ってきました！あと4カ月弱．頑張ります！！！

近所の実験場

前回のポストからちょっと日が開いてしまいました．．．7/8日，つくばチャレンジ説明会＆試走会一回目参加してきました．
結論としては，，，ボロボロでした（笑）やっぱりいろいろギリギリでやりすぎててる部分が多く，会場に行って一発勝負しても負けますね（笑）
結局今回はロボットの自律走行は全然無理だったので，とりあえずプレステのコントローラでロボットを動かせるようにして，それでデータを取ってくる予定だったのですが，
現地（つくば）でどうもパソコンの調子が悪くなり，結局最後の15分くらいしか有効活用できませんでした．

自分のロボットは結構小さいほうなので，モニターをつけずに別のノートPCからWIFIでリモートアクセスして使ってたんですが，どうやらPCのグラフィックボードがモニターを認識しなければまともに稼働せず，激遅になって話になりませんでした．．．事前にチェックしとけばよかったんですが，，，あとの祭りですね．一応レンタカーにPCのモニターとかを持参していたので，会場で不具合が発生するたびに駐車場に戻って車のシガーから電源を取って．という作業を繰り返しました．もう汗だくです．でも，同じような問題に直面している人ってやっぱりいるもんですね．AmazonでモニターをつないでいるようにFakeするためのアダプター？が売られていたので，さっそく購入しました．下記の部品つけると，PCがモニターつながってると錯覚して，うまいこと動いてくれました．

CompuLab fit-Headless フィットヘッドレス ヘッドレスPC用ディスプレイエミュレーター 正規輸入品

で，ほかのチームからすると当たり前のことなんですが，やっぱり近所に実験場を作って実際に動かしまくるしかないですね．でも，問題はその場所があんまりないことと，周りの目がちょっと恥ずかしいんで控えてたんですが，もう恥ずかしいとか言ってられる状況じゃないので近所で実験場を探すことにしました．

で，見つけました．

近所の月出松公園！とかっていったら住んでるとこモロバレですが（笑），ええ，近所の公園なんです．
まずは公園の赤で線を引いたところの自律走行を目指したいと思います．早速今週末にロボットを持って行ってデータを取ってきます！
あと，試走会一回目を終えて，いろいろなものが欲しくなり，またまたいろいろ無駄金を投資してしまったのですがそれは明日ポストします．

複数 PC に分散したノードの ROS を使った通信

Jetsonを導入したことで，ロボットにくっついているPCが二つになりました．
今までもマイコンは使っていたのですが，マイコンとPCはシリアル通信だったので複数のPCに散らばったROSノードをくっつけるという作業をするのは今回が初めてになります．きっとはまるんだろうな．．．と思っていたのですが，ROSのチュートリアルを読みながら進めると意外とすんなりと行きました．

ROS/NetworkSetup - ROS Wiki

ROS/Tutorials/MultipleMachines - ROS Wiki

具体的には，添付のスライドのJetsonとINTELPCをつなぐ部分に当たります．

で、簡単にできたんですが、ちょっとメモって置かないと忘れそうだったので備忘録を残しておきます。

0．　ネットワーク環境のセットアップ

それぞれのPCに固定IPアドレスを割り振ります。/etc/network/interfacesを上書きする方法しか知らなかったのですが、これ超めんどくさいので、GUIでなんとかならないかなと思って調べてたんですが、できました。あと、WiredのポートをROSの通信に使って、Wirelessの通信でインターネットにつなげたかったのですが、これもどうにか解決しました。画面右上のネットワーク関連のマークを右クリックして、そこからGUIで設定していけば完結しました。

上記のNetwork Connections画面でAddしていく感じですが、対象のボードのMACアドレスがEthernetのDeviceコンボボックスから選べるので、ここで選択します。で、次にIPV4設定タブに移って、MethodをManualにして、設定したいIPアドレスを追加します。このときに、AddressとNetmaskだけ指定してGatewayは空白にして設定を完了します。これで固定IPのWiredとDHCPのWirelessが共存してネットも繋がるようになりました。ちなみに、これは正しいかわかんないんですが、/etc/network/interfacesの記述は全部コメントアウトしました。

１．SSH環境のセットアップ

おそらく、マスタのPCからスレーブにログインしていろいろと操作をする形になるのかなと思います。なので、リモートログインできるようにスレーブ側にSSHを入れておきます。いれて再起動すれば、ポートを開けて待っててくれました。

sudo apt-get install openssh-server

2．ホスト名の解決

IPアドレスで毎回入れるのも辛いので、それぞれのパソコンにホスト名解決のためにIPアドレス・ホスト名の対を記述します。/etc/hostsファイルにｌ，０で決めたIPアドレスとそのアドレスのPCの名前の対を記述します。

10.0.0.10 master_pc
10.0.0.20 slave_pc

みたいな感じです。ここで登録しとけば、sshでつなぐ時とかにもtab補完で教えてくれます。

3. PINGで通信の確認。

マスターからスレーブ、スレーブからマスタへPINGをうって、通信確認。

4. ROSノードの起動

複数PCがあっても、ROSのマスタはひとつ出なければいけないので、マスタとなるPC上でroscoreします。

マスタ側でやること。

roscore実行

roscore

システム上で、どいつがマスタか宣言、あと自分のIPをROSに伝える。

export ROS_MASTER_URI=http://master_pc:11311
export ROS_IP=10.0.0.10
roslaunch hogehogemaster.launch

スレーブ側でやること。（SSHでリモートログインすると仮定してます。）

sshログイン

ssh xxxx@slave_pc
(パスワード入力)
export ROS_MASTER_URI=http://master_pc:11311
export ROS_IP=10.0.0.20
roslaunch hogehogeslave.launch

5. Jetsonでステレオの視差計算を実施して、マスタで描画。

ということで，ひとまずできました．上記添付写真を見てもわかる通り，JETSON使うとステレオの視差計算が自分のPCでやるよりだいぶ早くなりました！
大体10FPS位出てます．能力的にはまだ余裕ありそうなので，どこまでをJETSONの役割にするか，考えてみます．JETSONの性能とか，細かいとこはまた後日エントリで書きます．
うーん．やっぱりやること多すぎで間に合わないな．．．

NVIDIA Jetson Tx2 + Ubuntu 16.04 + アサヒドライプレミアム

月末のプレミアムフライデー，定時退社日と酒を飲むにはもってこいの状況だったのですが，来週には試走会一回目があり．．．
かえってひたすら作業に没頭すべきかどうか，非常に難しい判断を強いられました．結果的に，ビールが飲みたくて仕方なく，のみ「ながら」にしました（笑）
朝のエントリで書いたと思うのですが，NVIDIAのJetsonTx2を買って画処理をそれでやろうと思っているのですが，その設定で時間がかかりそうだったので，
少々酔っててもいいいかと思い．
（先日，一人ロボットの孤独に耐えかねて，Facebookでブログのリンクを公開したこともあり，だいぶゆるゆるの雰囲気になってると思いますが，今後はこんな感じで行きます（笑））

で，NVIDIAのJetsonですが，自腹で買いました．10万円なり．．．やっぱいろいろと金かかりますね．．．

Jetpackの最新版3.0を入れて，ホストPCからJetsonにフラッシュしました．Jetson自体は Ubuntu 16.04で動くのですが，HostPCはUbuntu14.04での動作保証しかしていないらしく，なんていけてないんだと思ったのですが，Ubuntu16.04でも大丈夫でした．Jetpackから最新のバイナリイメージをフラッシュして，今ROSをインストールしてます．

で，Jetsonとは何ぞや？という話なんですが，いまDeepLearningのブームもあって並列計算を手軽に実現できるハードウェア？がオタクの間では大流行してます．
何かの仕事（A, B, C）をしないといけないときに，Aを片付けてからBを，Bを片付けてからCをという順番でないと片付かない仕事なら順番にやっていくしかないと思うのですが，仕事の種類によってはAの結果がBには関係なく，Bの結果もCには関係なくみたいなことって結構あると思います．（まさに画像処理とかDeepLearningがそんな感じだとおもうんですが．）この場合，同時にいっぱい仕事ができたほうが早くおわりますよね？こんな感じで，互いの仕事の結果に依存関係がない場合，できる限り同時進行できる数を増やしたほうが早く終わると思うんですが，このJetson，その同時進行できる数が普通のパソコンよりけた違いに多いんです．

もともと，NVIDIA自体はパソコンのグラフィックスを表示するための計算処理ハードウェアを作ってた会社（だと思うんですが）だったんですが，どうやらこのグラフィックス処理のハードウェアというのもいかに並列に処理できる数を増やすか的なことにポイントがあるらしく，今はグラフィックスというよりいろんなとこに進出してます．車載の世界では，Toyota, Audi, BoschもNVIDIAのDrive PXというハードを使って自動運転を実現するロードマップを描いていて，車載の分野で存在感が抜群に上がってきてます．

monoist.atmarkit.co.jp

で，このDrive PXですが，個人で買うと200万くらいします（笑）かつ，電気をいっぱい食うので大きなバッテリーが必要で，僕のロボットに乗せると確実にタイヤがつぶれます．で，その廉価版というか，もっと小規模なロボット用に作られたものが Jetson になります．これを今回買いました．こいつにステレオの視差計算とかビジュアルオドメトリの計算とかをさせて，何とか5FPS位出ればハッピーかななんて思ってます．一応，普通のPCっぽくも動くんですが，フラッシュした後，JetsonでUbuntuを立ち上げたのが下記の写真になります．

今晩ステレオの視差計算をJetsonでできるとこまで行けばいいんですが，，，そこは作業のはかどり具合と酔いの回りの勝負ですね．

今後の人のために，二つだけ
１．JetpackはUbuntu16.04のホストPCからでもフラッシュできる．
２．Jetpackのフラッシュは若干相性があり，うまくいかないことがある．自分の場合は，手持ちのノートPCでどうしてもうまくいかなかったので，デスクトップでやったらすんなりいきました．

ということで，ステレオを動かすとこまで行けば，またポストします．