テンプレートマッチング with CUDA

昨晩の深酒のせいで，昼まで寝てしまい．．．．掃除・洗濯もろもろしたらもう17時．．．．
9/23に試走会2回目があるのに，この感じだと間に合わないなあと言ってても始まらないので，タイトルどおり今からテンプレートマッチングの部品を作ります．
0から作るのはしんどいので，OpenCVのCUDA実装をほぼコピーして必要な個所だけをアレンジすることにしました．

完成したらエントリ更新します．

ーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーー

上記投稿からはや４日．．．テンプレートマッチング試してみました．
CUDAの実装方法をちょっと復習したり（若干モチベーションが上がらなかったり．．．）してたので，思ったより時間がかかってしまいました．
上記テンプレートマッチングですが，なんとかうまく行きそうな気がしてます．

カートを押して，大清水公園のマップ（下記）を作成します．

で，もう一回大清水公園をカートを押してマップを作りながら，いま事前に作っていた地図の中でどこを走っているかを調べます（自己位置推定）

実際には，赤の正方形（10m x 10m）の中心にロボットがいると仮定します．で，赤の正方形だけを切り出したものが下記です．

で，上記の正方形領域をもともと作っていた地図に対して照らしあわせて，一番結果がマッチしたところにロボットがいると判断します．
そのマッチング結果が下記になります．一番白が強い箇所が一番マッチが強い箇所です．

下記のマッチング結果はCUDA実装の一部を変更した自作バージョンのマッチングスコア

下記のマッチング結果はCUDA実装にもともとあるTM_CCOEFFを使ってマッチングしたスコア

CUDA実装にあるテンプレートマッチング方式のほうがいい感じにマッチングスコアが出たのが意外でしたが，灰色（Unknown）領域ではマッチングしたくないので，ここは少し手を入れる必要があると思います．あと，実際にはオドメトリからロボットが走っている位置はだいたいわかっているので，マップ全体に対してテンプレートマッチングする必要はなく，実際には今ロボットがいると思っている位置±3m，±5degくらいでマッチングをして一番スコアが高い位置にロボットがいると判定し，そこから経路を引いてロボットをすすめる．という流れになるかと思います．ひとまず，土曜までには自己位置推定する部品を完成させ，日曜には「近所の実験場」で実際にロボットを走らせて見たいのですが，，，，また状況アップします．

ステレオカメラを用いた OGM の作成３ @ 大清水公園

というわけで，ちょっと時間がかかってしまいましたが，大清水公園のマップを作成しました．
実際には OGM というよりも，TGM?とでもいうべきなのでしょうか？マップは対象のグリッドが通行可能かどうか「Traversable or not」の条件で作ってあります．

具体的には，グリッド間の高さの差が20cm以上ある箇所は通行不可能として黒色に，そうでない場合は通行可能であるとして白色に，検知範囲に入っておらずわからない場所は灰色に表示しています．

まず，グリッドの高さを濃淡で表したマップが下記です．

一部緑になってしまってますが，これは変換ミスです．
一応，同じ日に3回走行したデータを用いてマップを作っているのですが，見た感じ再現性がありそうなので自律走行に使えるのではないか．．と思ってます．
灰色の箇所はUnknownということでマッチングに使用せず，白と黒の領域だけを使ってテンプレートマッチングを実施してマップ内部でどこにロボットが存在するか判定しようと考えています．具体的には，，，

事前にとっておいた地図を下記とします．

で，ロボットが自律走行しているときに作成している地図を下記とします．

赤のエリアをロボットの周辺10m x 10mの正方形とします．この正方形の位置を事前にとっておいた地図の中で探します．
具体的には，事前にとっておいた地図の領域に対して，赤の正方形でテンプレートマッチングを実施して一番スコアが高い箇所を現在の位置とします．
ステレオノイズの影響もあって，地図自体がまだ完璧なレベルではないですが，時間がもうないので，ひとまず現状のものを使って実験してみます．
ということで，今週末はテンプレートマッピングの部品を作ります！

カルマンフィルタを使ったノイズ除去

で，現在行き詰まり中のロボット作成ですが，何とかステレオカメラを使ってマップを作らないといけないのでノイズの除去をする方法を検討してます．
解こうとしている問題は，，，

１．ステレオカメラで取得した視差情報を３次元の点群に変換．
２．３次元の点群を地面に投影して，地面に切ったグリッドの高さを求める．
３．地面に切ったグリッドの高さ，もしくは地面の傾きが一定以上大きい箇所は通行不可能なエリアとしてマーキングする．

で，結果的にはマップを「通行可能」「通行不可能」の二通りの値に分類して，これをマップとして生成します．
このためには，地面に切ったグリッドのセル高さを求めないといけないのですが，ステレオの視差情報はノイズが大きく，これをどうやって扱えばいいものかとずっと悩んでました．

上図はカメラと地面に切ったグリッドの簡易図ですが，青の三角形がカメラでその前のマス目が地面に定義したグリッドです．ステレオの視差情報から生成したポイントクラウドは上記のセルのどこかに属する形になりますが，このポイントクラウド一つ一つの高さ情報を使ってマス目一つの高さ情報を定義します．（マス目一つ一つが高さ情報を持っているイメージです．）

ただ，ステレオの視差情報はノイズによる影響が大きいので，同じマス目なのに高さが１．５ｍの時もあれば，２．５メートルとなってしまう時もあります．今回は，このノイズを取るために１次元のカルマンフィルタを使ってみることにしました．そんなにカルマンフィルタには詳しくなかったのでGoogle先生にいろいろと教えてもらって勉強しました．一番わかりやすくまとめられてたのが，下記のサイトです．

https://logics-of-blue.com/kalman-filter-concept/

で，上記サイトのサンプルプログラムを参考に，自分の問題にあてはめて Python コードを作ってみました．
問題設定は，，，，
1.ロボットが前方に動いて，新たなセルが視界に入ってくる．新たなセルの高さの真値は１mとする．
2．新たなセルであるため，当然高さ情報はなし．つまり初期値を 0m としてはじめる．
３．ノイズがいっぱい乗っているステレオの情報を繰り返し使って，真値の１mに近づける．

という問題設定です．ちなみに，カルマンフィルタを選択した時点でステレオノイズに含まれている誤差が平均値０の白色ノイズという前提になってしまうのですが，今回取り除きたいノイズがこのモデルに従うかどうかはわかりません．．．

下記 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を生成してくれる部品はすでに存在するのですが，

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カ月弱．頑張ります！！！