Essential Matrix (基本行列) と R, T の復元
SfM のエントリ作成にあたって，せっかくなので構成要素となる小さなアルゴリズムをまとめています．このエントリでは，２つの項目を書き留めておきます．
０．このエントリで扱う内容
１．Essential Matrix (基本行列) とは何か？
２．Essential Matrix がわかった時に，それからカメラモーション (R, T) を取り出す方法．
１．Essential Matrix (基本行列) とは何か？
それでは，まず基本行列の説明です．かなり前にエピポーラ幾何を説明する下記のエントリを書いたんですが，その中で Essential Matrix (基本行列) に触れていました．
で，このエントリの中で，
「視点を変えても同じものが写っていた時には満たされるべき条件・数式があって，それが何か？」を表すのがエピポーラ幾何です．
という記述をしています．で，この表現から一歩進んで数式に落とすべく下記の図を考えてみます．これ以降，このエントリでは正規化画像座標系で考えています．
図中に記入しましたが，
１．２つのカメラの視線ベクトルをどちらかの座標系で表現する．
２．一方の視線ベクトルとカメラの並進ベクトルが作る平面の法線ベクトルを計算する．
３．他方の視線ベクトルと２で計算した法線ベクトルが直行する．
という３つの条件から，最後の数式が出てきます．ただ，この数式のままだとベクトルの外積とかが入ってきててややこしいです．なので，" t X R " の部分をもう少し扱いやすくするために行列演算で置き換えます．下記がその過程と結果です．
上記の結果から，ベクトルの外積演算が行列の積演算に置き換わることがわかりました．行列積への置き換えを使って，エピポーラ幾何から生成した数式を書き換えると，下記のようになります．
で，上図の最後に置き換え結果を記しましたが，この E が Essential Matrix (基本行列) になります．置き換えの過程を見てもわかる通り， E は２つのカメラの位置関係 R, T にのみ依存しています．
２．Essential Matrix がわかった時に，それからカメラモーション (R, T) を取り出す方法．
で，散々回りくどくEssential Matrix (基本行列) の説明とか式導出をしてきましたが，「で，Essential Matrix が求まってなにが嬉しいの？」 という質問にはまだ答えられてませんでした．ミソとなるのは，１の項の最後に書いた「E は２つのカメラの位置関係 R, T にのみ依存しています．」という部分で，画像の対応点を見つけて計算しE がわかれば，
「２つのカメラの位置関係（R, T）がわかる！」
ということになります．２の項では，Eがわかった時に，
「Eからどうやって R と T を計算するか」
という点を見ていきます．
２．１．Essential Matrix の特徴・特性
でEssential Matrixの特徴ですが，全部で３つの特徴があります．うち２つは先ほどの導出からわかります．
で，３つめがちょっとわかりにくいのですが，「０でない２つの特異値の値が同じ」というものがあります．
上記の証明では，歪対称行列を特異値分解すると，下記の形になることを使っています．（が，ここをなんでやねん？するのは諦めました（笑））
で，歪対称行列が上記のように特異値分解できることとがわかったので，あとはここから「０でない２つの特異値が同じで，残りの特異値は０である」ことを示すために，直行行列Wを使って使ってZを変換してあげています．この変換を経て E = U diag(1, 1, 0) W UTRと表すことができて，「０でない２つの特異値の値が同じ」であることがわかりました．
２．２．Essential Matrix からの R, T の計算
２．１ではEssential Matrix を特異値分解すると，E = U diag(1, 1, 0) VT となることがわかりました．
今度は
E が与えられた時に，Eを特異値分解してE = U diag(1, 1, 0) VTが求まったとした時に，そこからどうやって R, T を計算するか？
が問題になります．
ただ，先ほどの証明の過程で出てきた数式を使えば，
E = U diag(1, 1, 0) VT = U diag(1, 1, 0) W UTR
[t]x = UZUT
となることがわかっているので，結果的に求まる解は
１．[t]x = UZUT, -UZUT（もしくは，Uの３つめの列ベクトルでも同じ．）
２．R = UWTVT, UWVT
となります．ここで，Tに関してもRに関しても２つ解がでてきます．これは，特異値分解の箇所で符号が定まらないことが原因で（Rの場合はWとWTの場合で符号が反転します．），どの組み合わせを用いてもここまでの議論が成立します．そのため，最終的に求まるカメラ行列は４つになります．「この４候補の中から正しい解を選択する」というロジックが最後に必要になります．
２．３．Essential Matrix を分解するコードサンプル
下記，別のエントリで出てくる SfM のコードの一部ですが，実際に基本行列を分解して R, T を求めています．分解した結果として，２つづつの並進ベクトル，回転行列を呼び出し元に返しています．
bool decompose_E_to_R_T_internal( const cv::Matx33d& E, cv::Matx33d& R1, cv::Matx33d& R2, cv::Matx31d& T1, cv::Matx31d& T2) { // Use OpenCV SVD. cv::SVD svd(E, cv::SVD::MODIFY_A); // Result check. double singular_value_ratio = std::abs(svd.w.at<double>(0) / svd.w.at<double>(1)); // If two singular values are too far apart, this caluclation fails. if (singular_value_ratio < 0.7 || 1.5 < singular_value_ratio) { return false; } cv::Matx33d W(0,-1,0,1,0,0,0,0,1); R1 = cv::Mat(svd.u * cv::Mat(W) * svd.vt); T1 = cv::Mat(svd.u.col(2)); cv::Matx33d Wt(0,1,0,-1,0,0,0,0,1); R2 = cv::Mat(svd.u * cv::Mat(Wt) * svd.vt); T2 = cv::Mat(-svd.u.col(2)); return true; }
Plane Sweeping Stereo 〜 実装編２（コスト計算と深度計算）
ということで，Plane Sweeping Stereo 最終編です．本家の論文は Plane Sweeping Stereo そのものでなく，魚眼カメラを使って Plane Sweeping Stereo すれば広い FOV を有効に使えるぞ！って話だったんですが，この辺はまた必要に応じておいおいまとめられればと思います．
０．前回までの流れ
下記のエントリで，全体構成が大きく分けて５つのステップからなっていることを説明しました．
daily-tech.hatenablog.com
０．CreatePlanes
設定パラメータを使って，仮想平面の下準備．
１．PrepareBuffer
CUDAのバッファ準備．
２．ComputeAccumulationScales
コスト計算のためのパラメータ計算．
３．AccumulateCostForPlanes
１つ１つの仮想平面に対するコスト計算．
４．UpdateBestPlaneBuffer
最良コストバッファの更新．
５．ComputeBestDepth
深度計算．
で，本質的なところは「０．CreatePlanes」「３．AccumulateCostForPlanes」「５．ComputeBestDepth」の３つになると思うのですが，とっても読みにくくなることを覚悟の上で（笑）まとめたいと思います．
１．仮想平面作成部「０．CreatePlanes」の流れ
平面の方程式は ax + by + cz + d = 0 と表現できるので，ここでは平面の方程式の４係数を収めた４Dベクトルを使って平面を表現しています．
① 最大／最小奥行きを視差単位に換算．
Reference Image のカメラと Source Image のカメラの最大距離を基線長として採用します．
② 仮想平面同士の間隔を計算
仮想平面を視差空間で均等な距離になるように，間隔を求めます．
まず，パラメータで指定された奥行きはメトリックなので，これを視差単位に換算します．この変換では①で計算した基線長を使います．
最小視差＝基線長 * 焦点距離 / 最大奥行き
最大視差＝基線長 * 焦点距離 / 最小奥行き
視差空間でのステップ＝（最大視差ー最小視差）／ 仮想平面の枚数
③ 平面の方程式の係数を求める．
仮想平面の方程式を求めます．仮想平面は Reference 画像の Z 軸に垂直な平面としています．一応導出というか確認的なものを下記の図に書いておいたのですが，平面の係数ベクトルは下記のようになります．
Coeff T = [0, 0, -1, 基線長 * 焦点距離 / (最大視差 - i * 視差空間でのステップ)]
該当部コード
void CreatePlanes(const int ref_cam_idx, std::map<int, vis::mvs::LocalizedCudaImage>& cams, const int num_planes, const double near_z, const double far_z, std::vector<Eigen::Vector4d>& planes) { // Step 1. Calculate max distance between camera and set it as baseline. double baseline = ComputeLargestBaseline(ref_cam_idx, cams); planes.resize(num_planes); const Eigen::Matrix3d& K = cams[ref_cam_idx].first.GetK(); double focal = (K(0, 0) + K(1, 1)) / 2.0; // Step 2. Calculate disparity step for plane. (Disp = Baseline * focal / Z) double min_disp = baseline * focal / far_z; double max_disp = baseline * focal / near_z; double disp_step = (max_disp - min_disp) / (num_planes - 1); // Step 3. Calculate coefficients vector for all hypothetical planes. // All plane is perpendicular to Z axis. for (int i = 0; i < num_planes; i++) { planes[i].setZero(); planes[i](2) = -1; planes[i](3) = baseline * focal / (max_disp - i * disp_step); } }
２．コスト計算部「３．AccumulateCostForPlanes」の流れ
コスト計算部は「ホモグラフィー行列計算」と「コスト計算」の2つのパートからなってます．この関数自体は一つの仮想平面に対して呼ばれることを想定してますが，下記コードを見てもわかるように，すべての「Reference 画像」と「Source 画像」の組み合わせでコストを計算します．その結果をバッファ「m_buffers->common.dev_cost_accum」にひたすら加算していきます．
該当部コード
bool PlaneSweepStereo::AccumulateCostForPlane(const int ref_img_idx, const Eigen::Vector4d& hyp_plane, const double scale0, const double scale1) { const int width = m_buffers->dev_img_map[ref_img_idx].second.GetWidth(); const int height = m_buffers->dev_img_map[ref_img_idx].second.GetHeight(); m_buffers->common.dev_cost_accum.Clear(0); for (const auto& localized_img : m_buffers->dev_img_map) { // Skip if this is reference image. if (localized_img.first == ref_img_idx) { continue; } // Compute homography between to compare reference image and source image with respect to THIS Hopothecal plane. std::vector<float> H; ComputeHomographyToReferenceImage(hyp_plane, m_buffers->dev_img_map[ref_img_idx].first, localized_img.second.first, H); // Compare pix value by using homography matrix calulated above. cuda::mvs::PlaneSweepAbsDiffAccum( H, scale0, m_buffers->dev_img_map[ref_img_idx].second, m_buffers->dev_img_map[localized_img.first].second, m_buffers->common.dev_cost_accum); } // Just visualization. cv::Mat tmp1(height, width, CV_32FC1); cv::Mat tmp2(height, width, CV_8UC1); m_buffers->common.dev_cost_accum.Download((float*)tmp1.data, tmp1.step); tmp1.convertTo(tmp2, CV_8UC1); cv::resize(tmp2, tmp2, cv::Size(), 1.0, 1.0); cv::imshow("After", tmp2); cv::waitKey(10); FilterAccumulatedCost(); return true; }
２．１．ホモグラフィー行列計算部「ComputeHomographyToReferenceImage」の流れ
ホモグラフィー行列の計算部ですが，ここではシンプルに理論式を実装してます．もともと外部パラメータとしてファイルに保存されている値が
Xcamx = [R | T] Xglobal
の R と T なので，このことを踏まえて理論編で記載した下記の数式をコードに落とすと，該当部コードになります．
で，上記のメモを見ると，ホモグラフィー行列は計算の結果下記のようになってます．
H = SrcRRef + 1 / depth * SrcTRef * NTRef
ここで，もともと外部パラメータとして保存されている値を座標変換していくと．．．
SrcRRef = SrcRGlob * RefRGlobT
SrcTRef = SrcRRef * RefTGlob - SrcTGlob
となって，下記の計算で正しいことがわかります．（これがなかなかハマったんですよね（笑））ということで，キモとなるホモグラフィー行列の完成です．ちなみに，簡単のために「同じカメラで撮影した」という前提で進めているのでここではカメラ行列 K には特に深くは触れません．
該当部コード
bool ComputeHomographyToReferenceImage(const Eigen::Matrix<double, 4, 1>& plane, const vis::CameraMatrix<double>& ref_cam, const vis::CameraMatrix<double>& src_cam, std::vector<float>& H) { H.resize(9); const Eigen::Matrix<double, 3, 3> ref_K = ref_cam.GetK(); const Eigen::Matrix<double, 3, 3> src_K = src_cam.GetK(); const Eigen::Matrix<double, 3, 3> ref_R = ref_cam.GetR(); const Eigen::Matrix<double, 3, 3> src_R = src_cam.GetR(); const Eigen::Matrix<double, 3, 1> ref_T = ref_cam.GetT(); const Eigen::Matrix<double, 3, 1> src_T = src_cam.GetT(); const Eigen::Matrix<double, 3, 1> unit_n = plane.head(3); const Eigen::Matrix<double, 3, 3> rel_R = src_R * ref_R.transpose(); const Eigen::Matrix<double, 3, 1> rel_T = rel_R * ref_T - src_T; Eigen::Matrix<double, 3, 3> Hmat = src_K * (rel_R + rel_T * unit_n.transpose() / plane(3)) * ref_K.inverse(); H[0] = (float)Hmat(0, 0); H[1] = (float)Hmat(0, 1); H[2] = (float)Hmat(0, 2); H[3] = (float)Hmat(1, 0); H[4] = (float)Hmat(1, 1); H[5] = (float)Hmat(1, 2); H[6] = (float)Hmat(2, 0); H[7] = (float)Hmat(2, 1); H[8] = (float)Hmat(2, 2); return true; }
２．2．コスト計算部「PlaneSweepAbsDiffAccum」の流れ
次に，２．１で計算したホモグラフィー行列をつかって，コスト計算をします．ここではポイントとなるのは，Reference 画像をラスタスキャンするという点です．主役となるのは Reference 画像で，Reference 画像の (x, y) に該当する Source 画像のピクセル (u, v) をホモグラフィー行列を使って計算することによって画素値を取得し，Reference (x, y) と Source (u, v) の比較をします．
該当部コード（C++）
void PlaneSweepAbsDiffAccum(const std::vector<float> &H, const float accum_scale0, CudaImage &dev_ref_img, CudaImage &dev_src_img, CudaBuffer<float> &dev_cost_buff) { CHECK(H.size() == 9) << "Invalid Homography Matrix."; dim3 grid_dim( GetNumTiles(dev_ref_img.GetWidth(), mvs::PLANE_SWEEP_BLOCK_WIDTH), GetNumTiles(dev_ref_img.GetHeight(), mvs::PLANE_SWEEP_BLOCK_HEIGHT)); dim3 block_dim(mvs::PLANE_SWEEP_BLOCK_WIDTH, mvs::PLANE_SWEEP_BLOCK_HEIGHT); // Bind src image to texture. CUDA_CHECK_CALL(cudaBindTexture2D(0, g_img_texture, dev_src_img.GetDevAddr(), g_channel_texture, dev_src_img.GetWidth(), dev_src_img.GetHeight(), dev_src_img.GetPitch());) float h11 = H[0], h12 = H[1], h13 = H[2], h21 = H[3], h22 = H[4], h23 = H[5], h31 = H[6], h32 = H[7], h33 = H[8]; PlaneSweepAbsDiffAccumGrayScaleKernel<<<grid_dim, block_dim>>>( h11, h12, h13, h21, h22, h23, h31, h32, h33, accum_scale0, dev_ref_img, dev_src_img, dev_cost_buff); // Unbind src image. CUDA_CHECK_CALL(cudaUnbindTexture(g_img_texture);) }
下記，CUDAコードになりますが，並列実行されるので各スレッドが一つのピクセルに対してこの関数を実行する形になります．(x, y) は Reference 画像のピクセル座標なので，dev_cost_buff (コストバッファ) が Reference 画像と同じ要素数を保持することになります．で，Cost(x, y) = Reference(x, y) - Source (u, v)を計算しています．一対の Source 画像と Reference 画像のコスト計算をする場合には dev_cost_buff は競合しないので，ここではロックをかける必要がないです．
該当部コード（CUDA）
__global__ void PlaneSweepAbsDiffAccumGrayScaleKernel( const float h11, const float h12, const float h13, const float h21, const float h22, const float h23, const float h31, const float h32, const float h33, const float accum_scale0, const CudaImage dev_ref_img, const CudaImage dev_src_img, CudaBuffer<float> dev_cost_buff) { // Index unsigned int x = blockIdx.x * blockDim.x + threadIdx.x; unsigned int y = blockIdx.y * blockDim.y + threadIdx.y; const int width = dev_ref_img.GetWidth(); const int height = dev_ref_img.GetHeight(); if (x < width && y < height) { // Apply homography from ref -> src float xw, yw; WarpCoordinateViaHomography(x, y, h11, h12, h13, h21, h22, h23, h31, h32, h33, &xw, &yw); const float u = (xw + 0.5f) / (float)dev_src_img.GetWidth(); const float v = (yw + 0.5f) / (float)dev_src_img.GetHeight(); // Pixel value at corresp source image. const float1 pix = tex2D(g_img_texture, u, v); float diff = fabs((float)(dev_ref_img(x, y) - __saturatef(fabs(pix.x)) * 255)); // Register cost for this pix. // No need to lock here! dev_cost_buff(x, y) += accum_scale0 * diff; } }
３．深度計算部「５．ComputeBestDepth」の流れ
そして最後の深度計算部です．この関数に処理が渡るまでに，Reference 画像のすべてのピクセルに対して，すべての仮想平面の検証が終了し，最良コストを記録した仮想平面の idx が保存されているので，この最良仮想平面の平面の方程式から該当ピクセルの奥行きを決定します．ちょっと CUDA へのメモリ転送の関係で複雑な書き方になってしまってますが，やっていることは最良コストの仮想平面 Idx から奥行きを取得しているだけです．
該当部コード（C++）
void PlaneSweepComputeBestDepth(const Eigen::Matrix3d &K_ref, vis::cuda::CudaBuffer<int> dev_best_plane_idxs, std::vector<Eigen::Vector4d> &planes, vis::DepthMap<float, double> &best_depth) { const int width = dev_best_plane_idxs.GetWidth(); const int height = dev_best_plane_idxs.GetHeight(); const int num_planes = planes.size(); // Make consecutive array of plane coeffs. std::vector<float> plane_coeffs; plane_coeffs.reserve(num_planes * 4); // TODO: Modify for better readability. for (int j = 0; j < 4; j++) { for (int i = 0; i < num_planes; i++) { plane_coeffs.push_back(planes[i](j)); } } // Allocate device memory for planes to upload to GPU. float *dev_plane_addr; size_t dev_plane_pitch; CUDA_CHECK_CALL(cudaMallocPitch(&dev_plane_addr, &dev_plane_pitch, sizeof(float) * num_planes, 4);) CUDA_CHECK_CALL(cudaMemcpy2D(dev_plane_addr, dev_plane_pitch, &plane_coeffs[0], sizeof(float) * num_planes, sizeof(float) * num_planes, 4, cudaMemcpyHostToDevice);) Eigen::Matrix3d K_inv = K_ref.inverse(); float3 K_ref_inv_col1; K_ref_inv_col1.x = K_inv(0, 0); K_ref_inv_col1.y = K_inv(1, 0); K_ref_inv_col1.z = K_inv(2, 0); float3 K_ref_inv_col2; K_ref_inv_col2.x = K_inv(0, 1); K_ref_inv_col2.y = K_inv(1, 1); K_ref_inv_col2.z = K_inv(2, 1); float3 K_ref_inv_col3; K_ref_inv_col3.x = K_inv(0, 2); K_ref_inv_col3.y = K_inv(1, 2); K_ref_inv_col3.z = K_inv(2, 2); // Allocate device memory for best depth to GPU. float *dev_best_depth_addr; size_t dev_best_depth_pitch; CUDA_CHECK_CALL(cudaMallocPitch(&dev_best_depth_addr, &dev_best_depth_pitch, sizeof(float) * width, height);) // Compute best depth dim3 grid_dim(GetNumTiles(width, PLANE_SWEEP_BLOCK_WIDTH), GetNumTiles(height, PLANE_SWEEP_BLOCK_HEIGHT)); dim3 block_dim(PLANE_SWEEP_BLOCK_WIDTH, PLANE_SWEEP_BLOCK_HEIGHT); PlaneSweepComputeBestDepthKernel<<<grid_dim, block_dim>>>( dev_best_plane_idxs, dev_plane_addr, dev_plane_pitch, dev_best_depth_addr, dev_best_depth_pitch, K_ref_inv_col1, K_ref_inv_col2, K_ref_inv_col3); CUDA_CHECK_ERROR // Download result to device. CUDA_CHECK_CALL(cudaMemcpy2D(best_depth.GetDataPtr(), best_depth.GetWidth() * sizeof(float), dev_best_depth_addr, dev_best_depth_pitch, best_depth.GetWidth() * sizeof(float), best_depth.GetHeight(), cudaMemcpyDeviceToHost);) CUDA_CHECK_CALL(cudaFree(dev_plane_addr);) CUDA_CHECK_CALL(cudaFree(dev_best_depth_addr);) }
該当部コード（CUDA）
で，実際の計算部分ですが，論文では奥行きを求める計算は下記のようになってます．
が，この数式は仮想平面がZ軸に対して垂直でないケースも考慮したより一般的な数式になってます．式導出をできればよかったんですが，ちょっと分からず（笑）ただし，今回の場合は仮想平面がZ軸に対して垂直であるという前提で問題を説いているので実際にはとても簡単に奥行きが求まります．というより，そのまま平面の方程式の４番めの係数を使ってあげればOKです．ということで，簡単化したコードを下記に載せてます．
結局，今回の設定だと奥行きは下記のようになります．
// 4番目の係数取得
const float planeD =
*((float *)((char *)dev_plane_addr + 3 * dev_plane_pitch) +
best_plane_idx);
// 4番目の係数をそのまま利用．
*((float *)((char *)dev_best_depth_addr + y * dev_best_depth_pitch) + x) =
planeD;__global__ void PlaneSweepComputeBestDepthKernel( vis::cuda::CudaBuffer<int> dev_best_plane_idxs, float *dev_plane_addr, size_t dev_plane_pitch, float *dev_best_depth_addr, size_t dev_best_depth_pitch, float3 K_ref_inv_col1, float3 K_ref_inv_col2, float3 K_ref_inv_col3) { const int width = dev_best_plane_idxs.GetWidth(); const int height = dev_best_plane_idxs.GetHeight(); unsigned int x = blockIdx.x * blockDim.x + threadIdx.x; unsigned int y = blockIdx.y * blockDim.y + threadIdx.y; // TODO: Modify here for better readability. // Look at each pix in best_plane_idx and calculate depth. if (x < width && y < height) { const int best_plane_idx = dev_best_plane_idxs(x, y); float3 plane_normal; plane_normal.x = dev_plane_addr[best_plane_idx]; plane_normal.y = *((float *)((char *)dev_plane_addr + dev_plane_pitch) + best_plane_idx); plane_normal.z = *((float *)((char *)dev_plane_addr + 2 * dev_plane_pitch) + best_plane_idx); const float planeD = *((float *)((char *)dev_plane_addr + 3 * dev_plane_pitch) + best_plane_idx); #if 0 float3 K_ref_inv_Tn; K_ref_inv_Tn.x = K_ref_inv_col1.x * plane_normal.x + K_ref_inv_col1.y * plane_normal.y + K_ref_inv_col1.z * plane_normal.z; K_ref_inv_Tn.y = K_ref_inv_col2.x * plane_normal.x + K_ref_inv_col2.y * plane_normal.y + K_ref_inv_col2.z * plane_normal.z; K_ref_inv_Tn.z = K_ref_inv_col3.x * plane_normal.x + K_ref_inv_col3.y * plane_normal.y + K_ref_inv_col3.z * plane_normal.z; #endif float3 pos; pos.x = x; pos.y = y; pos.z = 1; #if 0 const float denom = K_ref_inv_Tn.x * pos.x + K_ref_inv_Tn.y * pos.y + K_ref_inv_Tn.z * pos.z; *((float *)((char *)dev_best_depth_addr + y * dev_best_depth_pitch) + x) = -planeD / denom; #endif *((float *)((char *)dev_best_depth_addr + y * dev_best_depth_pitch) + x) = planeD; } }
ということで，Plane Sweeping Stereo でした！ちょっと走り走りになってしまいましたが，SFMとかつくばチャレンジに際して実際に使ったときにもう一度エントリをかければと思います．
Plane Sweeping Stereo 〜 実装編１（全体構成）
ということで，下記エントリの続きです．ここから中身に入っていきます．
０．全体構成
Plane Sweeping Stereo のコードの全体構成ですが，出来る限り論文の流れに合うようにリファクタしました．大きな関数のフローとしては，下記のようになります．
Step 0. CreatePlanes
ここでは仮想平面の準備をしてます．具体的にはパラメータとして設定した「最大奥行き」「最小奥行き」「仮想平面枚数」の３つのパラメータと「焦点距離」「カメラ間距離の最大値」を元に仮想平面を設置する間隔を計算します．
Step 1. PrepareBuffer
求まったパラメータをベースに，GPUのデバイスメモリを確保します．ちなみに，本家のライブラリではZNCCとかSub Pixelとかもサポートしてるんですが，CUDA の周りがとっても複雑になるのでまずは簡単のために「SAD, Sub Pix無し」の一番簡単なパターンをやってみました．
Step 2. ComputeAccumulationScales
コスト計算のためのパラメータ計算です．SADの場合は，１つのパラメータだけ計算してます．
※Step3 と Step4 はすべての仮想平面に対して計算します．
Step 3. AccumulateCostForPlane
この関数が論文のメインの内容になりますが，検証対象となっている仮想平面上で Reference 画像 と Source 画像のマッチングを実施してます．マッチングの手順としては，
① Source 画像（≠Reference 画像）とReference 画像を仮想平面上で比較するためにホモグラフィー行列を求めます．
② ①で求めたホモグラフィー行列を使って，Reference画像とSource画像の対応するピクセル１つ１つに対してSADを計算します．
③ ②の計算をReference画像の全ての画素に対して実施した時点で，Reference 画像の一つ一つのピクセルに対してコストが計算されています．
Step 3でやっていることを簡単にイメージにしてみました．
Step 4. UpdateBestPlaneBuffer
Step 3で特定の仮想平面に対するコスト計算が完了したので，今までに計算された最良（＝最小）コストと今回計算された仮想平面のコストをピクセル１つ１つに対して比較します．今回計算したコストが最良であれば，そのコストと平面の番号を記録しておきます．
Step 4でやっていることを簡単にイメージにしてみました．
Step 5. ComputeBestDepth
ループを抜けて，すべての仮想平面に対する検証が完了しているはずなので，記録してある最良コストを確認します．ピクセル１つ１つに対して，最良コストだった平面の番号が記録されているので，そのピクセルの奥行きはその平面の奥行きとなります．
該当部コード
bool PlaneSweepStereo::Run(const int ref_img_idx) { LOG(INFO) << "PlaneSweepStereo::Run()"; // Step 0. Prepare Hypothetical Planes CreatePlanes(ref_img_idx, m_buffers->dev_img_map, m_params.num_planes, m_params.min_z, m_params.max_z, m_buffers->hyp_planes); // Step 1. Buffer Preparaion. PrepareBuffer(ref_img_idx); // Step 2. Compute Accumulation Scale double accum_scale0, accum_scale1; CompuateAccumulationScales(accum_scale0, accum_scale1); for (unsigned int plane_idx = 0; plane_idx < m_buffers->hyp_planes.size(); plane_idx++) { // Step 3. Compute Cost for THIS plane. AccumulateCostForPlane(ref_img_idx, m_buffers->hyp_planes[plane_idx], accum_scale0, accum_scale1); // Step 4. Update Best Plane Buffer. UpdateBestPlaneBuffer(plane_idx); } // Step 5. Result Calculation. ComputeBestDepth(ref_img_idx); return true; }
ちょっと短いですが，次のエントリで細かいとこを書き留めて最終編にしたいと思います！
Plane Sweeping Stereo 〜 実装編０（テストコードと入出力）
関東に帰ってきてしまいました．．．
帰ってきて速攻フリーランスの面談があったのですが，やっぱりなかなか緊張しますね．お腹を下す，，，ほどではなかったものの，前日はそわそわしてました．ただ，仲介してくれている営業さんのフォローや，先方の方にも割とフランクに面談をしていただけたこともあって，うまく行ったかな？と思います．あと数件面談が残ってますが，それが終わって，面談結果をもらって，８月からは社会復帰ですね (｀・ω・´)ｼｬｷｰﾝ
８月までの貴重な時間で，沖縄で勉強したことをまとめておければなと思います．時間が経つと見事に忘れてしまうので．．．ということで，理論編まで書き残していた Plane Sweeping Stereo の実装編です．
１．ビルド＆実行方法
”ちょっとリファクタバージョン”のビルド方法ですが，自分が Ubuntu 16.04 を使っていることもあって，デフォルトでついてくる CMake, Eigen, CUDA とかの組み合わせがちょっと古くうまくコンパイルできなかったので，部分的に新しい物をダウンロードしてきて使ってます．クローンしたリポジトリに "build.sh" ってファイルが入っていると思うのですが，こいつのパスに適切なもの設定して，あとはビルド＆実行で動く．．．はず！
Git Clone
git clone git@github.com:koichiHik/PlaneSweepLib.git cd PlaneSweepLib
で，build.shのパスを少し変えてあげて．．．
ビルド
クローンしたフォルダのままで，
sh build.sh
実行
ビルドしたフォルダのままで，
../build/bin/plane_sweep_stereo_test
実行まですんなり行けば，下記の様なコスト計算過程の画像と視差画像が表示されます．
２．実行結果
コスト計算の過程
コストが低い（＝画像が暗い）ほど仮想平面との一致度が高いことを意味してます．カメラに近い仮想平面から遠い仮想平面へ順にコスト計算をしてますが，近くから遠くへ向けて一致度が高い（＝画像が暗い）部分が移動していくことがわかります．
視差画像
最終的に算出された視差画像です．今回はコスト計算にSADを使っていることと，Occlusion対策をしてないのであんまり綺麗ではないですが，本家のライブラリにはすべて実装されてまして，実行時に選べます．
３．実験データ
実行に使っている実験データですが，git clone したフォルダの配下にある "data" というフォルダにも入っています．ピンホールカメラ用のデータとしては３つほどのデータを提供してくれてあるのですが，どのデータフォルダにも下記のファイルが入っています．
０．K.txt
内部パラメータの "fx, アスペクト比, cx, fy, cy " がこの順番で入っています．
１．image.txt
フォルダの中に画像そのものが入ってますが，そのファイルリストがこのファイルになります．このリストの順番はカメラの外部パラメータリストファイル "model-0-cams.txt" と一致してまして，並び順が何気に重要です．
２．model-0-cams.txt
カメラの外部パラメータリスト．各画像間のホモグラフィー行列を計算する必要があるので，各写真を撮影した時の相対位置関係が必要です．このファイルに記載されている内容はどこかのグローバル座標に対するカメラ行列の R と T になります．つまり，，，，
Xcamx = [R | T] Xglobal
です．このRとTがどっちからどっちの変換かわからなくなって，とってもハマったんですよね．．．
４．実験コード
中身の解剖は次のエントリからにするとして，実験コードのザクっとした内容です．
Step １．Extract all necessary file paths
実行時の引数に応じて適当なファイルをロードします．異なるデータセットを試したい場合は，下記みたいな感じで指定してもらえば提供してくれているデータセットにたいしては動きました．
../build/bin/plane_sweep_stereo_test --data_folder_path=./data/pinholeCamera/niederdorf2
Step ２．Load necessary files. (K matrix.)
内部パラメータ・外部パラメータの読み込みです．
Step ３．Parameter settings.
ここがちょっとミソになる部分なんですが，ここで最大奥行き・最小奥行きの設定をしています．本家のコードでは読み込んだ外部パラメータから各カメラの平均距離を計算して，下記のように計算してました．
最大奥行き：各カメラの平均距離 X 100.0
最小奥行き：各カメラの平均距離 X 5.0
この「5.0」とか「100.0」っていうのが結構マジックナンバーに思えたんですが，自分が持ってるステレオカメラ（ZED）の基線長が 12cm で，
計測最大奥行き：12 ~ 13m（≒基線長 x 100）
計測最小奥行き：0.5m ~ 1m（≒基線長 x 5 ~ 10）
ということを考えると，大体その程度のレンジの数字になるのかなと思います．
Step ４．Initialize algorithm
ここでパラメータの設定と，画像の Cuda メモリへの転送をしてます．
Step ５．Run algorithm.
アルゴリズムの実行部分で，次回以降のエントリでここをふかぼっていきます．
Step ６．Visualize final result
可視化．．．
Step ７．Terminate algorithm.
終了処理．．．
int main(int argc, char **argv) { // 0. System setup. google::ParseCommandLineFlags(&argc, &argv, true); FLAGS_alsologtostderr = 1; FLAGS_stderrthreshold = google::GLOG_INFO; google::InitGoogleLogging(argv[0]); // 1. Extract all necessary file paths. std::vector<std::string> k_mat_file, cammat_file, image_file_paths; { RaiseAllFilesInDirectory(FLAGS_data_folder_path, k_mat_file, std::vector<std::string>{FLAGS_kmat_file_name}); CHECK_EQ(k_mat_file.size(), 1) << "Need 1 K.txt file."; RaiseAllFilesInDirectory(FLAGS_data_folder_path, cammat_file, std::vector<std::string>{FLAGS_cammat_file_name}); CHECK_EQ(cammat_file.size(), 1) << "Need 1 cammat file"; RaiseAllFilesInDirectory(FLAGS_data_folder_path, image_file_paths, std::vector<std::string>{".JPG"}); CHECK_GT(image_file_paths.size(), 1) << "Need at least 2 image files."; } // 2. Load necessary files. (K matrix.) Eigen::Matrix3d K = LoadKMatrixFromFile(k_mat_file[0]); std::map<int, vis::CameraMatrix<double>> cammap = LoadCameraMatrixFromFile(cammat_file[0], K); // 3. Parameter settings. // Specify image idxes to be used. std::vector<int> image_idx_to_be_used{0, 1, 2}; vis::mvs::PlaneSweepStereoParams params; params.img_scale = 0.25; CalculateMinMaxRange(image_idx_to_be_used, cammap, params.min_z, params.max_z); // 4. Initialize algorithm vis::mvs::PlaneSweepStereo pss; pss.Initialize(params); for (auto idx : image_idx_to_be_used) { cv::Mat img = cv::imread(image_file_paths[idx]); CHECK(!img.empty()) << "The image " << image_file_paths[idx] << " does not exist."; pss.AddImage(img, cammap[idx]); } // 5. Run algorithm. int ref_img_idx = image_idx_to_be_used[image_idx_to_be_used.size() / 2]; pss.Run(ref_img_idx); // 6. Visualize final result cv::Mat depth_map; pss.DrawColoredDepthMap(depth_map); cv::imshow("Depth Map", depth_map); cv::waitKey(0); // 7. Terminate algorithm. pss.Finalize(); return 0; }
ということで，まずはさわりのとこしかかけませんでしたが，次のエントリで中身に入っていきます．
ANA470便
ということで，，，那覇空港離陸20分前です．．．
未だに信じ難いデスが，沖縄プチ移住，今日が最終日です．なんだか二ヶ月早かったなー．
アップする記事はたくさんあるので帰ってから少しずつ上げていきたいと思います！
沖縄病
ちなみに，沖縄病と呼ばれる恐ろしい病気があるのですが，
【沖縄病】おきなわ-びょう
沖縄に魅せられ、思考や行動が沖縄中心となる病。沖縄をもっと知りたい／理解したい／深めたいという循環に陥り、症状が進むと一定の期間及び短いスパンで沖縄を訪れるようになる。
詳細は，下記のサイトを参照のこと（笑）
www.kakone.net
自分，沖縄病発症してしまいましたね．まあ，，，沖縄で半年滞在＆仕事したら完全に治ると思いますが（笑）自分の場合，1年のうち10ヶ月は関東で，2ヶ月沖縄が最高のバランスですかね．ただ，5月，6月は梅雨のせいで湿度が毎日90％越えてたので，意外と冬に来るのがいいかも．
プチ移住第二弾！
次のプチ移住は，
1. ハワイ
友情出演していただいた皆様
1. ジモトモのN氏
2. おかん（爆）
3. 元会社先輩W氏
4. 元々&元会社同僚C家様
5. 元会社後輩Y氏，G氏，S氏
C家様以外のメンバーが来沖した時には，なんだかんだ少しは晴れたんですが，，，C家様の時はずっと雨で，，，なんだかすみませんでしたm(_ _)m（次のハワイで挽回や！）
ということで，次回の開催は2年~3年後と予想されます！国内，海外関わらず，万障お繰り合わせの上お越し下さい！
Hatenaブログからお知らせメールが来て気づいたんですが，ちょうど一年前は何をやっていたかというと，フランクフルト空港から帰国してました．
ちなみに，この時は仕事をやり遂げて，（終わってなかったかも？）「エトピリカ」を聞きながら情熱大陸気分に浸ってたんですが，今回はそんな感じじゃないですね．「バカンスしてたんやから，当然やろ！」って突っ込まれそうですが（笑）今の気分は，，，そうですね，ケツメイシの「歩いてく」ですかね．「仕事きまってないけど〜．歩いてく〜」みたいな（笑）
ということで，那覇空港からでした！皆様，次はハワイでお会いしましょう！！
魚眼カメラのキャリブレーション 〜 実装編２（再投影誤差の最小化）
相変わらず順番前後してますが，魚眼カメラのキャリブレーション最終編です．
０．前回までの流れ
下記のエントリで，全体構成が大きく分けて５つのステップからなっていることを説明しました．
daily-tech.hatenablog.com
１．InitializeParamsBasedOnFlagSetting
キャリブレーションフラグの設定値読み取り．
２．CalibrateExtrinsics
各画像撮影時のキャリブボードとカメラの位置計算．
３．MinimizeReprojectionError
すべての画像に対する再投影誤差の「和」の最小化．
４．EstimateUncertainties
キャリブの結果まとめ．
５．ConvertFormat
フォーマットの変換．
で，キャリブレーションの本質的なところは「２．CalibrateExtrinsics」「３．MinimizeReprojectionError」になります．他のステップは設定値の読み取りやフォーマットの変換的な箇所です．前回の下記エントリで，
daily-tech.hatenablog.com
「２．CalibrateExtrinsics」の内容を書いたので，このエントリでは「３．MinimizeReprojectionError」の部分を見ていきます．ちなみに，前回のエントリでもガウスニュートン法を使った最適化が出てきましたが，前回のパートでは一枚の画像に写っているキャリブボードに対して最適化してます．今回のエントリでは，「２．CalibrateExtrinsics」で求めたすべてキャリブボードの画像に対しての誤差の「和」が最小になるように最適化します．
１．今回の処理「３．MinimizeReprojectionError」の流れ
コード内にもざっくりとステップを書いたんですが，後述４ステップの繰り返しが再投影誤差最小化の大きな流れになります．ちなみに，この再投影誤差最小化のパラメータベクトルは下記になります．キャリブ対象となるカメラが１つなので，内部パラメータは９個固定ですが，外部パラメータは撮影枚数分存在するので６個 x キャリブボードの画像枚数となります．
最適化対象となるパラメータ（９個 ＋ ６個 x 画像枚数）
内部パラメータ（合計９個）
fx, fy : 焦点距離
cx, cy : カメラの主点
k1, k2, k3, k4 : 魚眼モデルのパラメータ（詳しくは理論編を参照くださいませ．）
外部パラメータ（合計６個 x キャリブボードの画像枚数）
r1, r2, r3 : キャリブボードのロドリゲスの各要素．
t1, t2, t3 : キャリブボードの並進の各要素
再投影誤差最小化の４ステップ
Step 1 : Condition check for loop termination.
最適化計算のループ終了条件チェックです．
Step 2 : Compute jacobian and gradient in parameter space. (Gauss Newton 1)
最適化対象となっているパラメータ空間でのヤコビアンを計算．ヤコビアンからヘッシアンを近似して，特異値分解を使うことによってパラメータ空間の最急降下ベクトルを計算します．Gauss Newton法の前半部分です．
Step 3 : Update parameter by d_param * alpha_smooth2. (Gauss Newton 2)
最急降下ベクトルによる更新をパラメータベクトルに対して反映し，このターンでのGauss Newton法を終了します．Gauss Newton法の後半部分です．
Step 4 : Recomputing extrinsics based on the updated parameter sets.
パラメータベクトルが更新されたので，これを元に再度外部パラメータを更新します．
該当部コード
bool MinimizeReprojectionError(const cv::TermCriteria& criteria, const vector<vector<cv::Point3f>>& objectPoints, const vector<vector<cv::Point2f>>& imagePoints, const int check_cond, const double thresh_cond, const int recompute_extrinsic, IntrinsicParams& final_param, std::vector<Vec3d>& board_rotations, std::vector<Vec3d>& board_translations) { double updated_ratio = 1; cv::Vec2d err_std; for (int iter = 0; iter <= std::numeric_limits<int>::max(); ++iter) { // Step 1 : Condition check for loop termination. if ((criteria.type == 1 && iter >= criteria.maxCount) || (criteria.type == 2 && updated_ratio <= criteria.epsilon) || (criteria.type == 3 && (updated_ratio <= criteria.epsilon || iter >= criteria.maxCount))) { break; } // Step 2 : // Compute jacobian and gradient in parameter space. (Gauss Newton 1) Mat d_param; { Mat JtJ, Jtex; #if 1 ComputeJacobians(objectPoints, imagePoints, final_param, board_rotations, board_translations, check_cond, thresh_cond, JtJ, Jtex); #else ComputeJacobiansNaively(objectPoints, imagePoints, final_param, board_rotations, board_translations, check_cond, thresh_cond, JtJ, Jtex); #endif solve(JtJ, -Jtex, d_param); } // Step 3 : // Update parameter by d_param * alpha_smooth2. (Gauss Newton 2) { const double alpha_smooth = 0.4; double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0); IntrinsicParams temp_param = final_param + alpha_smooth2 * d_param; updated_ratio = norm(Vec4d(temp_param.f[0], temp_param.f[1], temp_param.c[0], temp_param.c[1]) - Vec4d(final_param.f[0], final_param.f[1], final_param.c[0], final_param.c[1])) / norm(Vec4d(temp_param.f[0], temp_param.f[1], temp_param.c[0], temp_param.c[1])); final_param = temp_param; } // Step 4 : // Recomputing extrinsics based on the updated parameter sets. if (recompute_extrinsic) { CalibrateExtrinsics(objectPoints, imagePoints, final_param, check_cond, thresh_cond, board_rotations, board_translations); } } return true; }
２．Step 2 : ヤコビアン計算とパラメータ空間での最急降下ベクトル計算
ちょっとヤコビアンの計算部分が大きいですが，，，書き切ります．まず，全体のコード概要です．このブロックでは，問題全体に対する近似ヘッシアン「JtJ」とヤコビアンｘ誤差ベクトル「-Jtex」を計算します．で，「JtJ * d_param = -Jtex」というガウスニュートン法の線形方程式をときます．線形方程式を解いているのが「solve(JtJ, -Jtex, d_param)」の部分になります．
該当部コード
// Step 2 : // Compute jacobian and gradient in parameter space. (Gauss Newton 1) Mat d_param; { Mat JtJ, Jtex; #if 1 ComputeJacobians(objectPoints, imagePoints, final_param, board_rotations, board_translations, check_cond, thresh_cond, JtJ, Jtex); #else ComputeJacobiansNaively(objectPoints, imagePoints, final_param, board_rotations, board_translations, check_cond, thresh_cond, JtJ, Jtex); #endif solve(JtJ, -Jtex, d_param); }
２．１ ヤコビアンの計算ロジック
では，ComputeJacobiansという関数を見ていきたいと思います．この関数はすべてのパラメータ（内部パラメータ９個＋外部パラメータ６個ｘ画像枚数）に対するヤコビアンから近似ヘッシアン（JtJ）を計算し，またすべての画像のすべての検出コーナーに対する誤差ベクトルを生成し，誤差ベクトルとヤコビアンから（Jtex）を計算します．数式，理論的なところは，ホモグラフィー計算の時にもお世話になりましたが，下記のスライドのP18にわかりやすくまとめてくれてあります．
www.slideshare.net
で，入れ子になってしまうので，とっても見難くなるんですが，この関数は下記のとおりです．このエントリでは，Step1，Step3，Step4，Step5 を説明します．
Step 1 : Create approxiomated hessian matrix and error vector.
問題全体（内部パラメータ９個＋外部パラメータ６個ｘ画像枚数）に対する近似ヘッシアンとヤコビアン x 誤差ベクトルの領域を確保します．
Step 2 : Convert format.
あとの計算で都合がいいように行列の形とかフォーマットを変換します．
Step 3 : Compute reprojection error and jacobians at projected point.
Step3, Step4, Step5 がおそらく今回のキモですが，Step3でやっていることは大きく２つあります．
①推定内／外部パラメータを用いて，キャリブの３次元点を画像に投影．実際の検知点と比較することで再投影誤差ベクトルを計算します．
②再投影点のヤコビアンを計算します．
Step 4 : Extract jacobians elements of this image.
Step3で求めたヤコビアン，再投影誤差ベクトルは特定の画像に対してのものですが，これを「問題全体に対する近似ヘッシアン」と「問題全体に対するヤコビアン x 誤差ベクトル」に組み込まないと行けないので，内部パラメータのヤコビアン（J_int）と外部パラメータのヤコビアン（J_ext）に分割します．
Step 5 : Update jacobians and error vector for whole problem.
Step 4で分割した内部パラメータのヤコビアン（J_int）と外部パラメータのヤコビアン（J_ext）を「問題全体に対する近似ヘッシアン」と「問題全体に対するヤコビアン x 誤差ベクトル」に組み込みます．
Step 6 : Shrink matrix if some parameters are not to be estimated.
最適化対象外のパラメータを行列から削除しないといけないので，設定パラメータに応じて不要な部分を「問題全体に対する近似ヘッシアン」と「問題全体に対するヤコビアン x 誤差ベクトル」から削除します．
該当部コード
void ComputeJacobians(const vector<vector<cv::Point3f>>& objectPoints, const vector<vector<cv::Point2f>>& imagePoints, const IntrinsicParams& param, vector<cv::Vec3d>& board_rotations, vector<cv::Vec3d>& board_translations, const int& check_cond, const double& thresh_cond, Mat& JtJ, Mat& Jtex) { int image_num = (int)objectPoints.size(); // Step 1 : Create approxiomated hessian matrix and error vector. // 9 Intrinsic Params + 6 Extrinsic Param * Board Number. JtJ = Mat::zeros(9 + 6 * image_num, 9 + 6 * image_num, CV_64FC1); Jtex = Mat::zeros(9 + 6 * image_num, 1, CV_64FC1); for (int image_idx = 0; image_idx < image_num; ++image_idx) { // Step 2 : Convert format. cv::Mat object_points_mat, image_points_mat; ConvertVector2dToMat(objectPoints[image_idx], imagePoints[image_idx], object_points_mat, image_points_mat); // Step 3 : Compute reprojection error and jacobians at projected point. Mat jacobians, reproj_err; { Mat projected_point_mat(cv::Size(2, object_points_mat.rows), CV_64FC1); ProjectPoints(object_points_mat, projected_point_mat, Mat(board_rotations[image_idx]), Mat(board_translations[image_idx]), param, jacobians); reproj_err = projected_point_mat - image_points_mat; } // Step 4 : Extract jacobians elements of this image. Mat J_int, J_ext; { J_int = Mat(jacobians.rows, 9, CV_64FC1); // fx, fy, cx, cy jacobians.colRange(0, 4).copyTo(J_int.colRange(0, 4)); // alpha jacobians.col(14).copyTo(J_int.col(4)); // k1, k2, k3, k4 jacobians.colRange(4, 8).copyTo(J_int.colRange(5, 9)); // r1, r2, r3, t1, t2, t3 J_ext = jacobians.colRange(8, 14).clone(); } // Step 5 : Update error vector. { JtJ(Rect(0, 0, 9, 9)) += J_int.t() * J_int; JtJ(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = J_ext.t() * J_ext; JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)) = J_int.t() * J_ext; JtJ(Rect(0, 9 + 6 * image_idx, 9, 6)) = JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)).t(); Jtex.rowRange(0, 9) += J_int.t() * reproj_err.reshape(1, 2 * reproj_err.rows); Jtex.rowRange(9 + 6 * image_idx, 9 + 6 * (image_idx + 1)) = J_ext.t() * reproj_err.reshape(1, 2 * reproj_err.rows); } if (check_cond) { Mat JJ_kk = J_ext; SVD svd(JJ_kk, SVD::NO_UV); CV_Assert(svd.w.at<double>(0) / svd.w.at<double>(svd.w.rows - 1) < thresh_cond); } } // Step 6 : Shrink matrix if some parameters are not to be estimated. std::vector<uchar> idxs(param.isEstimate); idxs.insert(idxs.end(), 6 * image_num, 1); subMatrix(JtJ, JtJ, idxs, idxs); subMatrix(Jtex, Jtex, std::vector<uchar>(1, 1), idxs); }
２．２ Step 1：問題全体（内部パラメータ９個＋外部パラメータ６個ｘ画像枚数）に対する近似ヘッシアンとヤコビアン x 誤差ベクトルの領域を確保
だいぶ見づらいなあ．．．というのは承知のうえで（笑），説明を続けます．このステップでは必要な行列の領域を確保していますが，まずは再投影誤差の定義です．
上記ノートに書いてあるように，再投影誤差の最小要素は画像点のx座標（もしくはy座標）です．上記のように再投影誤差を設定すると，ヤコビアンは下記のようになります．
ちょっとメモがいっぱいいっぱいになってしまいましたが，この問題のヤコビアンは [2xnxN] X [9+6xN] の巨大行列になります．ここで，nは画像内の検知コーナー点の数，Nは画像の枚数です．で，次に誤差ベクトルは下記のようになります．
で，ガウスニュートン法で必要なのは「近似ヘッシアン」と「ヤコビアン x 誤差ベクトル」なので，この行列のサイズは下記のようになります．
ということで，JtJ のサイズは（9 + 6 x N）x（9 + 6 x N）の正方行列，Jtex のサイズが（9 + 6 x N）x 1 となることがわかりました．このサイズ確保をしているのがこのステップです．
該当コード
// Step 1 : Create approxiomated hessian matrix and error vector. // 9 Intrinsic Params + 6 Extrinsic Param * Board Number. JtJ = Mat::zeros(9 + 6 * image_num, 9 + 6 * image_num, CV_64FC1); Jtex = Mat::zeros(9 + 6 * image_num, 1, CV_64FC1);
２．３ Step ３：次に一枚一枚の画像に対して再投影誤差とヤコビアンを計算します．
で，ここの関数がとっても大きく，ちょっと別エントリにしないと書ききれないので，あとでリンクを貼ります．
２．４ Step ４：Step 3で求めた個々の画像に対するヤコビアンを内部パラメータのヤコビアン，外部パラメータのヤコビアンに分けます．
OpenCVのコードを追っていってわかったんですが，ヤコビアンの行に関する構造体（下記参照）が定義されてまして，
struct JacobianRow { Vec2d df, dc; Vec4d dk; Vec3d dom, dT; double dalpha; };
Step３の関数で得られるヤコビアン行列の並びは下記のようになってます．
で，上のノートにも書いたんですが，このヤコビアン，α（アスペクト比）がヤコビアンの最後の列に来てしまってます．ここに注意して，これを内部パラメータのヤコビアンと外部パラメータのヤコビアンの２つに分割します．分割している該当のコードが下記です．
// Step 4 : Extract jacobians elements of this image. Mat J_int, J_ext; { J_int = Mat(jacobians.rows, 9, CV_64FC1); // fx, fy, cx, cy jacobians.colRange(0, 4).copyTo(J_int.colRange(0, 4)); // alpha jacobians.col(14).copyTo(J_int.col(4)); // k1, k2, k3, k4 jacobians.colRange(4, 8).copyTo(J_int.colRange(5, 9)); // r1, r2, r3, t1, t2, t3 J_ext = jacobians.colRange(8, 14).clone(); }
で，上記の分割によって１つの画像から下記の２つのヤコビアンが求まりました．
２．５ Step 5 : Step４で求めた部分問題に対する２つのヤコビアンを全体のヤコビアンに組み込みます．
「ヤコビアンの分割とかせずに，Step3で出てくるヤコビアンを積み上げていけばいいやん？」と最初は思ったんですが，ちゃんと理由がありました．
内部パラメータはすべての画像の再投影誤差に影響を与えるが，外部パラメータはその画像の再投影誤差にしか影響をあたえない！
というのがミソでした．外部パラメータが自分の画像の再投影誤差にしか影響を与えないという事実によって，他の画像の要素に対する微分がすべて０になります．この特徴を活かして事前に行列の形を把握しておくと，Jt * J という巨大な行列の掛け算を避けることができます．このトリックを使わずに問題を解いてみたんですが，同じ解にはなりましたが明らかに実行速度が遅くなりました．で，行列の形を予め把握しておくために，Jt * J の行列構造を書き下して見ます．
次に，実際に Jt * J を計算したくないので，上述の問題全体のヤコビアンを使って近似ヘッシアン（Jt * J）の構造を書き出してみます．
近似ヘッシアンの構造に規則性があることがわかりました．ということで，巨大ヤコビアンの掛け算を避けて，上部ノートの構造に従って近似ヘッシアンを計算しているのが下記の部分です．ちなみに，普通に巨大ヤコビアンの掛け算をしたコードは”ComputeJacobiansNaively”という関数になります．
下記が該当部コードになりますが，近似ヘッシアン以外にも，ヤコビアン * 誤差ベクトルも計算しています．reproj_err が 2 x n の行列なので，これを（2 x n）の 列ベクトルに変換してから掛け算してます．
該当部コード
// Step 5 : Update error vector. { JtJ(Rect(0, 0, 9, 9)) += J_int.t() * J_int; JtJ(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = J_ext.t() * J_ext; JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)) = J_int.t() * J_ext; JtJ(Rect(0, 9 + 6 * image_idx, 9, 6)) = JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)).t(); Jtex.rowRange(0, 9) += J_int.t() * reproj_err.reshape(1, 2 * reproj_err.rows); Jtex.rowRange(9 + 6 * image_idx, 9 + 6 * (image_idx + 1)) = J_ext.t() * reproj_err.reshape(1, 2 * reproj_err.rows); }
最後に，上記計算で求まった「近似ヘッシアン」と「ヤコビアン * 誤差ベクトル」は，内部パラメータ，外部パラメータ両方に対する行列になってますが，この問題では内部パラメータのみが最適化の対象なので，行列から不要な箇所を削除します．これを実施しているのが下記の部分です．
（だったら最初からそう計算しとくべきなのでは，，，という気もしないでもないですが，ヤコビアンの全体構造がわかったので良しとしましょう．）
// Step 6 : Shrink matrix if some parameters are not to be estimated. std::vector<uchar> idxs(param.isEstimate); idxs.insert(idxs.end(), 6 * image_num, 1); subMatrix(JtJ, JtJ, idxs, idxs); subMatrix(Jtex, Jtex, std::vector<uchar>(1, 1), idxs);
２．６ ガウスニュートン法を使った最急降下ベクトルの計算
ということで，やっとComputeJacobiansの外に出ました．．．近似ヘッシアンが求まったので，ガウスニュートン法を使って最適化対象となるパラメータ空間中での最急降下ベクトルを求めます．
該当部コード
solve(JtJ, -Jtex, d_param);
３．Step3 & Step4 : 最急降下ベクトルを使ったパラメータベクトルの更新と，外部パラメータの再計算．
ここまでくれば，あとは最急降下ベクトルをパラメータベクトルに反映させ，パラメータベクトルの更新前後で変化率が一定以下になった場合に計算を終了します．
該当部コード
// Step 3 : // Update parameter by d_param * alpha_smooth2. (Gauss Newton 2) { const double alpha_smooth = 0.4; double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0); IntrinsicParams temp_param = final_param + alpha_smooth2 * d_param; updated_ratio = norm(Vec4d(temp_param.f[0], temp_param.f[1], temp_param.c[0], temp_param.c[1]) - Vec4d(final_param.f[0], final_param.f[1], final_param.c[0], final_param.c[1])) / norm(Vec4d(temp_param.f[0], temp_param.f[1], temp_param.c[0], temp_param.c[1])); final_param = temp_param; } // Step 4 : // Recomputing extrinsics based on the updated parameter sets. if (recompute_extrinsic) { CalibrateExtrinsics(objectPoints, imagePoints, final_param, check_cond, thresh_cond, board_rotations, board_translations); }
ということで，最後はなんかちょっと走り走りになってしまったですが，カメラキャリブレーション時の再投影誤差の最小化でした．
リファクタしたコードは下記のGithubにあげておきます．もちろん本家はOpenCVなので，あくまでも流れをつかむ用途でしか使えないですが．．．
github.com
ジモトモと沖縄探索 〜 後半戦
沖縄探索完結です！（GWから１．５ヶ月経過してますが（笑））
沖縄探索３日目後半，４日目，最終日になりますが，相方も自分も割とメジャーなスポットにはすでに行ってしまっていたので，後半戦はわりとマイナーな箇所をめぐりました．
３日目午後（斎場御嶽）
斎場御嶽
神話では，「アマミキヨ」という神が神の世界から降り立って国づくりを始めたというのが，琉球の始まりだとされているそうなのですが，そのアマミキヨの霊地として巡拝されていた御嶽の１つがこの斎場御嶽だそうです．「御嶽」というのは神様がいる場所として昔から信仰されている場所で，斎場御嶽は数ある御嶽の中でも最大の御嶽だそうです．ちょっと調べて書こうと思ったのですが，なんだか難しいので割愛します．．．
島唄にも出てくる「デイゴの花」です．こいつが咲き乱れる年は，たくさん台風が来ると言われているとのことです．
霊感の強い人が撮影すると，三角岩の頂点（上の部分）当たりに光のオーブがあらわれるらしいです．これ，僕がとったやつですが，オーブと言われるとオーブみたいなものが写ってなくもないような．．．
山ちゃんの部屋 〜 国際通り店
初日の焼き肉失敗を挽回すべく美味しい焼肉を探し求めてたんですが，なかなかクリティカルヒットが出ず，，，「３度目の失敗は許されない！」ということで食べログでくまなくお店を探したんですが，星がたくさんあるお店は当然のことながらなかなか高く．．．カルビ一人前２０００円位するお店ばっかりで，我々の財布レベルを超えてしまっていて途方にくれていたんですが，相方が石垣島の旅行で行った「山ちゃんの部屋」という美味しい焼肉屋の店舗が那覇にもあることを発見！３度めの正直に，，，，，，なりました！とても美味しかったです．
立地があまり良くないから（国際通りから一本入った通り沿い）かお店も空いていたんですが，味はバッチリでした．ここ，マジで美味しかったので，沖縄で焼き肉が食べたくなったらおすすめです！
ちなみに，，，お店の人とちょっと話していると，「インスタに写真載せていいならビールおごってあげるよ！」と言われ，初インスタデビューしました（笑）．
www.instagram.com
４日目午前（安定のA&Wバーガーからスタート！）
４日目午後（城跡見学〜ナイトカヤック）
中城城
ナイトカヤックまで時間が結構あったので，急遽行ってみることにしました．沖縄には「琉球王国のグスク及び関連遺産群」として，下記の９つの世界遺産があるみたいです．
グスク（城跡）（５つ）
首里城跡，今帰仁城跡，座喜味城跡，勝連城跡，中城城跡
で，この内の１つが中城城跡になります．城跡ですが，なかなか立派な石垣と絶景が印象的でした．
ナイトカヤック
相方が西表島に行った時にナイトカヤックをやったとのことで，面白いからやってみようという話になりました．夜にカヤックに乗る事自体初めてだったので，ちょっと神秘的でした．ただ，思いっきり住宅街っぽい川？での探索だったので，そこがちょっと残念でしたが，石垣とか宮古島とか行ったら，もう一回やってみようと思います！
下の写真みたいに，マングローブの木の下をカヤックでくぐったりするんですが，いつもの如く私はビビりモード全開で，，，相方とインストラクターのイケメン兄ちゃんに笑われっぱなしでした（笑）
やっぱりステーキ
沖縄最後の晩餐はステーキです！本当は最後の晩餐を盛大にやるべきだったのかもしれないですけど，金欠です（笑）関東だと，「いきなり！ステーキ」が有名ですが，沖縄では「やっぱりステーキ」が有名です．調べたところ，「いきなり！ステーキ」は銀座発，「やっぱりステーキ」は沖縄発らしいです．
「やっぱりステーキ」はとにかく料金が安く，赤身のステーキ（２００ｇ）なら，１０００円でご飯・サラダ食べ放題がついてきます．が，自分はちょっと行き過ぎて飽きました（笑）
