カルマンフィルタを使ったノイズ除去
で,現在行き詰まり中のロボット作成ですが,何とかステレオカメラを使ってマップを作らないといけないのでノイズの除去をする方法を検討してます.
解こうとしている問題は,,,
1.ステレオカメラで取得した視差情報を3次元の点群に変換.
2.3次元の点群を地面に投影して,地面に切ったグリッドの高さを求める.
3.地面に切ったグリッドの高さ,もしくは地面の傾きが一定以上大きい箇所は通行不可能なエリアとしてマーキングする.
で,結果的にはマップを「通行可能」「通行不可能」の二通りの値に分類して,これをマップとして生成します.
このためには,地面に切ったグリッドのセル高さを求めないといけないのですが,ステレオの視差情報はノイズが大きく,これをどうやって扱えばいいものかとずっと悩んでました.
上図はカメラと地面に切ったグリッドの簡易図ですが,青の三角形がカメラでその前のマス目が地面に定義したグリッドです.ステレオの視差情報から生成したポイントクラウドは上記のセルのどこかに属する形になりますが,このポイントクラウド一つ一つの高さ情報を使ってマス目一つの高さ情報を定義します.(マス目一つ一つが高さ情報を持っているイメージです.)
ただ,ステレオの視差情報はノイズによる影響が大きいので,同じマス目なのに高さが1.5mの時もあれば,2.5メートルとなってしまう時もあります.今回は,このノイズを取るために1次元のカルマンフィルタを使ってみることにしました.そんなにカルマンフィルタには詳しくなかったのでGoogle先生にいろいろと教えてもらって勉強しました.一番わかりやすくまとめられてたのが,下記のサイトです.
https://logics-of-blue.com/kalman-filter-concept/
で,上記サイトのサンプルプログラムを参考に,自分の問題にあてはめて Python コードを作ってみました.
問題設定は,,,,
1.ロボットが前方に動いて,新たなセルが視界に入ってくる.新たなセルの高さの真値は1mとする.
2.新たなセルであるため,当然高さ情報はなし.つまり初期値を 0m としてはじめる.
3.ノイズがいっぱい乗っているステレオの情報を繰り返し使って,真値の1mに近づける.
という問題設定です.ちなみに,カルマンフィルタを選択した時点でステレオノイズに含まれている誤差が平均値0の白色ノイズという前提になってしまうのですが,今回取り除きたいノイズがこのモデルに従うかどうかはわかりません...
下記 Python のサンプルコードです.
import numpy as np from numpy.random import * import matplotlib.pyplot as plt def Kalman(y, xK1, pK1, sigmaW, sigmaV): # Predict State Variable xK = xK1 # Update Sigma Variable pK = pK1 + sigmaW # Update Kalman Gain kGain = pK / (pK + sigmaV) # Filter State and Variable xFilt = xK + kGain * (y - xK) pFilt = (1 - kGain) * pK return xFilt, pFilt def CreateFilteredData(measData, initPredVar, sigmaW, sigmaV): length = len(measData) filteredMeasData = np.zeros(length + 1) filteredPredVar = np.zeros(length + 1) filteredPredVar[0] = initPredVar for i in range(0, length): xFilt, pFilt = Kalman(measData[i], filteredMeasData[i], filteredPredVar[i], sigmaW, sigmaV) filteredMeasData[i + 1] = xFilt filteredPredVar[i + 1] = pFilt return np.delete(filteredMeasData, 0), np.delete(filteredPredVar, 0) def CreateSampleData(height, var, size): indexes = [idx for idx in range(0, size)] heightListWithNoise = np.random.normal(height, var, size) return indexes, heightListWithNoise if __name__ == '__main__': print("Sample Kalman Filter started.") height = 1.0 var = 0.5 size = 100 indexes, data = CreateSampleData(height, var, size) filteredData, filteredVars = CreateFilteredData(data, 10000, 100, 10000) plt.plot(indexes, data) plt.plot(indexes, filteredData) #plt.plot(indexes, filteredVars) plt.show()
上記コードを実行した結果ですが,下記のようになりました.
縦軸がステレオの視差ベースで求めた高さのイメージです.横軸は単純にサイクルカウントです.図を見てわかるように,青の値が純粋な入力値であり,ノイズの影響でだいぶブレブレになってますが,フィルタをかけたあとの緑の値は 1m 付近をあるていどキープしているように見えます.ということで,取りあえずカルマンフィルタを使って高さ方向のノイズ除去をし,その値を用いてマップ作成をしてみます!結果はできれば明日にアップします!
お盆休みを終えて....
前回のブログ更新からちょうど一カ月...
さぼっていたわけでなく,割と作業をしてたんですが,,,,,行き詰まりました.
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を生成してくれる部品はすでに存在するのですが,
一度この部品を使ってマップ生成・自己位置推定をやってみます.これで精度が出ればいいんですが,おそらく難しいと思うので,そしたらちょっと考えます.
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の作成
うーん.こんなに順調にはいかないだろうな..
お買い物
つくばチャレンジに参加して今年で3年目になるんですが,毎回つくばに行くたびに思っていたことがありました.
「電源がない!」
屋外でロボットを走らせるというコンテストなのですが,当然ながらロボットの調整をするバックヤードも屋外で,電気を確保する手段がありませんでした.
で,どうしていたかというと,ロボットを抱えて最寄りの駐車場に往復してエンジンかけてシガーから電気を取って....ということを繰り返していたんですが,時間はかかるわ,暑いわで,,,,思い切って購入することにしました.そうです.発電機です(笑)ついでに,この機会にテントとか机とかも買うことにしました.地面に座って作業してると蟻も寄ってくるし,火蟻かもしんないし...
大体ほかの参加チームは大学の研究室なので,発電機とかテントとか必要なものって代々引き継がれているものがあるんですよね.個人参加だと当然このあたりのものがないので,買いそろえないといけなくて,出費が止まりません.
インバータ発電機とガソリン一斗缶
工進 インバーター発電機 (定格出力0.9kVA) GV-9i
- 出版社/メーカー: 工進 (KOSHIN)
- メディア: Tools & Hardware
- この商品を含むブログを見る
まさか,プライベートでガソリンスタンドに行って一斗缶にガソリン入れてもらうことになるとは...
ガレージ・ゼロ ガソリン携行缶 5L [赤/横型/亜鉛メッキ鋼板/消防法適合品](GZKK01)
- 出版社/メーカー: 株式会社 ガレージ・ゼロ
- メディア:
- この商品を含むブログを見る
テントとおもり
FIELDOOR 組立て簡単!! 2.0x2.0m 軽量アルミタープテント グリーン G03 (高耐水加工&シルバーコーティング/UVカットコーティング/高さ6段階調整)
- 出版社/メーカー: コンポジット
- メディア: その他
- この商品を含むブログを見る
エリッゼ(ELLISSE) マルチウェイト(ウェイトバッグ)10L 4個セット バンドテープ付 持ち運びに便利なバッグ付 ワンタッチタープの重しに最適!
- 出版社/メーカー: エリッゼ(ELLISSE)
- メディア: その他
- この商品を含むブログを見る
机と椅子
クイックキャンプ 折りたたみ テーブル 120×60cm アウトドア イベント用 ホワイト AL2FT-120
- 出版社/メーカー: eSPORTSオリジナル
- メディア: その他
- この商品を含むブログを見る
山善(YAMAZEN) 折りたたみチェア シルバー/ブラックYZX-08SB
- 出版社/メーカー: 山善(YAMAZEN)
- メディア: ホーム&キッチン
- この商品を含むブログを見る
土曜日に届いたんですが,やっぱり買ったもの届くとテンション上がりますね.思わず部屋の中でテントを広げてしまいました.
ということで,ロボット以外はだいぶ整ってきました!あと4カ月弱.頑張ります!!!
近所の実験場
前回のポストからちょっと日が開いてしまいました...7/8日,つくばチャレンジ説明会&試走会一回目参加してきました.
結論としては,,,ボロボロでした(笑)やっぱりいろいろギリギリでやりすぎててる部分が多く,会場に行って一発勝負しても負けますね(笑)
結局今回はロボットの自律走行は全然無理だったので,とりあえずプレステのコントローラでロボットを動かせるようにして,それでデータを取ってくる予定だったのですが,
現地(つくば)でどうもパソコンの調子が悪くなり,結局最後の15分くらいしか有効活用できませんでした.
自分のロボットは結構小さいほうなので,モニターをつけずに別のノートPCからWIFIでリモートアクセスして使ってたんですが,どうやらPCのグラフィックボードがモニターを認識しなければまともに稼働せず,激遅になって話になりませんでした...事前にチェックしとけばよかったんですが,,,あとの祭りですね.一応レンタカーにPCのモニターとかを持参していたので,会場で不具合が発生するたびに駐車場に戻って車のシガーから電源を取って.という作業を繰り返しました.もう汗だくです.でも,同じような問題に直面している人ってやっぱりいるもんですね.AmazonでモニターをつないでいるようにFakeするためのアダプター?が売られていたので,さっそく購入しました.下記の部品つけると,PCがモニターつながってると錯覚して,うまいこと動いてくれました.
CompuLab fit-Headless フィットヘッドレス ヘッドレスPC用ディスプレイエミュレーター 正規輸入品
- 出版社/メーカー: CompuLab
- メディア: Personal Computers
- この商品を含むブログを見る
で,ほかのチームからすると当たり前のことなんですが,やっぱり近所に実験場を作って実際に動かしまくるしかないですね.でも,問題はその場所があんまりないことと,周りの目がちょっと恥ずかしいんで控えてたんですが,もう恥ずかしいとか言ってられる状況じゃないので近所で実験場を探すことにしました.
で,見つけました.
近所の月出松公園!とかっていったら住んでるとこモロバレですが(笑),ええ,近所の公園なんです.
まずは公園の赤で線を引いたところの自律走行を目指したいと思います.早速今週末にロボットを持って行ってデータを取ってきます!
あと,試走会一回目を終えて,いろいろなものが欲しくなり,またまたいろいろ無駄金を投資してしまったのですがそれは明日ポストします.
複数 PC に分散したノードの ROS を使った通信
Jetsonを導入したことで,ロボットにくっついているPCが二つになりました.
今までもマイコンは使っていたのですが,マイコンとPCはシリアル通信だったので複数のPCに散らばったROSノードをくっつけるという作業をするのは今回が初めてになります.きっとはまるんだろうな...と思っていたのですが,ROSのチュートリアルを読みながら進めると意外とすんなりと行きました.
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の記述は全部コメントアウトしました。
1.SSH環境のセットアップ
おそらく、マスタのPCからスレーブにログインしていろいろと操作をする形になるのかなと思います。なので、リモートログインできるようにスレーブ側にSSHを入れておきます。いれて再起動すれば、ポートを開けて待っててくれました。
sudo apt-get install openssh-server
2.ホスト名の解決
IPアドレスで毎回入れるのも辛いので、それぞれのパソコンにホスト名解決のためにIPアドレス・ホスト名の対を記述します。/etc/hostsファイルにl,0で決めたIPアドレスとそのアドレスのPCの名前の対を記述します。
10.0.0.10 master_pc 10.0.0.20 slave_pc
みたいな感じです。ここで登録しとけば、sshでつなぐ時とかにもtab補完で教えてくれます。
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
5. Jetsonでステレオの視差計算を実施して、マスタで描画。
ということで,ひとまずできました.上記添付写真を見てもわかる通り,JETSON使うとステレオの視差計算が自分のPCでやるよりだいぶ早くなりました!
大体10FPS位出てます.能力的にはまだ余裕ありそうなので,どこまでをJETSONの役割にするか,考えてみます.JETSONの性能とか,細かいとこはまた後日エントリで書きます.
うーん.やっぱりやること多すぎで間に合わないな...