2020-05-14

Scale Space

ということで，下記２つのエントリを経てやっとこさ Scale Space のとこまで来ました．
daily-tech.hatenablog.com
daily-tech.hatenablog.com
で，微分フィルタ及び二階微分フィルタ（Laplacian Filter）のエントリの最後で，
「スケールが 2σ 位の被写体が写っている画像に対して，標準偏差が σ のガウスフィルタと二階微分フィルタを適用すると，被写体の部分が強く反応する．」
という話をしました．このエントリではこれを実際に実験＆確認して，最後に Scale Space の定義を紹介します．

Laplacian of Gaussian Filter を適用してみる．

ここでは，実験用に下記の画像を使います．

f:id:rkoichi2001:20200514133841j:plain — 実験用のひまわり畑写真

この画像に写っているひまわりですが，大体下記のようなサイズ感になっています．

f:id:rkoichi2001:20200514163523j:plain — ひまわり畑写真 with サイズ円

ここで，赤の円が半径 45pix，青の円が半径 10pix，緑の円が半径 5pix になります．ちなみに，ひまわりは花の部分が黄色，種の部分が黒なので，円で囲ってある部分のエッジはx方向からスキャンしていくと，
「下がりエッジー＞上がりエッジ」
になります．なので，二階微分は
「下がりエッジー＞上がりエッジと上がりエッジー＞下がりエッジ」
の組み合わせになるので，「強く反応する＝円の中心付近で出力値が大きくなる」ことを意味します．

それではこの写真に対して，前回のエントリで実験した「Laplacian of Gaussian Filter」を適用して結果を見てみます．理論的にはガウシアンフィルタの標準偏差が10pix くらいになったとこで青の円が強く反応（＝明るくなって）して，45pix くらいになったとこで赤の円が強く反応（＝明るくなって）するはずですが，，，実験してみます．

σ = 1.6

f:id:rkoichi2001:20200514164631p:plain

σ = 2.2

f:id:rkoichi2001:20200514164654p:plain

σ = 3.2

f:id:rkoichi2001:20200514164708p:plain

σ = 4.5

f:id:rkoichi2001:20200514164721p:plain

σ = 6.4

f:id:rkoichi2001:20200514164735p:plain

σ = 9.0（青の円付近のひまわりが強く反応．）

f:id:rkoichi2001:20200514164749p:plain

σ = 12.8

f:id:rkoichi2001:20200514164801p:plain

σ = 18.1

f:id:rkoichi2001:20200514164818p:plain

σ = 25.6（赤の円付近のひまわりが強く反応．）

f:id:rkoichi2001:20200514164830p:plain

σ = 36.2（赤の円付近のひまわりが強く反応．）

f:id:rkoichi2001:20200514164840p:plain

σ = 51.2

f:id:rkoichi2001:20200514164852p:plain

σ = 72.4

f:id:rkoichi2001:20200514164903p:plain

ちょっと写真でいっぱいになってしまいましたが，σが大きくなるにつれて強調されるひまわりが画像遠方から近方に写ってくる様子がわかると思います．遠方はほぼ黄色（花びらの部分）と茶色（種の部分）で構成されているのに対し，近方になってくると緑色（葉や茎）の要素が出てくるため，これによってエッジ構造が複雑になり，強め合い・弱め合いが起こることによって実際のサイズとピークを取るσからの乖離が大きくなってしまいました．が，ある程度は理論に沿った動きをすることがわかりました．

Scale Spaceの定義．

上記ひまわり畑の実験を踏まえると，LoG Filter (Laplacian of Gaussian Filter) のσを変化させることで，スケールが大体 σ 位の物体・構造を検知することができるようになりました．そこで，画像の縦・横に加え，新たにσの次元を加えることによって，画像中にどんなスケールの物体が写っているかがある程度わかるようになります．下記，簡単ですがいつものポンチ絵です．

f:id:rkoichi2001:20200514174537j:plain — Scale Space のイメージ

図中にスケールがそれぞれ r1, r2, r3 である３つの物体が書かれていますが，σ を変化させて LoG filter をかけ，その反応が強くなった箇所を書き留めておくことによって，書き留めた座標 (x, y) を中心とする物体のスケールが大体 σ くらいであるということがわかるようになります．SIFTではこの原理を使って特徴点を抽出します．ということで，次は SIFT に入ります．あー．時間がかかる．．．

実験に使ったコード．

github.com

import os
import cv2
import numpy as np


def scale_space_test1():

    cur_dir = os.path.dirname(os.path.abspath(__file__))
    path = cur_dir + '/../data/himawari.jpg'
    img = cv2.imread(path)
    gray_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

    k = np.sqrt(2)
    sigma = 1.6

    # for sigma in sigmas:
    for i in range(12):
        sigma_3_radius = 10 * sigma * 2

        # Applying Gaussian filter
        gauss_kernel_1d = cv2.getGaussianKernel(
            ksize=int(sigma_3_radius), sigma=sigma)
        gauss_kernel_2d = np.outer(
            gauss_kernel_1d, gauss_kernel_1d.transpose())
        log_filter = cv2.Laplacian(gauss_kernel_2d, cv2.CV_32FC1, ksize=31)

        laplacian_of_gaussian = cv2.filter2D(
            gray_img, cv2.CV_32FC1, log_filter)

        # Normalize value for visualization.
        min_val, max_val, _, _ = cv2.minMaxLoc(laplacian_of_gaussian)
        abs_max = max(abs(min_val), abs(max_val))
        tmp = (laplacian_of_gaussian * (0.5 / (2.0 * abs_max)) + 0.5) * 255.0
        laplacian_of_gaussian_uchar = np.uint8(tmp)

        title = 'Sigma = ' + str(sigma)
        cv2.namedWindow(title)
        cv2.imshow(title, laplacian_of_gaussian_uchar)
        #cv2.imwrite(title + str('.png'), laplacian_of_gaussian_uchar)
        cv2.waitKey(0)
        cv2.destroyWindow(title)

        sigma = k * sigma


if __name__ == "__main__":

    scale_space_test1()

参考文献．

Scale-space theory: A basic tool for analyzing structures at different scales

Distinctive image features from scale-invariant keypoints

http://www.cs.unc.edu/~lazebnik/spring11/lec08_blob.pdf

2020-05-08

微分フィルタ及び二階微分フィルタ（Laplacian Filter）

前回の Gaussian Filter のエントリに引き続き，微分フィルタについてもエントリを残しておこうと思います．前回のエントリは下記の通り．
daily-tech.hatenablog.com

一階微分フィルタ・二階微分フィルタとは？

　その名の通り，隣接する画素値の変化率を計算し，その値を出力とするフィルタです．一般的に画像のエッジを取りたいときによく使われます．こちらもググればいっぱい有益な情報が出てくるので，ここでは実際に使ってみてどんな出力が得られるか見てみたいと思います．

２D画像に一階微分フィルタ・二階微分フィルタを適用する．

　手始めに，下記のサンプル画像に対して一階微分フィルタ・二階微分フィルタを適用します．下記の画像はわが母校．．．ではありませんが，東工大の大岡山キャンパスです．この画像に対して，x方向の一階微分フィルタ，ｙ方向の一階微分フィルタ，二階微分フィルタを適用してみます．

原画像

f:id:rkoichi2001:20200508132858p:plain — 大岡山キャンパス

ｘ方向の一階微分フィルタを適用
　ｘ方向に関して，上がりエッジがある場所では値が高く（白く），下がりエッジがある場所では値が低く（黒く）なっている様子がわかります．

f:id:rkoichi2001:20200508132946p:plain — 大岡山キャンパス＋ｘ方向一階微分フィルタ

ｙ方向の一階微分フィルタを適用
　ｙ方向に関して，上がりエッジがある場所では値が高く（白く），下がりエッジがある場所では値が低く（黒く）なっている様子がわかります．

f:id:rkoichi2001:20200508133017p:plain — 大岡山キャンパス＋ｙ方向一階微分フィルタ

二階微分（ラプラシアン）フィルタを適用
　二階微分フィルタの適用結果ですが，一階微分フィルタの応答とは異なり，よく見るとエッジがあるところが白黒になっていることがわかると思います．

f:id:rkoichi2001:20200508133048p:plain — 大岡山キャンパス＋二階微分（ラプラシアン）フィルタ

二階微分（ラプラシアン）フィルタの結果拡大図
　上の画像の一部拡大版ですが，二階微分（ラプラシアン）フィルタを適用すると，エッジの存在する箇所が「黒白，もしくは白黒」になっていることがわかります．

f:id:rkoichi2001:20200508133758p:plain — 二階微分（ラプラシアン）フィルタを適用すると，エッジの箇所が白黒・黒白に．

　次に，一階微分フィルタ・二階微分フィルタの動きを１D画像に適用して見てみます．

１D画像に微分フィルタを適用する．

　ここではそれぞれのフィルタの動きを見るために，問題を簡単化して１D画像で考えます．前回のエントリ同様に白帯の画像を考えます．上から順に，「原画」「原画のｘ方向断面」「ｘ方向の一階微分フィルタ出力」「二階微分フィルタの出力」となっています．

f:id:rkoichi2001:20200508134236p:plain — 一階微分フィルタ・二階微分フィルタの出力

　上記の図から，二階微分フィルタの出力がなぜエッジ付近で「黒白，もしくは白黒」になることがわかるかと思います．上がりエッジが立つと，そのエッジを達成するために一階微分（勾配）が
「０ー＞正の値ー＞０」
と変化し，この勾配を達成するために二階微分が
「０ー＞正の値ー＞負の値ー＞０」
となるためです．二階微分フィルタを適用した大岡山キャンパス画像の出力結果ともつながりました．

　ひとまず，一階微分フィルタ・二階微分フィルタの概要はこのくらいにしておきます．ここではエッジがとてもくっきりしていて，ある意味ではステップ入力のような白帯を使いました．次に，この白帯をGaussian Filter でぼかしたときの二階微分（ラプラシアン）フィルタの出力変化を見てみます．

Gaussian Filter と二階微分（ラプラシアン）フィルタの関係

　下記，原画に対して Gaussian Filter をかけたときの一階微分フィルタ，二階微分フィルタの応答です．σが大きくなるに従って勾配が緩やかになり，一階微分フィルタ，二階微分フィルタの応答も緩やかになっている様子がわかると思います．

f:id:rkoichi2001:20200508164628p:plain — σを変化させたときの微分フィルタの応答

　σの標準偏差を持つガウスフィルタを適用した場合，ステップ入力の勾配が小さく（緩やかに）なるため，一階微分・二階微分の値は小さくなっていきます．この出力だと形がわからないので，一階微分フィルタ・二階微分フィルタの出力を正規化して，どんな形になっているのか見てみます．（一階微分の出力値にσを，二階微分の出力値にσ^2をそれぞれかけてあげると正規化できます．）

f:id:rkoichi2001:20200508164659p:plain — σを変化させたときの微分フィルタの応答（正規化版）

　正規化した画像から，おもしろいことがわかります．４段目の二階微分フィルタの値を見てほしいのですが，白帯の上がりエッジ・下がりエッジにて発生した波がガウスフィルタの標準偏差（σ）を大きくしていくに従って互いに接近しはじめ．．．．σ = 16 で完全に合体しました．ちなみに，この白帯の幅は 32pix なのですが，白帯の幅のちょうど半分くらいの標準偏差を持つガウスフィルタをかけてあげると，二階微分フィルタの出力が極値を迎えることがわかりました．ということで，

「スケールが 2σ 位の被写体が写っている画像に対して，標準偏差が σ のガウスフィルタと二階微分フィルタを適用すると，被写体の部分が強く反応する．」

ことがわかります．今回は一つの画像に対して σ を変化させて微分フィルタの応答を見ましたが，この σ の方向の次元を 「Scale Space」 と呼びます．Scale Space の極値を見ることによって，（＝今回実験したように，σを変えながら二次微分フィルタの極値を探ることによって）被写体の大体のスケールがわかることになります．SIFTではキーポイント検出でこれを使っているのですが，それはまた次のエントリで．．．．

実験に使ったコード．

github.com

参考文献．

Scale-space theory: A basic tool for analyzing structures at different scales

Distinctive image features from scale-invariant keypoints

http://www.cs.unc.edu/~lazebnik/spring11/lec08_blob.pdf

2020-05-07

Gaussian Filter を用いた画像の暈し．

「SFMをやります！」と言いながらだいぶ基礎的なエントリになるのですが，

「SFMをやる！」
ー＞「特徴点はSFMの基礎だから，SIFTとAKAZEくらいは抑えておこう．」
ー＞「まてよ，Scale Space ってなんや？」

という流れで，フィルタリングを簡単にまとめておくことにしました．「SFMシリーズ！」としてエントリをまとめてみようと思ったのですが，そうするとだいぶ先まで見通してエントリを書かないといけないので，小出しに個別のエントリを書くことにしました．一通りやりきることができたら，あとでリライトします．

ということで，国際通りの３次元復元まで，道のりは遠いですね．．．

Gaussian Filter とは．

　テンプレートマッチングや機械学習するときに，画像の前処理としてフィルタリングをすることもあると思います．自分はこのあたりあまり考えず，「ちょっと画像暈したいなあ．」とかって具合に使ってたんですが（笑），Gaussian Filter には意味があったみたいです．Gaussian Filter はググればいっぱい有益な情報が出てくるので，かっちりとした話はそちらを見ていただくとして，ここではその意味合い的な部分を掘り下げられればと思います．

参考サイト１
imagingsolution.blog.fc2.com

標準偏差 (σ)と暈しの程度を考える．

　Gaussian Filterはガウスカーネルを持つフィルタなので，フィルタの設定値として標準偏差（σ）を指定します．OpenCV の関数でいうと，，，

    cv2.getGaussianKernel(ksize, sigma, ktype)

の sigma の部分です．この関数を使う上で気を付けないといけないことは，sigma を大きくした場合，それに伴って ksize の値も大きくしないといけないということです．（σが大きくなるにつれて，当該ピクセルの値を計算するために必要な隣接ピクセルの数が増えていくため．）

それでは，画像に Gaussian Filter を適用する場合にこの σ がどんな意味を持つのかちょっと考えてみます．実験として，下記画像に対してガウシアンフィルタを適用してみます．まず簡単のために，x方向だけの1Dガウシアンフィルタを使いたいので，画像はとてもシンプルにy方向には何も変化がないものにしています．

f:id:rkoichi2001:20200507123241p:plain — ガウシアンフィルタを適用する実験画像．

ちなみに，上記の実験画像は高さが61pix, 幅が501pixで，画像の中心の白帯の幅が11pix になります．次に，σを変化させたときの1Dガウシアンカーネルをプロットしてみると，下記のようになります．

f:id:rkoichi2001:20200507131433p:plain — σを変化させたときの1Dガウシアンカーネルの様子．

で，実際に実験画像に上記の1Dガウシアンカーネルを適用した結果が下記になります．ここでは「画像の中で幅11pixの領域として映っているものが，σ を変化させたときにどのようにぼかされていくか？」を見ています．左側のグラフは，右側画像をx軸に平行な断面で切って横から見た図です．

f:id:rkoichi2001:20200507131539p:plain — σを変化させたガウシアンカーネルを適用した実験画像．

上図を見るとわかると思うのですが，σ が11を超えたあたりから急速に白帯が消失してく様子がわかります．σが 11 を超えるまでは，どちらかというとエッジの部分がぼかされていたのに対して，11を超えてくると黒画像領域に白帯が溶け出してきます．ということでガウスフィルタを使うときは，

「画像のなかで暈したいスケール (pix) を決め，σをそのスケールよりも少し大きくとる」

ことをすれば，いい具合に画像をぼかせそうです．

画像にGaussian Filterを適用する．

次に，実際の2D画像に Gaussian Filter を適用してみたいと思います．ここでは実際に上の議論が成立しているかどうかを見るために，下記のような実験画像を用意しました．四角形，丸，三角形が並んでいますが，上から順に 10 x 10pix，20 x 20pix, 40 x 40pix, 80 x 80pix, 80 x 160pix の大きさで書いてます．一番下はちょっと扁平にしてみました．

f:id:rkoichi2001:20200507142214p:plain — 異なるスケールの図形を描いた実験画像．

で，この画像に対して，「それぞれの図形スケール + α」の標準偏差を持つガウスフィルタを適用してみます．ここまでの議論が正しければ，図形のスケールよりも大きい標準偏差を持つガウスフィルタを適用すると図形は消えるはず！といことで，やってみます．

σ = 5

１番スケールの小さい図形（10 x 10 pix）はだいぶ暈されてますが，まだ残ってます．

f:id:rkoichi2001:20200507143208p:plain — σ = 5 のガウスフィルタ適用

σ = 12.5

１番スケールの小さい図形（10 x 10 pix）が消えました．

f:id:rkoichi2001:20200507143942p:plain — σ = 12.5 のガウスフィルタ適用

σ = 25

２番目スケールの図形（20 x 20 pix）が消えました．

f:id:rkoichi2001:20200507144003p:plain — σ = 25 のガウスフィルタ適用

σ = 50

３番目スケールの図形（40 x 40 pix）が消えました．

f:id:rkoichi2001:20200507144026p:plain — σ = 50 のガウスフィルタ適用

σ = 100

４番目スケールの図形（80 x 80 pix）が消えました．

f:id:rkoichi2001:20200507144053p:plain — σ = 100 のガウスフィルタ適用

σ = 180

５番目スケールの図形（80 x 160 pix）が消えました．

f:id:rkoichi2001:20200507144124p:plain — σ = 180 のガウスフィルタ適用

ということで，思ったような結果になりました．ただ，暈しなので完全には消えておらず薄っすらと残ってますが，大体のスケールを決めてやれば暈しの効果をある程度操ることはできそうです．ということで，次は Laplacian FIlter と Scale Space の話をします．

実験に使ったコード．

github.com

参考文献．

Scale-space theory: A basic tool for analyzing structures at different scales

Distinctive image features from scale-invariant keypoints

http://www.cs.unc.edu/~lazebnik/spring11/lec08_blob.pdf

2020-05-02

春休み！

2020年もはや5月になってしまいましたが，，，推定5万人の僕のブログ読者の皆様（笑），元気にされてますでしょうか！！ちょっとブログの更新タイミングを逃してしまい，だいぶ間が空いてしまったんですが，令和元年初のポストです．

１．ここまでの報告

　昨年８月から始まったフリーランス生活，ここまでいい感じに来ています．といってもまだ９か月ですが，，，ちょうど先月いっぱいで１件目のお客さんの仕事を離れたのですが，現場の雰囲気が良かったこともあり，とても充実した９か月でした．

　今までのキャリア的に「R＆D畑で論文を読み漁りながらスクラッチで物をつくりまくる．」というよりは，どちらかというと「量産開発やある程度形になっている物を改編・導入」という仕事が多かったので，アルゴリズム検討＆スクラッチ開発してアウトプットが出せるかどうかビクビクでした．が，数年にわたってロボット作ったり画像処理の勉強をしてきた複利効果が表れてきたのか，一応それなりにアウトプットを出すことができ，お客さんにも喜んでもらえたと思っています．

　まだまだ修行の身ではありますが，テーマを絞って継続的に勉強なり実験なりすれば，２～３年くらいたてば実感できる程度にはスキルが上がってくるということに気づけたこともとても大きな収穫でした．

２．やりたいこと！

　実はこれからちょっと春休みします（笑）
　本当は「ハワイ or 八重山諸島」だったのですが，コロナ的にそれはとても難しそうなので，南流山で勉強＆モノづくりに勤しみます．３か月くらい春休みしようと思っているんですが，想定以上に景気が怪しくなってきたので，，，様子を見ながらちょっと短くするかもです．

　で，肝心のやりたいこと！なんですが，下記になります．いくつかは去年のブログにも書いていたと思うのですが，脳力不足のせいでどれも未完になってます．．．今年はどれか一つでも終わればいいな．．．

１．つくばチャレンジ完走！
　ことしで６回目の出場になる，「つくばチャレンジ」の完走です．が，，，コロナのせいで今年は実験走行・本走行が中止となってしまい．．．今年は完走できたはずなので（笑）とても残念ですが，2021年に備えます(｀・ω・´)ゞ　
　子分との勉強会もちょっと要調整ですね．

２．国際通りの三次元復元．
　那覇の国際通りをSFMで三次元復元したい！

３．ディープラーニングの勉強．
　自分のスキルセットに「機械学習・深層学習」がまだない状態でして．．．学習系のスキルの有り無しで受けられる仕事の数がだいぶ変わってくるので，そろそろ勉強したい．

４．数学の勉強．
　ロボティクスにしても画像処理にしても最近の論文はかなり最適化を使っていて，数学のバックグラウンドがないとあんまり理解が深まらないので，すこし遠回りかと思いつつも数学（主に線形代数・最適化）の勉強をしたいと思っています．

５．会計・金融の勉強．
　２０１９年に人生で初めて青色申告したんですが，いままで工学系のことしか勉強してこなかった自分にとってはこれがとても新鮮で．

３．２０２０年，やること！

　ということでどれも捨てがたいのですが，さすがに３ヵ月しかないので浅く広くといっても絞らざるを得ず．．．ひとまず，下記の３つにします．

・国際通りの三次元復元．
　３度目の正直です（笑）SFMやります！ただ，今すぐ沖縄には行けないので，南流山駅前でやってみて，うまくいく目途が立ったらコロナの収束具合を見て那覇にいきます．こいつは引き続きオープンソースとにらめっこして，適宜下記を読みながらやっていきます．

Multiple View Geometry

Multiple View Geometry in Computer Vision

作者:Hartley, Richard,Zisserman, Andrew
発売日: 2004/03/25
メディア: ペーパーバック

・数学の勉強．
　実は，数学に関しては年始から少しずつやり始めているんですが，下記を課題図書にしてます．もともとそんなに数学が強くないことと，中身が濃すぎてまとめ記事を書くのがすごく難しそうなんですが，このあたりってそれほどブログ記事になってるものが少ないような気がするので，できればエッセンスを抜き出して記事にしたいと思っています．（これは今年中目標です（笑））

Matrix Computation

Matrix Computations (Johns Hopkins Studies in the Mathematical Sciences)

作者:Golub, Gene H.,Van Loan, Charles F.
発売日: 2012/12/27
メディア: ハードカバー

Convex Optimization

Convex Optimization (English Edition)

作者:Boyd, Stephen,Vandenberghe, Lieven
発売日: 2004/03/08
メディア: Kindle版

Numerical Optimization

Numerical Optimization (Springer Series in Operations Research and Financial Engineering)

作者:Nocedal, Jorge,Wright, Stephen
発売日: 2006/06/01
メディア: ハードカバー

ちなみに，なんで洋書やねん ( ﾟДﾟ) って話があると思うんですが，アマゾンのレビューがすこぶるいいことと，全部ネットに pdf で落ちてるからです！

・会計・金融の勉強．
　で，もう一つ！独立っていうとなんだか大げさですが，一応青色申告をする身分になったので，会計の基礎的なことを勉強したいと思います．ただ，勉強するだけだと面白くないので，企業の決算報告書とかをある程度読めるようになります！で，真剣に株式投資をやってみます！（＜－これが真のモチベーションなのかも（笑））ということで，下記，課題図書です．

【増補改訂】財務3表一体理解法 (朝日新書)

作者:國貞克則
発売日: 2016/10/13
メディア: 新書

財務3表図解分析法 (朝日新書)

作者:國貞克則
発売日: 2016/10/13
メディア: 新書

財務3表実践活用法会計でビジネスの全体像をつかむ (朝日新書)

作者:國貞克則
発売日: 2012/07/13
メディア: 新書

会社四季報ワイド版 2020年2集春号 [雑誌]

発売日: 2020/03/16
メディア: 雑誌

投資で一番大切な20の教え　賢い投資家になるための隠れた常識

作者:ハワード・マークス
発売日: 2012/10/23
メディア: 単行本

ピーター・リンチの株で勝つ―アマの知恵でプロを出し抜け

作者:リンチ,ピーター,ロスチャイルド,ジョン
発売日: 2001/03/01
メディア: 単行本

こちらの課題図書はすべてお金を出して購入させていただきました（笑）

あとは，日経新聞のオンラインと四季報オンラインを申し込むかどうか．．．．迷い中です．
ということで，２０２０年はここから怒涛のブログ更新率を見せつけますので，５万人の読者のみなさま，どうぞよろしくおねがいしますm(__)m

2019-12-16

制約のある軌道を生成する方法〜 Constrained Trajectory Generation 〜

なかなか仰々しいタイトルになってしまいました（笑）

つくばチャレンジも終わりSFMに戻ろうかと思っていたのですが，今年のつくばチャレンジではなかなか愛犬が言うことを聞いてくれないこと（＝Plannerの重要性）を痛感し，来年のために Planner 周りの勉強をしてます．で，せっかくなのである程度広く参考文献をあさってみたのですが，なかなかこれが自分には難しく．．．どうやってまとめればいいか．．．と思っている最中なんですが，ひとまず重要そうなトピックを小出しに書いてみることにしました．そこそこ部品ができてきたら大きなまとめを作れればと思ってます．

X．はじめに

　人間が自動車を運転している状況を考えてみると，普通は進みたい軌道があると思います．例えば高速道路を走行しているときは，できれば走行レーンのど真ん中を走りたいですし，おしっこしたくなったら SA に入りたくなります．で，「走行レーンのど真ん中」とか「SAに入るためにレーンチェンジ」とかって，地球の表面を完全に x,y 平面だとすると，軌道は複数の点 (x, y) で定義されます．
　が，実際に運転しているときにこの軌道を頭のなかで計算している人はいないと思います．しかも，実際に車を操作するときに使うインプットって，アクセル（≒加速度）とハンドル（≒角速度）ですよね？つまり，「走行レーンの真ん中を走る」というのは結果でしかなくて，走行レーンの真ん中からちょっとずれたらハンドルを逆に回して走行レーンの真ん中に戻す．という操作を繰り返して走行レーンの真ん中を走っています．
　ロボットを制御するときも同じロジックで，レーンの真ん中からちょっとずれたらフィードバックして．．．という方法で制御することができます．自分は 2013年にETロボコンというコンテストに出場したことがあるのですが，このロボコンのロボットは光センサーを使ったライントレースで目的地まで進んでいきます．このライントレースがここで言うフィードバックのイメージです．ロボットは進む方向も何も全くわかっておらず，自分の直下の黒線との位置関係だけをみて制御をしています．
（下記動画の最初の部分にライントレースしながら進むロボットが出てきます．）

が，この方法だとロボットの慣性が大きくなったり，緻密な制御が求められる場合に対応できなくなります．ETロボコンでも上位チームのロボットはめちゃ早いんですが（笑），速度が上がってくるとラインを追従しきれなくなってコースアウトするロボットも出てきてしまいます．

で，この問題を解決するためには，

「目標出力（走行レーンの真ん中を走る）を満たす入力（アクセル，ハンドル）を事前に計算して，そいつを入力する」

が必要になるわけですが，これが計算できれば，得られた値を入力として使うことによって，ある程度は期待に沿った目標出力が得られます．このエントリでは，目標出力を満たす入力を事前計算する方法をまとめたいと思います．

X．問題設定

ということで，具体的な問題設定をします．

出発点の車の状態

X_start = [x_st, y_st, φ_st, v_st, θ_st]^T

到達点の車の状態

X_end = [x_end, y_end, φ_end, v_end, θ_end]^T

x : 車両の x 座標
y : 車両の y 座標
φ : 車両の向き
v : 車両の速度
θ : ステアリング角度（≒タイヤの切れ角）

上の２つがわかっている状況下で，２つの点をうまく繋いでくれる制御入力 [v, θ]^T を求めます．ここでは，車両がレーンチェンジして停車するマヌーバを考えてみます．下図の点線のラインが，おそらく車両がたどるであろうラインです．

f:id:rkoichi2001:20191207201149j:plain — サンプルの問題設定（レーンチェンジして停車）

X．処理の大きな流れ

軌道生成法の大きなながれをまずクリアにして，それから各パートに入っていきたいと思います．このアルゴリズムで解く問題設定は下記のようになります．

１．制御入力をモデル化して関数として表現する．（v = f(t), θ = g(t)）

２．モデル化関数のパラメータの初期値を決める．（v = a_vt² + b_vt + c_v, θ = a_θt² + b_θt + c_θの，係数 a, b, c のこと．）

３．各時点での制御入力をモデル化関数から求め，数値積分して最終時間での車両状態を求め，到達点からの差分を計算する．（diff = X_end - X(tf)）

４．到達点からの差分を，モデル化関数のパラメータに関して線形化してヤコビアンを計算する．

５．３で求まった目標値からの差分，４で求まったヤコビアンをもとに，ニュートン法を使って修正ベクトルを計算．

６．５で計算した修正ベクトルを使って，パラメータを更新し，３に戻る．

X．運動モデル

実際に入力から出力を計算するためには，制御対象となるロボットのモデルを考えてやる必要があります．自動車の例が一番イメージしやすいかと思ったので，二輪モデルを使って考えてみます．ここでは，入力と目標出力の関係は下記のように設定しました．

入力：速度（v），ステアリング角度（θ）
出力：車両の位置，向き（x, y, φ）

で，アクセルの場合は入力を加速度としたほうがよりイメージに近いかと思ったのですが，論文が速度入力だったので速度入力にしています．あと，ステアリング角とタイヤの切れ角は実際は違いますが，問題が複雑になるだけなので同じとしてます．

f:id:rkoichi2001:20191207193441j:plain — 運動モデル

X．パラメータ化された制御入力（Parameterized Control Input）

今回のアプローチでは数値最適化を使います．なので，何らかの形で制御入力（v, θ）を数式に落としてあげないといけません．論文では，速度入力を時間に関しての台形関数，角速度を時間に関してのスプライン曲線で近似していたのですが，簡単のためにそれぞれを時間に関しての n次多項式としています．

f:id:rkoichi2001:20191207194514j:plain — パラメータ化された制御入力

ちなみに，論文では "Parameterized Vehicle Control" というワードが頻繁に出てきてたんですが，上図のように「入力を何らかの関数でモデル化して，そのパラメータ（上図だと，多項式の係数）として表現すること」なんですね．このイメージがなかなかわかずに「？」してました．

X．数値最適化を用いた最適パラメータの計算

ここからは最適化の導出をします．今回の最適化対象は，

「目標到達点」と「モデルを数値積分することによって求まった到達点」の差分

を最小化することなので，下記のように定式化します．

f:id:rkoichi2001:20191207205218j:plain — 最適化問題の定式化

で，目的関数が定義できたので，あとは多変数のニュートン法を使ってパラメータ修正ベクトルを求めます．

f:id:rkoichi2001:20191207210918j:plain — パラメータ修正ベクトルの導出

次に，上記ノートに出てきたヤコビアンを計算します．このあたりは実際に組み込むときにはライブラリを使うんだとおもいますが，最適化マスターを目指して，今回は泥臭くやります(｀･ω･´)ゞ

f:id:rkoichi2001:20191207212620j:plain — ヤコビアンの導出

X．実装

ということで，実装です．ごちゃごちゃとプロットや設定をしているとこがありますが，重要なとこは２つだけです．

収束計算を実施しているとこ．

下記の大きなループで収束計算を回しています．

    for i in range(6):

        v_coeffs = params[0: init_v_coeffs.shape[0]]
        w_coeffs = params[init_v_coeffs.shape[0]: params.shape[0]]
        
        # Forward Integration.
        x_tf = b_traj.compute_states(x_init, v_coeffs, w_coeffs, t_st, t_end, dt)

        # Plot Result of Forward Integration.
        pose.plot(x_tf[0,:], x_tf[1,:], label="Iteration Count : " + str(i))

        # Compute Jacobian via Linearization.        
        jaco = jacobian_calc.calc_jacobian(x_init, x_final, v_coeffs, w_coeffs, t_st, t_end, dt)
        
        # Compute Parameter Correction Vector via Newton Methods
        dx = np.append(x_init, x_final, axis=0) - np.append(x_tf[:,0], x_tf[:,-1], axis=0)
        jaco_pseudo_inv = np.linalg.pinv(jaco)
        dp = np.matmul(-jaco_pseudo_inv, dx)
        params = params + dp

重要な箇所としては，

        jacobian_calc.calc_jacobian(x_init, x_final, v_coeffs, w_coeffs, t_st, t_end, dt)

のラインでヤコビアンをつどつど計算して，その後の，

        dx = np.append(x_init, x_final, axis=0) - np.append(x_tf[:,0], x_tf[:,-1], axis=0)
        jaco_pseudo_inv = np.linalg.pinv(jaco)
        dp = np.matmul(-jaco_pseudo_inv, dx)
        params = params + dp

のパートでパラメータ修正ベクトルを計算，パラメータの更新をしています

ヤコビアンを計算しているとこ

下記のパートでヤコビアンを計算しています．

    def calc_jacobian(self, x_0, x_f, v_coeffs, w_coeffs, t_st, t_end, dt):
        jaco = np.zeros([x_0.shape[0] + x_f.shape[0], v_coeffs.shape[0] + w_coeffs.shape[0]])
        params = np.append(v_coeffs, w_coeffs, axis=0)
        x_tgt = np.append(x_0, x_f, axis=0)

        for i in range(jaco.shape[1]):
            d = np.zeros(jaco.shape[1])
            d[i] = self.__ep
            n_params = params - d
            p_params = params + d

            # Numerical Integration
            states = self.__b_traj.compute_states(x_0, n_params[0:v_coeffs.shape[0]], n_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt)
            x_f_n = np.append(states[:, 0], states[:, -1], axis=0)
            states = self.__b_traj.compute_states(x_0, p_params[0:v_coeffs.shape[0]], p_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt) 
            x_f_p = np.append(states[:, 0], states[:, -1], axis=0)

            # Differentiation
            diff_n = x_tgt - x_f_n
            diff_p = x_tgt - x_f_p
            jaco[:, i] = (diff_p - diff_n) / (2*self.__ep)

        return jaco

今回のケースでは数値計算でヤコビアンを求めていますが，ヤコビアンの一つの列が一つのパラメータに対する変動成分を表すので，下記のコードをパラメータ数分実行してヤコビアンを作ってます．

            d = np.zeros(jaco.shape[1])
            d[i] = self.__ep
            n_params = params - d
            p_params = params + d

            # Numerical Integration
            states = self.__b_traj.compute_states(x_0, n_params[0:v_coeffs.shape[0]], n_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt)
            x_f_n = np.append(states[:, 0], states[:, -1], axis=0)
            states = self.__b_traj.compute_states(x_0, p_params[0:v_coeffs.shape[0]], p_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt) 
            x_f_p = np.append(states[:, 0], states[:, -1], axis=0)

            # Differentiation
            diff_n = x_tgt - x_f_n
            diff_p = x_tgt - x_f_p
            jaco[:, i] = (diff_p - diff_n) / (2*self.__ep)

X．実験１（収束する場合）

　実験結果は次のようになりました！出発点と到着点がそれぞれ色付きの丸で表されてまして，イテレーション毎に軌道が描かれてます．パラメータの初期値はすべて０からスタートさせましたが，このサンプルではいい感じに収束しました．二点，上述の説明と異なる部分がありまして，論文 ”Optimal rough terrain trajectory generation for wheeled mobile robots” では，速度，ステアリング角の初期条件は予め多項式の係数として事前に求めてから最適化を実施していましたが，そのロジックを作る代わりに初期状態と終了状態の両方のベクトルを制約条件として最適化しています．あと，レーンチェンジのマヌーバはステアリング操作が２次の多項式で表現できないため，速度，ステアリング角ともに３次の多項式にしてます．
　走行開始時，走行終了時の両方の境界条件が満たされていることが下記のプロットから解ると思います．

f:id:rkoichi2001:20191215224946p:plain — 軌道生成結果

X．実験２（収束しない場合）

　実験１はいい感じに目標に対して収束していきました．「じゃあ，収束しないときはどうなんの？」という疑問が湧いたので，実験してみます．上の結果を見てわかるとおり，レーンチェンジのマヌーバーはハンドルを左右に切らないと行けないので二次式では表現できません．ここではあえて，速度，ステアリング角を２次式でモデル化して問題を解いてみます．

f:id:rkoichi2001:20191215225226p:plain — 軌道生成結果（失敗版）

上図を見るとわかると思うのですが，目標に対して収束しませんでした．また，開始端，終了端ともに境界条件を満たせておらず，下記のような誤差になりました．
[dx_st, dy_st, dφ_st, dv_st, dθ_st, dx_end, dy_end, dφ_end, dv_end, dθ_end]
=
[ 0, 0, 0, -8.7e-05, -4.1e-01, -1.1e-01, 1.0e-01 -7.0e-01, -1.2e-02, 4.1e-01]

ということで，実際にこのアルゴリズムを使うときは

１．収束に必要であろう回数を定義して，最適化計算を実施．

２．定義回数を超えた時点で境界条件と数値積分の結果を比較し，誤差が閾値内に収まれば生成成功，そうでなければ生成失敗

という感じに作ると良さそうです．

X．実験で使ったコード

Planner の学習過程で作ったサンプルコードはここに入れて行こうと思います．
github.com

X．参考文献

論文

Optimal rough terrain trajectory generation for wheeled mobile robots

お世話になったブログ＆Github

myenigma.hatenablog.com

github.com

2019-11-25

Google Cartographer を使ったマッピング　〜パラメータチューニング編〜

ということで，今年のつくばチャレンジで習得したスキルを書き残しておきたいと思います．

０．マップ作成対象

今年のつくばチャレンジのコースは下記の全長２．Xkmのコースだったんですが，全部のコースマップを一括で作るとなかなか大変だったので，自分は下記の６つに分割しました．

f:id:rkoichi2001:20191115034319p:plain — つくばチャレンジ２０１９のコース

コース１．確認走行区間

f:id:rkoichi2001:20191117074725p:plain — 確認走行区間

コース２．交差点まで（行き）

f:id:rkoichi2001:20191117074755p:plain — 交差点まで

コース３．研究学園駅

f:id:rkoichi2001:20191117074817p:plain — 研究学園駅

コース４．研究学園駅前公園

f:id:rkoichi2001:20191117074850p:plain — 研究学園駅前公園

コース５．交差点まで

f:id:rkoichi2001:20191117074934p:plain — 交差点まで

コース６．交差点からゴール（帰り）

f:id:rkoichi2001:20191117075003p:plain — 交差点からゴールまで

で，いつものように gmapping で地図を作ればいいかと思っていたのですが，コース４（研究学園駅前公園）が結構長く，なかなか安定してマップが作成できなかったために Cartographer を使うことにしました．ただ，この Cartographer，なかなかパラメータチューニングが大変でうまく使いこなすまでに時間がかかったので，このエントリではそのあたりを書き残せればと思います．時間があれば理論編も書きたいですが，，，いつものことながら理論系のエントリは時間がかかるので，気が向いたら書きます．

１．参考文献＆ブログ

パラメータチューニングのやり方は， Cartographer の作者の方が作ってくれている下記ドキュメントやブログを見ながら実施すればある程度は理解できると思います．

Cartographer ROS Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer-ros/latest/google-cartographer-ros.pdf

Cartographer Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer/latest/google-cartographer.pdf

あと，下記はお世話になったブログです．
ssk0109.hatenablog.com

ssk0109.hatenablog.com

２．前提・システム構成

このエントリでは，下記の前提でマップ作っています．

2D Lidar が一つ，Odometry（x, y, yaw）有り，IMU無し

f:id:rkoichi2001:20191117093646j:plain — 今回のマップ制作に使用したロボット，改め愛犬．前から

f:id:rkoichi2001:20191117093752j:plain — 今回のマップ制作に使用したロボット，改め愛犬．横から

主要パラメータファイル一覧

変更が必要となるパラメータファイルは下記のとおりです．

backpack_2d.urdf

　各センサーの搭載位置情報が記載された urdf ファイル．今回のエントリでは horizontal_lidar のみ使用．

backpack_2d.lua

　map 作成時の構成情報が記載された lua ファイル．

trajectory_builder2d.lua

　Local SLAM のパラメータが記載された lua ファイル．　　

pose_graph.lua

　Global SLAM のパラメータ（大域的最適化）のパラメータが記載された lua ファイル．

手順

で，実際にマップを生成するにあたっては，下記の手順で設定・パラメータを変更していきます．

１．センサーの搭載位置情報を backpack_2d.urdf に記載する．
２．Topic 名や構成情報を backpack2d.lua に記載する．
３．Odometry がある程度正しくなるようにパラメータ調整．
４．調整された Odometry を使って Local SLAM のパラメータ調整（trajectory_builder2d.lua）．
５．狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整（pose_graph.lua）．

ステップ１　センサーの搭載位置情報を backpack_2d.urdf に記載

ここの設定はとても大事です！これを忘れるとうまく行きません！

f:id:rkoichi2001:20191115061647p:plain — horizontal scanner の位置を設定

ステップ２　Topic 名や構成情報を backpack2d.lua に記載

Topic 名や構成情報を backpack2d.lua に記載します．

f:id:rkoichi2001:20191115054802p:plain — backpack_2d.lua の差分

変更したパラメータですが，
published_frame：今回はオドメトリを使用しているので，オドメトリフレームである odom を指定します．cartographer が生成する map フレームは，ここで指定したフレームを子供に持つ事になります．
provide_odom_frame：true にすると，Local SLAM で生成された自己位置情報が odom_frame として供給されます．自分の場合は別のノードが odom_frame の tf を供給していたので， false にしました．
use_odometry：マップ生成にオドメトリを使うか否か．今回は true です．
num_laser_scans：レーザスキャンセンサーの数．今回は１つです．
num_multi_echo_laser_scans：Echo Laser Scan を生成するレーザスキャンセンサの数．今回は０です．

ステップ３　Odometry がある程度正しくなるようにパラメータ調整

このステップはちょっと詐欺かもですが，，，，正しいマップを手っ取り早く作成したかったので，自分のオドメトリで軌跡がつながるようにヨーレートのオフセットを事前に計算して使うことにしました．

オフセット調整前

例えば，こんな感じで全く繋がらないオドメトリを．．．

f:id:rkoichi2001:20191115064643p:plain — オフセット調整前オドメトリ

オフセット調整してそこそこつながるようにします．

f:id:rkoichi2001:20191115064445p:plain — オフセット調整後オドメトリ

ステップ４　調整された Odometry を使って Local SLAM のパラメータ調整

Local SLAM の調整です．Cartographer のパラメータ調整マニュアルにも有りますが，下記のパラメータを変更して Global SLAM を OFF にして，Local SLAM だけで性能出しします．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90
【After】
POSE_GRAPH::optimize_every_n_nodes = 0

※それ以外はデフォルトパラメータのままで動かしてみます．

f:id:rkoichi2001:20191115070810p:plain — Local SLAM の結果（デフォルトパラメータ）

上図を見てわかるように，デフォルトパラメータだとヨー角の誤差が積算してしまい，一周して元の地点に戻ってきてもマップが繋がらなくなってしまいました．ということで，ヨー角に関しては Local Scan Matcher の結果よりももう少しオドメトリを信じたほうが良さそうです．ひとまず効果代を見るために，パラメータを目一杯変えてみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

f:id:rkoichi2001:20191115073656p:plain — オドメトリのヨー角を強く信じる設定．

だいぶいい感じになりました．というか，オドメトリしか使ってない感じになりました（笑）で，図中の吹き出しにも書いたんですが，ヨー角に関してはオドメトリ生成値の重みを強くしたものの，並進に関しては設定をいじらなかったため，２つのオドメトリ間でまだ差分が見られます．ということで，並進もオドメトリを強く信じてみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000

f:id:rkoichi2001:20191115075034p:plain — オドメトリのヨー角，並進を強く信じる設定．

完全にオドメトリを信じて作ったマップになりました！マップもきれいになったし，めでたいめでたし．．．．だったらSLAMはいらないわけで．．．大域的にも局所的にもオドメトリには誤差があるので，細かいとこを見ていくとマップが変になってます．例えば，一つの平面をLIDARで取っているにもかかわらず，MAP上には２つの線として現れています．ここからは，，，おそらく微調整が必要になるのかと思います．ということで，もうちょっとスキャンマッチングの結果を信用してみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000

f:id:rkoichi2001:20191115083542p:plain — スキャンマッチングの結果をもうちょっと信用

うーん．あんまり変わらないですね．．．．ということで，次に Global SLAM の調整をしてみます．

ステップ５　狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整．

まず，ステップ４でOFFにした Global SLAM を ON にします．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 0
【After】
POSE_GRAPH::optimize_every_n_nodes = 90

f:id:rkoichi2001:20191117080829p:plain — Global SLAM を ON にしてマップ生成

再訪問したところで Loop Close してほしいのですが，デフォルトパラメータのままではそれほどループが閉じませんでした．ということで，パラメータを変更してループが閉じやすくします．

【Before】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e4
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e5

【After】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10

f:id:rkoichi2001:20191117084435p:plain — Loop Close しやすくした結果

８の字走行した部分（図の左側）に関してはループが閉じました．が，走行の最後の部分に関してはまだループが閉じていません．おそらく自分のデータのとり方が良くなかったんですが，一周回って同じところに帰ってきた時点でデータ取得を止めてしまったので，再訪箇所のオーバーラップが少なく，ループが閉じきれてないのかと思われます．ということで，Loop Close をもっと頻繁に発生させて，最後のループが閉じるかどうか実験してみます．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90

【After】
POSE_GRAPH::optimize_every_n_nodes = 5

うーん．なんかまだ微妙です．

f:id:rkoichi2001:20191117085541p:plain — 頻繁に Loop Close 走らせた結果

Cartographer では，マップの小領域（Submapと呼ぶみたいです）に対して Scan 結果から得られる尤度を蓄積していき，更新が一定回数以上になったときにその Submap を大域的最適化対象に登録します．この「一定回数以上」のパラメータが下記になるのですが，最適化対象にするタイミングをもっと早めてみます．

【Before】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 90

【After】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

f:id:rkoichi2001:20191117090401p:plain — 間違った Loop Closure を検出

うーん．ループを誤検知してしまいました．もう少しオドメトリを信じるべきか．．．
Cartographer のポーズ最適化では， Local SLAM のときと同様に，オドメトリ，Local SLAM の結果に対して重みを設定することができるのですが，もう少しオドメトリを信じる設定にしてみます．

【Before】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e5
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e5

【After】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7

f:id:rkoichi2001:20191117094137p:plain — 一応完成！

ということで，上記のパラメータ調整を経て，結構いい感じになったのでこれで良しとします．エンコーダとIMUを使ったヨーレート（赤線）は完全には正確でなく，一周回ってきたときには繋がってませんが，SLAMから生成されるオドメトリ（青色）は一周回った時点できれいにつながりました．

結果

ということで，全マップです！うーん．やっぱり，ループを綴じ込むのはかなり難しいですね．オドメトリを合わせればそこそこまでは行くものの，完全ではないのでループ綴じ込む程ではなく，逆に Local SLAM, Glocal SLAM の寄与を上げるとループじゃないところで誤マッチングが起こり．．．というシーソーゲームをひたすら繰り返してました．

コース１．確認走行区間

f:id:rkoichi2001:20191117190041p:plain — コース１のマップ

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10
POSE_GRAPH::optimize_every_n_nodes = 5
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

コース２．交差点まで（この区間は難しく，，，完璧にはできませんでした．）

f:id:rkoichi2001:20191117190556p:plain — コース２のマップ．

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

コース３．研究学園駅

f:id:rkoichi2001:20191117190751p:plain — コース３のマップ．

コース４．研究学園駅前公園（この区間は難しく，，，完璧にはできませんでした．）

f:id:rkoichi2001:20191117191016p:plain — コース４のマップ

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8

コース５．交差点まで

f:id:rkoichi2001:20191117191218p:plain — コース５のマップ．

POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 40

コース６．交差点からゴール

f:id:rkoichi2001:20191117191340p:plain — コース６のマップ．

あともう一つ，英語圏（≒アメリカ，ドイツ）からのアクセスをもっと増やせないかな〜という思いもあって，アクセス数が稼げそうなエントリに限って英語版を作ってみることにしました．アクセスが劇的に増えたら，また報告します（笑）

http://daily-tech.hatenablog.com/entry/2019/11/25/062304daily-tech.hatenablog.com

2019-11-25

Mapping with Google Cartographer ~ Parameter Tuning ~

I would like to summarize the skills I learned from this year's Tsukuba Challenge. This blog post is about mapping with Google Cartographer.

0. Mapping Target

The total length of Tsukuba Challenge 2019 is 2.X km. Since it is quite difficult to do SLAM for all the course at once, I divided it into the following 6.

Course 1. Confirmation Section

Course 2. Up to 1st Intersection

Course 3. Kenkyu Gakuen Station

Course 4. Kenkyu Gakuen Station Park

Course 5. Up to 2nd Intersection

Course 6. From 1st Intersection to Goal

I thought that I should make a map with gmapping as usual, but I decided to use Cartographer because Course 4 (Kenkyu Gakuen Station Park) was quite long and I couldn't create a stable map. It is quite difficult to tune parameters for Cartographer, and it took a long time for me. So I decided to create this entry as "Tuning manual with actual examples". I also wanted to write down theory that is used inside it, but this would take so much time, so,,, I will give it a shot when I have time.

1. References & Useful Blog Posts

How to tune parameters are roughly described in the following manual that are documented by the developer. I also referenced the following 2 blog posts.

2. Preconditions and System Configuration.

The maps shown in this entry are created with the following preconditions.

One 2d Lidar, Odometry (x, y, yaw), No IMU.

3. Parameter files subject to update.

The frollowing 4 files are necessary to be modified for 2d slam cases. (My case.)

backpack_2d.urdf

Urdf file that describes sensor configuration. Only horizontal_lidar is used for this entry.

backpack_2d.lua

Lua file that describes system configuration. (Topic, Frame etc ...)

trajectory_builder2d.lua

Lua file that describes parameters for Local SLAM.

pose_graph.lua

Lua file that describes parameters for Glocal SLAM (Loop Closure).

4. Procedure

Change the settings and parameters according to the following procesure.

1. Update sendor mounting position information described in backpack_2d.urdf.
2. Update topic name and system configuration described in backpack2d.lua.
3. Tune parameters so that odometry is correct to some extent.
4. Tune local SLAM parameters with already tuned odometry. (trajectory_builder2d.lua)
5. Tune global SLAM parameters so that loop closes correctly at the target location.

Step 1 : Update sendor mounting position information described in backpack_2d.urdf.

The setting here is very important! If you forget this, it will not work!

Step 2 : Update topic name and system configuration described in backpack2d.lua

Regarding the modified parameters,,,,
published_frame：Since odometry is used this time, specify odometry frame as "odom". The map frame generated by cartographer will have this frame as the child.
provide_odom_frame：If true, transform between map frame and localized position by local SLAM is supplied as odom_frame. In my case, since another node supplied tf of odom_frame, I set it to false.
use_odometry：Whether to use odometry for map generation. This time, set it to true.
num_laser_scans：Number of laser scan sensors. This time, set it to one.
num_multi_echo_laser_scans：Number of echo laser scan sensor. This time, set it to zero.

Step 3 : Tune parameters so that odometry is correct to some extent.

This step may be a bit tricky, but I wanted to quickly create a correct map, so I decided to tune the yaw rate offset in advance so that the odometry trajectory gets roughly connected.

Before offset adjustment

For example, odometry not connected at all like this....

Adjust offset so that it connects properly.

Step 4 : Tune local SLAM parameters with already tuned odometry.

Adjustment of Local SLAM. At first, turning off global SLAM by the following parameter, tune and improve local SLAM performance as described in "Cartographer parameter tuning manual".

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90
【After】
POSE_GRAPH::optimize_every_n_nodes = 0

Default settings are used except for the above modification.

As you can see from the above figure, the default parameter causes the yaw angle error to accumulate, and the map does not connect even if you return to the original point after going around. So, it seems better to believe in odometry a little more than the result of Local Scan Matcher for yaw angle. Let's change the parameters as much as possible and see the result.

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

As for the rotation, the map became match better. Since I only increased the weight of odometry for rotation and not for translation, there is still a translation offset between encoder based odometry (red) and slam base odometry (purple).

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000

A map made is completely based on encode odometry! Even though the map seems decent, if you take a look at it closely there are some errors, since odometry contains errors both in locally and globally. For example, even though one plane is taken with LIDAR, it appears as two lines on the MAP. From here, I think that fine tuning is probably necessary. So, let's trust the scan matching result a little more.

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000

Well. It doesn't change much. . . . So, let's go to next step,,, adjustment of Global SLAM.

Step 5 : Tune global SLAM parameters so that loop closes correctly at the target location.

Enable the global SLAM that was disabled in step 4.

【Before】
POSE_GRAPH::optimize_every_n_nodes = 0
【After】
POSE_GRAPH::optimize_every_n_nodes = 90

I would like a loop to get closed when it re-visited the same place, but it did not happen with the default parameters. So let's modify parameters for loop closure to happen.

【Before】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e4
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e5

【After】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10

The loop closed for the part of figure 8 (left side of the figure). However, the loop is not yet closed for the last part of the run. This is probably because the way I collect the data was not good. Since I stopped recording data immediately after I returned to the same place after going around the circle, there were not enough overlap around the revisited places. To complement this, let's modify parameter to make loop closure occur more easily and frequently, and see if this achieve correct loop closure.

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90

【After】
POSE_GRAPH::optimize_every_n_nodes = 5

Mmm.... No drastic change....

Cartographer accumulates the likelihood obtained from the scan results. This likelihood is accumulated for a small chunk of the map, called "Submap", and registers the submap as a global optimization target when the number of updates exceeds a certain number. This "certain number" parameter is as follows, and let's lower the threshold to achieve earlier registration of submap for global optimization.

【Before】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 90

【After】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

Well,,, wrong loop closure detected... Probably I should believe encoder based odometry a little more.
In Cartographer's pose optimization, as with Local SLAM, you can set weights for the results of encoder based odometry and scan matching based odometry. Let's trust encoder based odometry a little more.

【Before】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e5
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e5

【After】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7

After several parameter adjustment, the generated map seems to be OK. The encoder based odometry (red line) does not completely close when it goes around, but the odometry line generated from SLAM (purple) is connected smoothly where the loop closes.

Result

Result of all maps shown below! After all, it is quite difficult to achieve loop closure. If you increase weight on the encoder based odometry, the map becomes decent. However it is not perfect enough to achieve loop closure, and conversely, if you increase the contribution of local and global SLAM, a false match occurs in a place that is not a loop. . . I just repeated the seesaw game.

Course 1. Conrimation Section

Course 2. Up to 1st Intersection (This interval is difficult and the loop does not get closed completely.)

Modified parameters from Cartographer's default

POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

Laplacian of Gaussian Filter を適用してみる．

σ = 1.6

σ = 2.2

σ = 3.2

σ = 4.5

σ = 6.4

σ = 9.0（青の円付近のひまわりが強く反応．）

σ = 12.8

σ = 18.1

σ = 25.6（赤の円付近のひまわりが強く反応．）

σ = 36.2（赤の円付近のひまわりが強く反応．）

σ = 51.2

σ = 72.4

Scale Spaceの定義．

実験に使ったコード．

参考文献．

一階微分フィルタ・二階微分フィルタとは？

２D画像に一階微分フィルタ・二階微分フィルタを適用する．

１D画像に微分フィルタを適用する．

Gaussian Filter と二階微分（ラプラシアン）フィルタの関係

実験に使ったコード．

参考文献．

Gaussian Filter とは．

標準偏差 (σ)と暈しの程度を考える．

画像にGaussian Filterを適用する．

σ = 5

σ = 12.5

σ = 25

σ = 50

σ = 100

σ = 180

実験に使ったコード．

参考文献．

１．ここまでの報告

２．やりたいこと！

３．２０２０年，やること！

X．はじめに

「目標出力（走行レーンの真ん中を走る）を満たす入力（アクセル，ハンドル）を事前に計算して，そいつを入力する」

X．問題設定

出発点の車の状態

到達点の車の状態

X．処理の大きな流れ

１．制御入力をモデル化して関数として表現する．（v = f(t), θ = g(t)）

２．モデル化関数のパラメータの初期値を決める．（v = av*t2 + bv*t + cv, θ = aθ*t2 + bθ*t + cθの，係数 a, b, c のこと．）

３．各時点での制御入力をモデル化関数から求め，数値積分して最終時間での車両状態を求め，到達点からの差分を計算する．（diff = X_end - X(tf)）

４．到達点からの差分を，モデル化関数のパラメータに関して線形化してヤコビアンを計算する．

５．３で求まった目標値からの差分，４で求まったヤコビアンをもとに，ニュートン法を使って修正ベクトルを計算．

６．５で計算した修正ベクトルを使って，パラメータを更新し，３に戻る．

X．運動モデル

X．パラメータ化された制御入力（Parameterized Control Input）

X．数値最適化を用いた最適パラメータの計算

「目標到達点」と「モデルを数値積分することによって求まった到達点」の差分

X．実装

収束計算を実施しているとこ．

ヤコビアンを計算しているとこ

X．実験１（収束する場合）

X．実験２（収束しない場合）

１．収束に必要であろう回数を定義して，最適化計算を実施．

２．定義回数を超えた時点で境界条件と数値積分の結果を比較し，誤差が閾値内に収まれば生成成功，そうでなければ生成失敗

X．実験で使ったコード

X．参考文献

論文

お世話になったブログ＆Github

０．マップ作成対象

コース１．確認走行区間

コース２．交差点まで（行き）

コース３．研究学園駅

コース４．研究学園駅前公園

コース５．交差点まで

コース６．交差点からゴール（帰り）

１．参考文献＆ブログ

Cartographer ROS Documentation

Cartographer Documentation

２．前提・システム構成

2D Lidar が一つ，Odometry（x, y, yaw）有り，IMU無し

主要パラメータファイル一覧

backpack_2d.urdf

backpack_2d.lua

trajectory_builder2d.lua

pose_graph.lua

２．モデル化関数のパラメータの初期値を決める．（v = a_vt² + b_vt + c_v, θ = a_θt² + b_θt + c_θの，係数 a, b, c のこと．）

ステップ１　センサーの搭載位置情報を backpack_2d.urdf に記載

ステップ２　Topic 名や構成情報を backpack2d.lua に記載

ステップ３　Odometry がある程度正しくなるようにパラメータ調整

ステップ４　調整された Odometry を使って Local SLAM のパラメータ調整

ステップ５　狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整．