４．バンドル調整 〜 勾配ベクトルとヘッシアンの作成
このエントリでは，バンドル調整のヘッシアンが典型的にはどのような構造になるかを見てみます．その後，それに基づいて勾配ベクトルとヘッシアンを作ります．
ニュートン法からヘッシアンを考える．
上記の通り，とくに行列が大きくなってくると「エイや！」で計算してはいけないようです．ということで，今回の実験におけるヘッシアンがどのような構造になっているか見てみます．
勾配ベクトルとヘッシアンの構造
上記が今回のケースでのヘッシアンですが，右下のブロックに白のセルが目立つと思います．下のスライドで説明していますが，パラメータ間につながりがないもの同志で評価関数を微分すると０になるため，これが該当するヘッシアンのセルが０になっています．
例えば，下記のようなケースで二階微分がゼロになってます．
・評価関数を異なるカメラの外部パラメータで微分したもの．
・評価関数を異なる点で微分したもの．
・評価関数をあるカメラの外部パラメータで微分し，当該カメラと異なる内部パラメータで微分したもの．
次に，右下のブロックが Sparse であることを生かして，方程式を解いていきます．
シューアの補行列を使ってニュートンステップの計算
※ここでの議論では，ヘッシアンの正定性はすでに仮定してあります．
シューアの補行列を用いて，巨大な一つの線形方程式群を二つに分割できました．分割するだけだと計算量的にあまりメリットはないのかもしれませんが，ここで右下ブロックの Sparsity を使います．
勾配ベクトルの実装
ちなみに，実際の勾配ベクトルの計算は次の様になりました．ヘッシアンも同じような形で計算したのですが，多いので省きます（笑）
ヘッシアンの計算：
https://github.com/koichiHik/blog_bundle_adjust/blob/main/src/optimization/src/compute_hessian.cc
ヘッシアンに関しては，自分はメモリをすべて確保してから単純にそれぞれの要素を加えていったのですが，０の要素がいっぱいできてしまうのでこれは非効率で，本当は非ゼロの要素にだけメモリを割り当ててニュートンステップを計算するみたいです．
詳しくは下記の論文をどうぞ．．．
SBA: A software package for generic sparse bundle adjustment
勾配の計算部分を少しだけ．．．
// すべてのカメラでループ． for (size_t cam_ext_idx = 0; cam_ext_idx < cam_ext_num; cam_ext_idx++) { // すべての３次元点でループ． for (size_t trk_idx = 0; trk_idx < track_num; trk_idx++) { // 当該カメラに３次元点が写っていれば． if (tracks[trk_idx].count(cam_ext_idx)) { Eigen::Vector2d meas = tracks[trk_idx].at(cam_ext_idx); Eigen::Vector3d x = cameras[cam_ext_idx] * points3d[trk_idx].homogeneous(); Eigen::Vector2d proj = x.hnormalized(); // ０．残差の計算． double u_res = proj(0) - meas(0); double v_res = proj(1) - meas(1); // １．勾配のカメラ外部パラメータ部分の計算 size_t cam_int_idx = extrinsic_intrinsic_map.at(cam_ext_idx); { size_t col_idx = cam_ext_idx * cam_ext_var_num; grad(col_idx, 0) += u_res * du_dtx(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx], points3d[trk_idx], x) + v_res * dv_dtx(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx], points3d[trk_idx], x); // y, z, kx, ky, kz についても同様に計算 ・・・・・ } // ２．勾配のカメラ内部パラメータ部分の計算 { double scale = 1.0; size_t col_idx = cam_ext_num * cam_ext_var_num + cam_int_idx * cam_int_var_num; grad(col_idx, 0) += scale * (u_res * du_dfx(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx], points3d[trk_idx], x) + v_res * dv_dfx(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx], points3d[trk_idx], x)); // y, z, kx, ky, kz についても同様に計算 ・・・・・ } // ３．勾配の点群位置パラメータ部分の計算 { size_t col_idx = cam_ext_num * cam_ext_var_num + cam_int_num * cam_int_var_num + trk_idx * points_var_num; grad(col_idx, 0) += u_res * du_dX(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx], points3d[trk_idx], x) + v_res * dv_dX(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx], points3d[trk_idx], x); // Y，Z についても同様に計算 ・・・・・ } } } }
