３．バンドル調整 ～ Sympy を使った微分計算
前回のエントリで回転のパラメータ化をしたことによって，透視投影の式から冗長なパラメータがなくなりました．ここからはバンドル調整を「制約のない最適化問題（Unconstrained Optimziation）」として解いていきます．
実際には３次元復元自体に "基準座標の不定性" と "スケールの不定性" があるので，まだ冗長性が残っているのですが，これは後のエントリで出てきます．
制約条件の無い最適化（Unconstrained Optimization）
最適化の方法をざっくりとおさらいしておきたいと思います．前提となるアイデアは，
「最急降下方向に下っていけばいつかは最小値にたどり着く．」
ですが，これをストレートに実施する方法が最急降下法で，もう少し工夫して収束を速めた方法がニュートン法ということになります．ここで，実際に最急降下法とニュートン法の更新ステップをみて，必要な微分式を洗い出しておきたいと思います．
上記のスライドを見て頂ければわかる通り，変数の数が大きくなるとヘッシアンが巨大になります．．．次に，今回の問題の場合に勾配ベクトル，ヘッシアンがどうなるか見てみたいと思います．
バンドル調整の評価関数と微分
まず，今回の評価関数を再確認します．
この関数を微分していくことになります．シグマの形では見づらいので，ベクトルの Inner Product の形に式を変形しています．
Gradient，Hessianに出てくる微分要素
次に，実際に評価関数 E を任意のパラメータ x で微分したときにどうなるか見てみます．最終的に，勾配ベクトルはすべてのパラメータで E を偏微分したものをベクトルの形に並べたものになります．
Sympy を使った微分計算
勾配ベクトルに必要な微分式は２８個，ヘッシアンに必要な微分式は３９２個になりました．が，，，これを手計算するのはさすがにつらいので，ありがたく Sympy を使わせてもらいます．まず，Sympy による微分の方法ですが，とても簡単です．例として， f(x,y) = x*sin(x*x) + y*cos(y*y) を x と y に関して偏微分してみます．
import sympy if __name__ == "__main__": x = sympy.Symbol('x') y = sympy.Symbol('y') f = x * sympy.sin(x**2) + y * sympy.cos(y**2) print('df_dx : ' + str(sympy.diff(f, x))) print('df_dy : ' + str(sympy.diff(f, y)))
で，上記のコードを実行すると次のような結果が出ます．
df_dx : 2*x**2*cos(x**2) + sin(x**2) df_dy : -2*y**2*sin(y**2) + cos(y**2)
が，今回の実装では Python でなく C++ を使っているので，二乗の計算が "**2" になってしまうとつらいです．．．．最初は置き換えしようとしてたんですがあまりの数の多さにげんなりしまして（笑）Cのフォーマットでプリントしてくれる方法を見つけました．
import sympy from sympy import ccode if __name__ == "__main__": x = sympy.Symbol('x') y = sympy.Symbol('y') f = x * sympy.sin(x**2) + y * sympy.cos(y**2) print('df_dx : ' + str(ccode(sympy.diff(f, x), standard='c89'))) print('df_dy : ' + str(ccode(sympy.diff(f, y), standard='c89')))
上記のコードを走らせると，，，，だだーん(ﾟДﾟ)ﾉ
df_dx : 2*pow(x, 2)*cos(pow(x, 2)) + sin(pow(x, 2)) df_dy : -2*pow(y, 2)*sin(pow(y, 2)) + cos(pow(y, 2))
めでたしめでたし！です．
Sympy に微分させる実際の式
ということで，必要な微分式は上のスライドで導出したので，あとは Sympy に頑張ってもらえばよいだけです！本当に，ここまで立式した数式をそのまま突っ込んで実行すると計算してくれます．ありがたや．．．
ただ，回転のパラメータ化のところで触れたと思うのですが， Angle-Axis Representation では回転角が 0 のケースが特異点になります．この特異点を避けなければいけないので，角度の値に応じて数式を２種類用意します．
Symbolの定義．
# 1. 3D Point Coordinate. self.X = sympy.Symbol('p(0)') self.Y = sympy.Symbol('p(1)') self.Z = sympy.Symbol('p(2)') # 2. Internal Camera Parameters. self.fx = sympy.Symbol('K(0,0)') self.fy = sympy.Symbol('K(1,1)') self.cx = sympy.Symbol('K(0,2)') self.cy = sympy.Symbol('K(1,2)') self.s = sympy.Symbol('K(0,1)') self.f0 = sympy.Symbol('K(2,2)') self.K = sympy.Matrix([[self.fx, self.s, self.cx], [0, self.fy, self.cy], [0, 0, self.f0]]) # 3. External Camera Parameters. # Translation. self.tx = sympy.Symbol('T(0)') self.ty = sympy.Symbol('T(1)') self.tz = sympy.Symbol('T(2)') self.T = sympy.Matrix([self.tx, self.ty, self.tz]) # Rotation. self.vx = sympy.Symbol('rot(0)') self.vy = sympy.Symbol('rot(1)') self.vz = sympy.Symbol('rot(2)') self.theta = sympy.sqrt(self.vx**2 + self.vy**2 + self.vz**2) # Identity self.I = sympy.Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # Outer product matrix self.W = sympy.Matrix([[0, -self.vz, self.vy], [self.vz, 0, -self.vx], [-self.vy, self.vx, 0]])
回転角度が一定以上あるとき．
self.R = self.I + (sympy.sin(self.theta) / self.theta) * self.W + \ ((1 - sympy.cos(self.theta)) / self.theta**2) * self.W * self.W self.P = self.K * (self.R.row_join(self.T)) self.x = self.P * sympy.Matrix([self.X, self.Y, self.Z, 1]) self.u = self.x[0] / self.x[2] self.v = self.x[1] / self.x[2]
回転角度が微小な時．
self.R = self.I + self.W
self.P = self.K * (self.R.row_join(self.T))
self.x = self.P * sympy.Matrix([self.X, self.Y, self.Z, 1])
self.u = self.x[0] / self.x[2]
self.v = self.x[1] / self.x[2]
で，上記コードの "self.u" が xpred, "self.y" が ypred になります．あとはこれを各要素で偏微分していけば微分要素がすべて出てきます．
実験コード
下記リポジトリの，blog_bundle_adjust/python/compute_derivatives_1st.py を実行すれば１階微分，blog_bundle_adjust/python/compute_derivatives_2nd.py を実行すれば２階微分が出力されます．
数が多いので，全部関数の形でターミナルにプリントアウトしています．それをコピペすれば，include だけ自分で書けばコンパイルが通るようなっています．なので，実際に実行すると下記の様な形で関数が出力されます．
（すみません．ファイルが出力されて，コンパイルすればそのままOK！にはなってません（笑)）
inline double du2_dXdX(const Eigen::Vector3d &T, const Eigen::Vector3d &rot, const Eigen::Matrix3d &K, const Eigen::Vector3d &p, const Eigen::Vector3d &x) { double theta = std::pow( std::pow(rot(0), 2) + std::pow(rot(1), 2) + std::pow(rot(2), 2), 0.5); if (std::numeric_limits<double>::epsilon() < rot.dot(rot)) { return "回転角度が一定以上ある場合の微分式"; } else { return "回転角度が微小な場合の微分式"; } }
目次のページ
バンドル調整はシリーズ物として書いてまして，目次は下記のページになります．
daily-tech.hatenablog.com
２．バンドル調整 ～ 回転のパラメータ化
透視投影の式
前回エントリで扱ったモデル化で，カメラの透視投影の式を導出しました．下図のように基準座標となるカメラを決定すると，
カメラの投影点を求めるためには下記の式を使う必要がありました．
ここで，各変数の自由度は次のようになります．
- 各カメラの併進ベクトル Tk : 自由度３
- 各カメラの内部パラメータ行列 Kk : 自由度５
- 各カメラの併進ベクトル Rk : 自由度３
併進ベクトル，内部パラメータ行列に関しては，設定した変数の数がそのまま自由度となっています．一方で，回転行列は 3x3 行列で表現しているので，一見すると変数は９個あるように見えますが，回転の自由度は３なので冗長な表現となっていることがわかります．
回転のパラメータ化の必要性
冗長な表現を避けるために回転行列から変換する方法としては，次の３つの方法がよく使われているみたいです．
- Quaternion を使ったパラメータ化
- Angle-Axis Representation を使ったパラメータ化
- 微小回転を用いたパラメータ化
Ceres-Solver ではデフォルトで Angle-Axis Representation が使われていたこともあり，今回は Angle-Axis Representation でやってみました．微小回転を用いたパラメータ化の説明については，金谷先生の下記の本がとても分かりやすかったです．
- 作者:健一, 金谷
- 発売日: 2019/07/27
- メディア: 単行本
Angle-Axis Representation を用いた回転のパラメータ化
Angle-Axis Representation を考えるために，ロドリゲスの式を導出します．この導出では，任意の回転軸（k）周りの任意の回転角（θ）の回転移動を回転軸ベクトル k と 回転各 θ を用いて表現します．
ロドリゲスの公式で，回転前の点 r を回転後の点 r' に変換させることができました．一方で，この回転と等価の回転行列 R を使っても点は変換できる（r' = Rr） となるはずなので，ロドリゲスの公式から等価な回転行列を求めます．ベクトルの外積計算を表現するために，外積計算用の行列を導入しています．
ということで，回転行列を回転軸と回転角を使って表現できるようになりました．が，回転軸と回転角で変数が計４つあります．もう一つ減らしたい．．．ということで，減らします．ここまでは回転軸ベクトルは単位ベクトルでしたが，回転軸ベクトルの大きさを回転角とすることによって変数を３つにします．
ふ～．できました．が，このままだと回転角が小さい，もしくは０の時に式が不定になってしまいます．つまり，回転角が０の時がこの表現方法の特異点ということになりますが，このままではまずいので，次にこれを除去します．
目次のページ
バンドル調整はシリーズ物として書いてまして，目次は下記のページになります．
daily-tech.hatenablog.com
１．バンドル調整 ～ モデル化，及び問題設定
２年前に「最適化計算５ バンドル調整 理論編１」としてエントリを書いていたのですが，今回はこの記事をリライトしています．
バンドル調整とは
三次元シーンを複数のカメラ・視点で撮影した時，全体的に「一番つじつまが合っている．」構成を求めるための手法です．ここで，「つじつまが合う」というのが何なのかを定義してやる必要があります．三次元復元の場合，「再投影誤差を小さくする．」ということがそれに該当します．（うーん．二年前に書いた表現ですが，なんだかうまく書き直せないのでそのままにします．）
バンドル調整のイメージ
３次元点と当該点が各カメラのスクリーンに映った様子を図示します．
それぞれのカメラに関して，３次元座標とスクリーン投影座標には下記の透視投影の関係が成立します．簡単のために，レンズの歪は考えていません．
ただ，ここで成立している関係はそれぞれのカメラ座標系に関してのものです．実際にバンドル調整をするときには基準座標を決めてあげないといけません．今回は簡単のためにカメラ１を基準座標として設定します．基準座標を合わせるために，それぞれの座標系であらわされている点 a の三次元座標を基準座標系（＝カメラ１）で表します．それぞれのカメラの位置がわかっていれば，カメラ間の相対位置関係から次のように変換できます．ここでは，カメラ間の相対位置関係が次のようにわかっているとします．
次に，カメラ間の相対位置関係を用いて点 a の座標変換をします．この変換で，「カメラ k から見た点 a の座標」を「基準座標での点 a の座標」を用いて表せるようになります．
各カメラで成立する透視投影の関係に，上スライドの点 a の座標変換式を代入すると，結果的に下記のようにまとまります．
上スライドの最後の式を用いれば，「カメラの３次元位置」，「カメラの内部パラメータ」，「被写体（点 a）の３次元位置」のすべてがわかっていれば，「被写体が写真のどこに写るか」がわかることになります．この式をもう少し書き出すと，，，
これで，スクリーン投影点の "モデル” ができたことになります．一方で，被写体が画像のどこに写っているかは実測できるので，ここまでの導出で被写体の画像座標に関して「実測値」と「予測値（モデル値）」がわかることになります．ここで，モデルが正確だと仮定すると，点 a の位置，各カメラの内部／外部パラメータが完全にあっていれば実測値と予測値は一致します．逆にずれが大きいと，どこかに誤差があることになります．パラメータの誤差は間接的（実測値と予測値の乖離が大きいか否か）にしかわかりませんが，次に，この「誤差の程度」を定義するために評価関数を定義します．
最小化したい評価関数・誤差関数
ここで「誤差の程度」を定義するために，下記の評価関数を使います．式の定義を見て頂ければわかるように，「実測値」と「予測値（モデル値）」が完全に一致すれば評価関数の値は０になります．つまり，評価関数の値が小さくなるほど，モデルに使用されている値が正確であるということになります．
ここまでの式導出の流れだと，
１．パラメータ（「カメラの３次元位置」，「カメラの内部パラメータ」，「被写体（点 a）の３次元位置」）がわかる．
２．モデルに値を代入して，予測値を計算する．
３．予測値と実測値を評価関数に代入して，どの程度の誤差になるか求める．
４．誤差が大きいので，手修正で（笑）ちょっとパラメータをいじってみる．そして２にもどる（笑）
だと思うのですが，バンドル調整では誤差関数の最適化をすることによって「手修正によらず（笑）」最適パラメータを求めます．うーん．いつものことながら，うまいことできているなあと感心します．
今回の問題設定のイメージ
今回の問題設定のイメージは，下記になります．
３次元空間では下記の様な設定になってまして，それぞれのカメラで撮影した円環の画像も載せています．
問題の前提
問題があまり複雑になるといけない一方で，実際に使用する仮定と大きくずれると良くないので，下記の前提でバンドル調整を実施しました．
下記の３点を既知とする．
１．画像に写っている点の画像座標．
２．３台のカメラの位置・角度．（バンドル調整を実施する前に，この値に誤差を載せます．）
３．円弧状の点の３次元位置．（バンドル調整を実施する前に，この値に誤差を載せます．）
カメラの内部パラメータは２種類あり，カメラ１とカメラ２が同じものを共有．カメラ３のみ異なる内部パラメータを保持．
ということで次のエントリに続きます．．．
目次のページ
バンドル調整はシリーズ物として書いてまして，目次は下記のページになります．
daily-tech.hatenablog.com
０．バンドル調整 〜 目次
ご無沙汰しております！社長です！「月に１本のブログアップ」といった新年の誓いもなんのその（笑），更新せずに半年たっちゃいましたが，せっかくの自粛GWを活かして勉強したことまとめ書きします！ということで，以下，エントリです∠(｀・ω・´)
春だ，桜だ，バンドル調整だ～．ということで，牛歩のように進めている一人SFMプロジェクト，やっとこさバンドル調整まで来ました（笑）仕事でも最適化が使いこなせるようになりたいと思っていたことも有り，少しずつ勉強していたのですが，ちょうどいい機会なのでバンドル調整の文脈で最適化のエントリを書いてみたいと思います．このページは目次として書いていますが，個々のページが書き終わった段階でここにリンクを貼りたいと思います．心が折れなければ，，，，最後まで書ききれると思います！
ちなみに，どの文献をよんでも「ヘッシアンの計算は大変なので，，，」とあったので，力ずくで計算してみました．どろんこになりながら実装したので，少しは最適化を身近に感じるようになりました（笑）
Ｘ．モデル化，及び問題設定
バンドル調整の簡単な説明，モデル化，最小化する数式を説明します．
daily-tech.hatenablog.com
Ｘ．回転のパラメータ化
回転行列のエントリは９つ（３ｘ３）ありますが，回転の自由度は３なので，回転行列以外の方法でパラメータ化しなければいけません．今回は，Angle-Axisを用いてパラメータ化しています．
daily-tech.hatenablog.com
Ｘ．Sympy を使った微分計算
ヤコビアン，ヘッシアンともに近似をつかわず（！）計算しています（笑）さすがに手計算はむりだったので，Sympyを使っています．
daily-tech.hatenablog.com
Ｘ．勾配ベクトルとヘッシアンの作成
バンドル調整における典型的なヘッシアンの構造を踏まえて，勾配ベクトル／ヘッシアンを作ります．
daily-tech.hatenablog.com
Ｘ．直線探索
最急降下法・ニュートン法を実行する上で必要となる直線探索のアルゴリズムを調べています．
daily-tech.hatenablog.com
Ｘ．ニュートン法を用いた解法
力ずくで計算したヘッシアンを使って，最適化をやってみました．ただ，簡単には収束せず．．．
Ｘ．レベンバーグ・マルカート法を用いた解法
非線形最小二乗法のデファクトスタンダードである，レベンバーグマルカート法を試しました．
Ｘ．Ceres-Solver を用いた解法
最後に，，，同じ問題設定を Ceres-Solver を用いて解きました．ヤコビアンもヘッシアンも計算せず，あっという間ですね．．．
Ｘ．参考文献
Bundle Adjustment – A Modern Synthesis
SBA: A software package for generic sparse bundle adjustment
Numerical Optimization (Springer Series in Operations Research and Financial Engineering)
- 作者:Nocedal, Jorge,Wright, Stephen
- 発売日: 2006/06/01
- メディア: ハードカバー
- 作者:健一, 金谷
- 発売日: 2005/09/01
- メディア: 単行本
Ｘ．実験に使ったソースコード
ソースコードはまだちょっと整理中でして，個々のエントリのアップする時には間に合わせるようにします．
github.com
２０２１年と鳩の糞
あけましておめでとうございます！
年も明けた１月１日，クリスティアーノ・ロナウドばりの腹筋を維持するために，ランニングと筋トレに嫌々（笑）出かけたのですが，いつものランニングコースを走っているとなにやら上から落下物が．．．．「まさか！」と思ったのですが，鳩の糞でした．．．
うーん．これはどう解釈すればいいの？と思ってたんですが，ネットでちょっと調べてみたところ，鳩の糞はどうやら幸運のサインみたいです（笑）ということで，２０２１年は自分にとってかつてない豊作の年に，，，なればいいなあ．
やりたいことリスト！
ブログ書き始めて４年くらいになってまして，過去のエントリを振り返ってみてたんですが，「やりたいこと！」って書いてあることほとんど終わってないんですよね（笑）実際に結構時間かけてやってることもあるんですが，なかなかまとまったとこまでたどり着くのは大変で．．．なので，期限を切らずに（笑）じっくりやっていきたいと思います．
【仕事ネタ】
１．１人オフィスの近くに引っ越し．（流山市ー＞市川市）
通勤時間がもったいないので，南流山から引っ越します！南流山自体は大分気に入ってたので残念なのですが．横浜に戻るのもありかと思ったんですが，都内への移動が時間かかるのでおそらく行徳とか浦安とかになりそうです．
２．会社のロゴ作成
引っ越したら名前をちょっとづつ出していきます！
３．税理士を見つける．
４．初決算！
初決算，自分でやってみようか悩んでたんですが，法人決算は結構難しいらしく，ミスって追徴課税におびえるのもつらいので，税理士を探すことにしました．ただ，キャッシュフローがめっちゃシンプルなので，ちょっともったいないなあ．．．という思いもあるのですが．
５．お客さん＆売上を増やす！
完全に仕事漬けの２０２１年にするか，，，どうするか．．．
６．海外から仕事をとってくる！
今すぐは無理ですが，今後の目標に．
【勉強ネタ】
１．つくばチャレンジのスポンサーになる．
２．つくばチャレンジ完走．
コロナ次第だと思うのですが，今年実施されれば参加します！今年は会社で参加する（＝宣伝目的含む）ので，本気です（笑）
３．SFMをやりきる．
うーん．やっている．が，終わらない．
４．ディープラーニングを勉強する．
うーん．やりたい．が，始められない．
５．数学力強化！
現在進行形です！近いうちに何かまとめを残したい(/・ω・)/
６．月に１本のブログアップ
記憶の定着のために，，，，月に一つは勉強したことまとめたい．
でやりたいこと，見ての通り【仕事ネタ】と【勉強ネタ】に分かれるんですが，「１人会社でどこまで売り上げられるか！」に挑戦するか，「仕事をある程度の量に抑えて，勉強しつつ会社の名前を売っていく方向にもっていく」か．．．をちょっと悩んでいます．売上アゲアゲ方向にもっていくと，完全に１年間仕事漬けになることと，どうしても時間の切り売り的側面が強くなってしまう一方で，せっかく会社作ったので，どこまでお金稼げるかやってみたい気持ちもあり（笑）ただ，一回やり始めると途中下車できないので，悩み中です．
２０２１年！
ということで，今年もいままで通り，
All life is an experiment. The more experiments you make the better.
Ralph Waldo Emerson
をモットーに，いろんな実験をしていけたらと思います！ではでは，今年もよろしくお願いします！
ビックマック
前を向きたいとき，僕はビックマックを食べる．
仕事が終わらず，2.5㎡のオフィスでコードとにらめっこしていた時，僕はビックマックを食べた．
クリスマスに誰も出迎えてくれなかったときも，僕はビックマックを食べた．
「年末，休めないんじゃね．．」と不安になりながらも，僕はビックマックを食べた．
この先も，何度もくじけそうになるだろう．だから，これからもよろしく．
ということで，仕事終わったーーー！！！なんとか冬休みを迎えられそうです．
仕事納めのご褒美として，久々にビックマックを食べてみました．「ご褒美にマックって，どんな貧乏社長やねん」って言われそうですが，お金的にというよりは，カロリー＆背徳感的なご褒美ですかね．
結局今年はあんまりブログ書けませんでしたが，，，，冬休みはちょっと頑張ってアップできればと思います(ﾟДﾟ)ﾉ
オフィス
どうもどうも，社長です（笑）．
近況
ちょっと更新が滞ってしまったんですが，実は７月頭から会社が始動しておりまして，せっせと働き始めてます．だいぶスタートダッシュをかまさないといけない状況でありまして（笑），ブログの更新頻度がちょっと落ちてしまうと思うのですが，ちょこちょことアップしますのでまた暇なときに見ていただければありがたいですm(__)m
（今年はつくばチャレンジも不参加なので，SFMもなんとかやり切ります！）
オフィス契約
勢いで法人を立ち上げ，一人会社なので自宅を登記したのですが，これ，国税庁のホームページで住所出てくるんですよね．．．登記後にそれに気づいたんですが，後の祭りでして．．．会社のホームページ立ち上げたり，ブログで会社名を公表しようと思ってたんですが，身に覚えのないピザとかがいっぱい届いたら困るので（笑）オフィスを借りて，住所を登記しなおすことにしました．
で，まともなオフィスはさすがに高いので，レンタルオフィスを借りることにしました．残念ながら南流山近辺にはなかったので，ビズサークルという会社が運営している市川妙典のレンタルオフィスにしました．南流山からは少し離れているので，マンション更新のタイミングで市川市に引っ越そうと思います．ということで，なかなか港区民への道のりは遠そうです（笑）
肝心のオフィスですが，共益費込みで月額 32000 円でして，何とかやりくりできそうな額です．下にオフィスの写真を載せてます．自分でいうのもなんですが，あまりの ”おままごと” っぷりに自然と笑みがこぼれます（爆）が，今をときめく Apple もガレージから始まったことを考えると，先はどうなるかわかりませんよ～．目指せ１００兆円起業 ( ﾟДﾟ)
訪問調査
ある程度の規模の会社になると，初回取引では相手法人の信用調査をします．ただ，法人調査部門を会社毎に持つことは難しいので，普通は調査会社に外注します．日本の場合だと，東京商工リサーチと帝国データバンクの２社が有名どころだと思います．
実は，会社（＝自宅）に東京商工リサーチの調査員の方が来ました！訪問調査というよりは，家庭訪問みたいな形になってしまったんですが，一応となりのコンビニでおーいお茶を購入して，丁重にもてなしました．ということで僕の会社，企業情報データベースに乗ってます！スコアはおそらく最低レベルだと思いますが（笑）
それでは，今後ともわが社をよろしくお願いしますm(__)m
（まだ先になりそうですが，会社住所の登記変更が完了したら，社名＆ホームページも公開します！）
