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とかつくばチャレンジに際して実際に使ったときにもう一度エントリをかければと思います．
