代表取締役
ということで，なっちゃいました（笑）
ブログの更新が滞ってしまったんですが，実は絶賛会社設立中です．もともと法人化に興味はあったのですが，「個人事業がうまく行って，いつか会社を設立できればいいなあ．．．」くらいに考えていたところ，ちょうど仕事の話をいただきまして，法人格でないと発注できないということだったので「良い機会かも！」と思ってトライしてみることにしました．
ただ，会社設立までの具体的な作業や書類等，何一つ把握してなかったので，そこから作業をはじめました．
情報調査のために購入した書籍
個人事業主からの法人化になるのですが，下記の書籍を購入しました．一通り必要なことをまとめてくれていて，分かりやすかったです．
改訂4版 個人事業を会社にするメリット・デメリットがぜんぶわかる本
- 作者:俊輔, 関根
- 発売日: 2019/08/13
- メディア: 単行本
- 作者:寺内正樹
- 発売日: 2010/10/19
- メディア: 単行本
会社設立に使ったサービス
上記の本をちゃんと読んだあとに下記のサービスを使って法人化のための書類一式を作成したのですが，必要事項を入力していけば，ほぼ修正無しで会社設立できました．．．．ただ，事前に一連の流れを知っておくほうが良いとは思いますが．．．
www.freee.co.jp
直近のドタバタ．．．
・会社設立を決意！Amazonで本を買う．
・Amazon本で一通りの流れを調査．
・TODOリストを起こす．（法人登記までの流れ，銀行口座作成 etc etc...）
・市役所で個人印鑑の登録＆印鑑証明取得．
・freee の会社設立サービスを使って，書類一式の作成．
・会社名考える．．．
・会社名決定，定款最終版作成．
・定款認証＠公証役場，会社の印鑑発注．
・印鑑が届き，法務局で登記申請．
・登記完了！法人印鑑証明，登記簿謄本を取得．
・銀行への説明資料作成．．．
・税務署へ法人設立届出書を出した後，銀行へ口座開設申請＆資料送付．
ということで，頂いた仕事の話が急ぎだったこともありだいぶ突貫工事だったんですが，なんとか会社設立までたどりつけそうです．ネットで調べたところ法人用の銀行口座開設は結構難しいらしいのですが，，，頑張って資料作成して泣き落としできればと（笑）
飛躍の５月
なんだかこのところ，５月は毎年いろいろな転機が訪れる月になっています．
一昨年の５月は，「ドイツ人と日本人のはざまで苦悩すべく，ドイツへ２ヶ月の旅．」
daily-tech.hatenablog.com
去年の５月は，「個人事業主になることを決意．退職し，沖縄へ（笑）」
daily-tech.hatenablog.com
今年の５月は，「代表取締役として，会社を取り締まる立場に（笑）」
今後
ちょっと酔っていることもあって，だいぶ軽いノリのエントリになってしまいました．．．エンジニアとしても（当然のことながら会社としても）まだまだ未熟者ですが，「コンピュータビジョンとロボティクス」をメインテーマとして地道にやっていきたいと思います．会社名とか会社のホームページとか，もうちょっと形になってきたらブログでも紹介できればと思います．それでは，今後共よろしくお願いします！
SIFT ~ 実装編（全体構成）
SIFTの理論と実装のエントリ，どうやってまとめようか悩んでたんですが，ちょっと絵を書き始めると時間がかかりすぎて前に進めなくなってしまうので，ひとまず大雑把にまとめておくことにしました．コードはだいぶリファクタして読みやすく（自分としては．．．）なったはずなので，コードを追いかけてもらうとSIFTの実装がどんな感じになっているかはざっくりと解ると思います．一通りSFMに目処がついた段階で，各エントリをリライトできればと思います．
あと，ブログの最後に参考文献のリンクを貼っておいたのですが，中部大の藤吉先生が作られているスライドがとっても分かりやすかったので，かっちりとした理論の部分はそちらを参考にしていただければと思います．
X．全体構成 特徴点抽出
OpenCVの cv::xfeatures2d::SIFT クラスをそのままパクってリファクタしたのですが，論文の構成にできるだけ近づくようにリファクタしました．論文の流れと一致するよう，関数を「特徴点検出」と「記述子作成」の２つに分けたのですが，ここでは特徴点検出の部分を見ています．
１．CreateInitialImage
処理の前段階として，
① 入力画像を Float 型 グレー画像へ変換．
② 入力サイズの２倍に画像を拡大．この画像が First Octave (first_octave_idx =-1) となります．
③ ２で拡大した画像に対して，ガウシアンフィルタを適用します．
を実施します．
２．Octave 数の計算．
つぎに，必要となる Octave 数を計算します．処理画像（①で前処理された画像）に対して，高さと幅の大きい方を 2 で割っていったときに，一辺の長さが 4pix を下回らないところまで画像を縮小していきますが，この値が Octave 数になります．この値は３で計算される Gaussian Pyramid の段数と同じになります．
３．BuildGaussianPyramid
画像ピラミッドを作成します．ここでは処理画像をピラミッドの底辺として，Octave 数段のピラミッドを作成します．
４．BuildDifferenceOfGaussianPyramid
３で生成した Gaussian Pyramid の各 Octave の画像間で差分を計算し，Difference of Gaussian Pyramid を作成します．
６．RemoveDuplicateSorted
重複しているキーポイントを削除します．
７．RetainBest
特徴点の検出上限数が指定されている場合，スコアが低いものを取り除いて上限数に収まるようにします．
８．CompensateInitialScalingToOriginalImage
１のステップで入力画像サイズを２倍にされているので，計算されたキーポイントのスケール，位置，サイズを求まった値に対して半分にします．
該当部コード
void SIFT::Detect(cv::Mat& image, std::vector<SiftKeyPoint>& keypoints) { int first_octave_idx = -1; // 1. Initialize image. bool double_image_size = true; Mat base_image = CreateInitialImage(image, double_image_size, (float)sigma_); // 2. Compute number of octave that is solely computed from image size. double log2_num = std::log((double)std::min(base_image.cols, base_image.rows)) / std::log(2.); int num_octaves = cvRound(log2_num - 2) + 1; // 3. Build Gaussian pyramid. std::vector<Mat> gaussian_pyr; BuildGaussianPyramid(base_image, gaussian_pyr, num_octaves); // 4. Build difference of Gaussian pyramid. std::vector<Mat> diff_of_gaussian_pyr; BuildDifferenceOfGaussianPyramid(gaussian_pyr, diff_of_gaussian_pyr); // 5. Find scale space extrema in difference of Gaussian pyramid. FindScaleSpaceExtrema(gaussian_pyr, diff_of_gaussian_pyr, keypoints); // 6. Remove duplicated key point. RemoveDuplicateSorted(keypoints); // 7. If maximum number supeciried. if (num_features_ > 0) { RetainBest(keypoints, num_features_); } // 8. Compensating doubling of iamge at the beginning. CompensateInitialScalingToOriginalImage(keypoints, first_octave_idx); }
X．全体構成 記述子計算
次に，記述子計算部分の構成です．記述子は一つ一つのキーポイントに対して計算するため，for ループの入れ子構造になってますが，本質的な計算をしているのは下記の部分になります．
１．キーポイント周辺領域計算．
特徴点抽出の部分で抽出されたキーポイントに対して，記述子を計算するための周辺領域サイズを計算します．
２．必要なバッファ準備．
ヒストグラム計算に必要なバッファを確保します．必要なものとしては，「Mag : 勾配強度」「Ori : 勾配角度」「W : 重み（中心点からの距離による）」「RBin : 最終記述子の列方向成分」「CBin : 最終記述子の行方向成分」「hist : ヒストグラム計算領域」の６つです．
３．ComputeGradientForPrePatch
キーポイント周辺領域を分割した小領域に対して，勾配，勾配方向を計算します．
４．UpdateHistogramBasedOnTriLinearInterpolation
３では小領域に対する勾配，勾配方向を計算しましたが，これを記述子に統合します．記述子は１６領域（＝４ｘ４）からなり，一つ一つの領域に８ビンのヒストグラムをもちます．小領域の計算結果がそれぞれこの１６ヒストグラムの各ビンに振り分けられていきます．
５．RefineHistogram
生成されたヒストグラムを正規化し，結果を出力バッファにコピーします．
該当部コード
// Compute sift descriptor. static void ComputeSIFTDescriptor(const Mat& img, Point2f ptf, float angle, float scl, int final_patch_width, int final_ori_hist_bins, float* dst) { // 1. Patch size calculation. int radius, pre_patch_size, final_hist_size; float pre_patch_radius; { pre_patch_radius = Const::SIFT_DESCR_SCL_FCTR * scl; // Compute radius. float sqrt2 = 1.4142135623730951f; radius = cvRound(pre_patch_radius * sqrt2 * (final_patch_width + 1) * 0.5f); // Clip the radius to the diagonal of the image to avoid autobuffer too large exception radius = std::min(radius, (int)sqrt(((double)img.cols) * img.cols + ((double)img.rows) * img.rows)); pre_patch_size = (radius * 2 + 1) * (radius * 2 + 1); final_hist_size = (final_patch_width + 2) * (final_patch_width + 2) * (final_ori_hist_bins + 2); } // 2. Prepare buffer. std::vector<float> Mag(pre_patch_size, 0.0f), Ori(pre_patch_size, 0.0f), W(pre_patch_size, 0.0f), RBin(pre_patch_size, 0.0f), CBin(pre_patch_size, 0.0f), hist(final_hist_size, 0.0f); // 3. Computation for pre-patch element. int pix_cnt = ComputeGradientForPrePatch( angle, pre_patch_size, pre_patch_radius, final_patch_width, radius, Point2i(cvRound(ptf.x), cvRound(ptf.y)), img, RBin, CBin, Ori, Mag, W); // 4. Histogram update based on tri-linear interpolation. UpdateHistogramBasedOnTriLinearInterpolation(Mag, W, RBin, CBin, Ori, pix_cnt, angle, final_ori_hist_bins, final_patch_width, hist); // 5. Refine histogram. RefineHistogram(final_patch_width, final_ori_hist_bins, hist, dst); }
X．実装
論文の構成にできるだけ合うようにリファクタリングしたので，もしSIFTの勉強を急遽する必要が出てきた方は下記のコードを追っかけると少しは参考になるかもです．一応，OpenCVの元の SIFT の出力と比較しながらリファクタしたので，リファクタ前後で結果は一致してます．ただ，現状実行しても可視化結果が全く出ないです．．この辺も後でどうにか．．．
github.com
実行方法
git clone sh build.sh ./build/sift_test
X．参考文献
Distinctive image features from scale-invariant keypoints
http://www.cs.unc.edu/~lazebnik/spring11/lec08_blob.pdf
中部大 藤吉先生の説明スライドですが，めっちゃわかりやすいです．
Scale Space
ということで，下記２つのエントリを経てやっとこさ Scale Space のとこまで来ました．
daily-tech.hatenablog.com
daily-tech.hatenablog.com
で， 微分フィルタ及び二階微分フィルタ（Laplacian Filter）のエントリの最後で，
「スケールが 2σ 位の被写体が写っている画像に対して，標準偏差が σ のガウスフィルタと二階微分フィルタを適用すると，被写体の部分が強く反応する．」
という話をしました．このエントリではこれを実際に実験＆確認して，最後に Scale Space の定義を紹介します．
Laplacian of Gaussian Filter を適用してみる．
ここでは，実験用に下記の画像を使います．
この画像に写っているひまわりですが，大体下記のようなサイズ感になっています．
ここで，赤の円が半径 45pix，青の円が半径 10pix，緑の円が半径 5pix になります．ちなみに，ひまわりは花の部分が黄色，種の部分が黒なので，円で囲ってある部分のエッジはx方向からスキャンしていくと，
「下がりエッジー＞上がりエッジ」
になります．なので，二階微分は
「下がりエッジー＞上がりエッジと上がりエッジー＞下がりエッジ」
の組み合わせになるので，「強く反応する＝円の中心付近で出力値が大きくなる」ことを意味します．
それではこの写真に対して，前回のエントリで実験した 「Laplacian of Gaussian Filter」を適用して結果を見てみます．理論的にはガウシアンフィルタの標準偏差が10pix くらいになったとこで青の円が強く反応（＝明るくなって）して，45pix くらいになったとこで赤の円が強く反応（＝明るくなって）するはずですが，，，実験してみます．
σ = 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の定義．
上記ひまわり畑の実験を踏まえると，LoG Filter (Laplacian of Gaussian Filter) のσを変化させることで，スケールが大体 σ 位の物体・構造を検知することができるようになりました．そこで，画像の縦・横に加え，新たにσの次元を加えることによって，画像中にどんなスケールの物体が写っているかがある程度わかるようになります．下記，簡単ですがいつものポンチ絵です．
図中にスケールがそれぞれ r1, r2, r3 である３つの物体が書かれていますが，σ を変化させて LoG filter をかけ，その反応が強くなった箇所を書き留めておくことによって，書き留めた座標 (x, y) を中心とする物体のスケールが大体 σ くらいであるということがわかるようになります．SIFTではこの原理を使って特徴点を抽出します．ということで，次は SIFT に入ります．あー．時間がかかる．．．
実験に使ったコード．
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
微分フィルタ及び二階微分フィルタ（Laplacian Filter）
前回の Gaussian Filter のエントリに引き続き，微分フィルタについてもエントリを残しておこうと思います．前回のエントリは下記の通り．
daily-tech.hatenablog.com
一階微分フィルタ・二階微分フィルタとは？
その名の通り，隣接する画素値の変化率を計算し，その値を出力とするフィルタです．一般的に画像のエッジを取りたいときによく使われます．こちらもググればいっぱい有益な情報が出てくるので，ここでは実際に使ってみてどんな出力が得られるか見てみたいと思います．
２D画像に一階微分フィルタ・二階微分フィルタを適用する．
手始めに，下記のサンプル画像に対して一階微分フィルタ・二階微分フィルタを適用します．下記の画像はわが母校．．．ではありませんが，東工大の大岡山キャンパスです．この画像に対して，x方向の一階微分フィルタ，ｙ方向の一階微分フィルタ，二階微分フィルタを適用してみます．
原画像
ｘ方向の一階微分フィルタを適用
ｘ方向に関して，上がりエッジがある場所では値が高く（白く），下がりエッジがある場所では値が低く（黒く）なっている様子がわかります．
ｙ方向の一階微分フィルタを適用
ｙ方向に関して，上がりエッジがある場所では値が高く（白く），下がりエッジがある場所では値が低く（黒く）なっている様子がわかります．
二階微分（ラプラシアン）フィルタを適用
二階微分フィルタの適用結果ですが，一階微分フィルタの応答とは異なり，よく見るとエッジがあるところが白黒になっていることがわかると思います．
二階微分（ラプラシアン）フィルタの結果拡大図
上の画像の一部拡大版ですが，二階微分（ラプラシアン）フィルタを適用すると，エッジの存在する箇所が「黒白，もしくは白黒」になっていることがわかります．
１D画像に微分フィルタを適用する．
ここではそれぞれのフィルタの動きを見るために，問題を簡単化して１D画像で考えます．前回のエントリ同様に白帯の画像を考えます．上から順に，「原画」「原画のｘ方向断面」「ｘ方向の一階微分フィルタ出力」「二階微分フィルタの出力」となっています．
上記の図から，二階微分フィルタの出力がなぜエッジ付近で「黒白，もしくは白黒」になることがわかるかと思います．上がりエッジが立つと，そのエッジを達成するために一階微分（勾配）が
「０ー＞正の値ー＞０」
と変化し，この勾配を達成するために二階微分が
「０ー＞正の値ー＞負の値ー＞０」
となるためです．二階微分フィルタを適用した大岡山キャンパス画像の出力結果ともつながりました．
ひとまず，一階微分フィルタ・二階微分フィルタの概要はこのくらいにしておきます．ここではエッジがとてもくっきりしていて，ある意味ではステップ入力のような白帯を使いました．次に，この白帯をGaussian Filter でぼかしたときの二階微分（ラプラシアン）フィルタの出力変化を見てみます．
Gaussian Filter と二階微分（ラプラシアン）フィルタの関係
下記，原画に対して Gaussian Filter をかけたときの一階微分フィルタ，二階微分フィルタの応答です．σが大きくなるに従って勾配が緩やかになり，一階微分フィルタ，二階微分フィルタの応答も緩やかになっている様子がわかると思います．
σの標準偏差を持つガウスフィルタを適用した場合，ステップ入力の勾配が小さく（緩やかに）なるため，一階微分・二階微分の値は小さくなっていきます．この出力だと形がわからないので，一階微分フィルタ・二階微分フィルタの出力を正規化して，どんな形になっているのか見てみます．（一階微分の出力値にσを，二階微分の出力値にσ^2をそれぞれかけてあげると正規化できます．）
正規化した画像から，おもしろいことがわかります．４段目の二階微分フィルタの値を見てほしいのですが，白帯の上がりエッジ・下がりエッジにて発生した波がガウスフィルタの標準偏差（σ）を大きくしていくに従って互いに接近しはじめ．．．．σ = 16 で完全に合体しました．ちなみに，この白帯の幅は 32pix なのですが，白帯の幅のちょうど半分くらいの標準偏差を持つガウスフィルタをかけてあげると，二階微分フィルタの出力が極値を迎えることがわかりました．ということで，
「スケールが 2σ 位の被写体が写っている画像に対して，標準偏差が σ のガウスフィルタと二階微分フィルタを適用すると，被写体の部分が強く反応する．」
ことがわかります．今回は一つの画像に対して σ を変化させて微分フィルタの応答を見ましたが，この σ の方向の次元を 「Scale Space」 と呼びます．Scale Space の極値を見ることによって，（＝今回実験したように，σを変えながら二次微分フィルタの極値を探ることによって）被写体の大体のスケールがわかることになります．SIFTではキーポイント検出でこれを使っているのですが，それはまた次のエントリで．．．．
実験に使ったコード．
参考文献．
Scale-space theory: A basic tool for analyzing structures at different scales
Distinctive image features from scale-invariant keypoints
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方向には何も変化がないものにしています．
ちなみに，上記の実験画像は高さが61pix, 幅が501pixで，画像の中心の白帯の幅が11pix になります．次に，σを変化させたときの1Dガウシアンカーネルをプロットしてみると，下記のようになります．
で，実際に実験画像に上記の1Dガウシアンカーネルを適用した結果が下記になります．ここでは「画像の中で幅11pixの領域として映っているものが，σ を変化させたときにどのようにぼかされていくか？」を見ています．左側のグラフは，右側画像をx軸に平行な断面で切って横から見た図です．
上図を見るとわかると思うのですが，σ が11を超えたあたりから急速に白帯が消失してく様子がわかります．σが 11 を超えるまでは，どちらかというとエッジの部分がぼかされていたのに対して，11を超えてくると黒画像領域に白帯が溶け出してきます．ということでガウスフィルタを使うときは，
「画像のなかで暈したいスケール (pix) を決め，σをそのスケールよりも少し大きくとる」
ことをすれば，いい具合に画像をぼかせそうです．
画像にGaussian Filterを適用する．
次に，実際の2D画像に Gaussian Filter を適用してみたいと思います．ここでは実際に上の議論が成立しているかどうかを見るために，下記のような実験画像を用意しました．四角形，丸，三角形が並んでいますが，上から順に 10 x 10pix，20 x 20pix, 40 x 40pix, 80 x 80pix, 80 x 160pix の大きさで書いてます．一番下はちょっと扁平にしてみました．
で，この画像に対して，「それぞれの図形スケール + α」の標準偏差を持つガウスフィルタを適用してみます．ここまでの議論が正しければ，図形のスケールよりも大きい標準偏差を持つガウスフィルタを適用すると図形は消えるはず！といことで，やってみます．
σ = 5
１番スケールの小さい図形（10 x 10 pix）はだいぶ暈されてますが，まだ残ってます．
σ = 180
５番目スケールの図形（80 x 160 pix）が消えました．
ということで，思ったような結果になりました．ただ，暈しなので完全には消えておらず薄っすらと残ってますが，大体のスケールを決めてやれば暈しの効果をある程度操ることはできそうです．ということで，次は Laplacian FIlter と Scale Space の話をします．
実験に使ったコード．
参考文献．
Scale-space theory: A basic tool for analyzing structures at different scales
Distinctive image features from scale-invariant keypoints
春休み！
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 で落ちてるからです！
・会計・金融の勉強．
で，もう一つ！独立っていうとなんだか大げさですが，一応青色申告をする身分になったので，会計の基礎的なことを勉強したいと思います．ただ，勉強するだけだと面白くないので，企業の決算報告書とかをある程度読めるようになります！で，真剣に株式投資をやってみます！（＜－これが真のモチベーションなのかも（笑））ということで，下記，課題図書です．
- 作者:國貞克則
- 発売日: 2016/10/13
- メディア: 新書
- 作者:國貞克則
- 発売日: 2016/10/13
- メディア: 新書
財務3表実践活用法 会計でビジネスの全体像をつかむ (朝日新書)
- 作者:國貞克則
- 発売日: 2012/07/13
- メディア: 新書
![会社四季報ワイド版 2020年2集春号 [雑誌]](https://m.media-amazon.com/images/I/61h1F0LxWoL._SL160_.jpg "会社四季報ワイド版 2020年2集春号 [雑誌]")
- 発売日: 2020/03/16
- メディア: 雑誌
投資で一番大切な20の教え 賢い投資家になるための隠れた常識
- 作者:ハワード・マークス
- 発売日: 2012/10/23
- メディア: 単行本
- 作者:リンチ,ピーター,ロスチャイルド,ジョン
- 発売日: 2001/03/01
- メディア: 単行本
こちらの課題図書はすべてお金を出して購入させていただきました（笑）
あとは，日経新聞のオンラインと四季報オンラインを申し込むかどうか．．．．迷い中です．
ということで，２０２０年はここから怒涛のブログ更新率を見せつけますので，５万人の読者のみなさま，どうぞよろしくおねがいしますm(__)m
制約のある軌道を生成する方法 〜 Constrained Trajectory Generation 〜
なかなか仰々しいタイトルになってしまいました（笑）
つくばチャレンジも終わりSFMに戻ろうかと思っていたのですが，今年のつくばチャレンジではなかなか愛犬が言うことを聞いてくれないこと（＝Plannerの重要性）を痛感し，来年のために Planner 周りの勉強をしてます．で，せっかくなのである程度広く参考文献をあさってみたのですが，なかなかこれが自分には難しく．．．どうやってまとめればいいか．．．と思っている最中なんですが，ひとまず重要そうなトピックを小出しに書いてみることにしました．そこそこ部品ができてきたら大きなまとめを作れればと思ってます．
X．はじめに
人間が自動車を運転している状況を考えてみると，普通は進みたい軌道があると思います．例えば高速道路を走行しているときは，できれば走行レーンのど真ん中を走りたいですし，おしっこしたくなったら SA に入りたくなります．で，「走行レーンのど真ん中」とか「SAに入るためにレーンチェンジ」とかって，地球の表面を完全に x,y 平面だとすると，軌道は複数の点 (x, y) で定義されます．
が，実際に運転しているときにこの軌道を頭のなかで計算している人はいないと思います．しかも，実際に車を操作するときに使うインプットって，アクセル（≒加速度）とハンドル（≒角速度）ですよね？つまり，「走行レーンの真ん中を走る」というのは結果でしかなくて，走行レーンの真ん中からちょっとずれたらハンドルを逆に回して走行レーンの真ん中に戻す．という操作を繰り返して走行レーンの真ん中を走っています．
ロボットを制御するときも同じロジックで，レーンの真ん中からちょっとずれたらフィードバックして．．．という方法で制御することができます．自分は 2013年 にETロボコンというコンテストに出場したことがあるのですが，このロボコンのロボットは光センサーを使ったライントレースで目的地まで進んでいきます．このライントレースがここで言うフィードバックのイメージです．ロボットは進む方向も何も全くわかっておらず，自分の直下の黒線との位置関係だけをみて制御をしています．
（下記動画の最初の部分にライントレースしながら進むロボットが出てきます．）
が，この方法だとロボットの慣性が大きくなったり，緻密な制御が求められる場合に対応できなくなります．ETロボコンでも上位チームのロボットはめちゃ早いんですが（笑），速度が上がってくるとラインを追従しきれなくなってコースアウトするロボットも出てきてしまいます．
で，この問題を解決するためには，
「目標出力（走行レーンの真ん中を走る）を満たす入力（アクセル，ハンドル）を事前に計算して，そいつを入力する」
が必要になるわけですが，これが計算できれば，得られた値を入力として使うことによって，ある程度は期待に沿った目標出力が得られます．このエントリでは，目標出力を満たす入力を事前計算する方法をまとめたいと思います．
X．問題設定
ということで，具体的な問題設定をします．
出発点の車の状態
X_start = [xst, yst, φst, vst, θst]T
到達点の車の状態
X_end = [xend, yend, φend, vend, θend]T
x : 車両の x 座標
y : 車両の y 座標
φ : 車両の 向き
v : 車両の速度
θ : ステアリング角度（≒タイヤの切れ角）
上の２つがわかっている状況下で，２つの点をうまく繋いでくれる制御入力 [v, θ]T を求めます．ここでは，車両がレーンチェンジして停車するマヌーバを考えてみます．下図の点線のラインが，おそらく車両がたどるであろうラインです．
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．運動モデル
実際に入力から出力を計算するためには，制御対象となるロボットのモデルを考えてやる必要があります．自動車の例が一番イメージしやすいかと思ったので，二輪モデルを使って考えてみます．ここでは，入力と目標出力の関係は下記のように設定しました．
入力：速度（v），ステアリング角度（θ）
出力：車両の位置，向き（x, y, φ）
で，アクセルの場合は入力を加速度としたほうがよりイメージに近いかと思ったのですが，論文が速度入力だったので速度入力にしています．あと，ステアリング角とタイヤの切れ角は実際は違いますが，問題が複雑になるだけなので同じとしてます．
X．パラメータ化された制御入力（Parameterized Control Input）
今回のアプローチでは数値最適化を使います．なので，何らかの形で制御入力（v, θ）を数式に落としてあげないといけません．論文では，速度入力を時間に関しての台形関数，角速度を時間に関してのスプライン曲線で近似していたのですが，簡単のためにそれぞれを時間に関しての n次多項式としています．
ちなみに，論文では "Parameterized Vehicle Control" というワードが頻繁に出てきてたんですが，上図のように「入力を何らかの関数でモデル化して，そのパラメータ（上図だと，多項式の係数）として表現すること」なんですね．このイメージがなかなかわかずに「？」してました．
X．数値最適化を用いた最適パラメータの計算
ここからは最適化の導出をします．今回の最適化対象は，
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” では，速度，ステアリング角の初期条件は予め多項式の係数として事前に求めてから最適化を実施していましたが，そのロジックを作る代わりに初期状態と終了状態の両方のベクトルを制約条件として最適化しています．あと，レーンチェンジのマヌーバはステアリング操作が２次の多項式で表現できないため，速度，ステアリング角ともに３次の多項式にしてます．
走行開始時，走行終了時の両方の境界条件が満たされていることが下記のプロットから解ると思います．
X．実験２（収束しない場合）
実験１はいい感じに目標に対して収束していきました．「じゃあ，収束しないときはどうなんの？」という疑問が湧いたので，実験してみます．上の結果を見てわかるとおり，レーンチェンジのマヌーバーはハンドルを左右に切らないと行けないので二次式では表現できません．ここではあえて，速度，ステアリング角を２次式でモデル化して問題を解いてみます．
上図を見るとわかると思うのですが，目標に対して収束しませんでした．また，開始端，終了端ともに境界条件を満たせておらず，下記のような誤差になりました．
[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
