├── LICENSE
├── main.h
├── CMakeLists.txt
├── README.md
├── ACMM.h
├── main.cpp
├── colmap2mvsnet_acm.py
├── ACMM.cpp
└── ACMM.cu


/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 Qingshan Xu
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/main.h:
--------------------------------------------------------------------------------
 1 | #ifndef _MAIN_H_
 2 | #define _MAIN_H_
 3 | 
 4 | #include "opencv2/calib3d/calib3d.hpp"
 5 | #include "opencv2/imgproc/imgproc.hpp"
 6 | #include "opencv2/core/core.hpp"
 7 | #include "opencv2/highgui/highgui.hpp"
 8 | #include "opencv2/highgui/highgui.hpp"
 9 | #include "opencv2/opencv.hpp"
10 | 
11 | // Includes CUDA
12 | #include <cuda_runtime.h>
13 | #include <cuda.h>
14 | #include <cuda_runtime_api.h>
15 | #include <cuda_texture_types.h>
16 | #include <curand_kernel.h>
17 | #include <vector_types.h>
18 | 
19 | #include <vector>
20 | #include <string>
21 | #include <iostream>
22 | #include <fstream>
23 | #include <sstream>
24 | #include <algorithm>
25 | #include <map>
26 | #include <memory>
27 | #include "iomanip"
28 | 
29 | #include <sys/stat.h> // mkdir
30 | #include <sys/types.h> // mkdir
31 | 
32 | #define MAX_IMAGES 256
33 | #define JBU_NUM 2
34 | 
35 | struct Camera {
36 |     float K[9];
37 |     float R[9];
38 |     float t[3];
39 |     int height;
40 |     int width;
41 |     float depth_min;
42 |     float depth_max;
43 | };
44 | 
45 | struct Problem {
46 |     int ref_image_id;
47 |     std::vector<int> src_image_ids;
48 |     int max_image_size = 3200;
49 |     int num_downscale = 0;
50 |     int cur_image_size = 3200;
51 | };
52 | 
53 | struct PointList {
54 |     float3 coord;
55 |     float3 normal;
56 |     float3 color;
57 | };
58 | 
59 | #endif // _MAIN_H_
60 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required (VERSION 2.8)
 2 | project (ACMM)
 3 | 
 4 | find_package(CUDA 6.0 REQUIRED ) # For Cuda Managed Memory and c++11
 5 | find_package(OpenCV REQUIRED )
 6 | 
 7 | include_directories(${OpenCV_INCLUDE_DIRS})
 8 | include_directories(.)
 9 | 
10 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 --use_fast_math --maxrregcount=128 --ptxas-options=-v -std=c++11 --compiler-options -Wall -gencode arch=compute_30,code=sm_30 -gencode arch=compute_52,code=sm_52)
11 | 
12 | if(CMAKE_COMPILER_IS_GNUCXX)
13 |     add_definitions(-std=c++11)
14 |     add_definitions(-pthread)
15 |     add_definitions(-Wall)
16 |     add_definitions(-Wextra)
17 |     add_definitions(-pedantic)
18 |     add_definitions(-Wno-unused-function)
19 |     add_definitions(-Wno-switch)
20 |     set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -ffast-math -march=native") # extend release-profile with fast-math
21 | endif()
22 | 
23 | find_package(OpenMP)
24 | if (OPENMP_FOUND)
25 |     set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
26 | endif()
27 | 
28 | 
29 | # For compilation ...
30 | # Specify target & source files to compile it from
31 | cuda_add_executable(
32 |     ACMM
33 |     main.h
34 |     ACMM.h
35 |     ACMM.cpp
36 |     ACMM.cu
37 |     main.cpp
38 |     )
39 | 
40 | # For linking ...
41 | # Specify target & libraries to link it with
42 | target_link_libraries(ACMM
43 |     ${OpenCV_LIBS}
44 |     )
45 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # ACMM
 2 | [News] The code for [ACMH](https://github.com/GhiXu/ACMH) is released!!!  
 3 | [News] The code for [ACMP](https://github.com/GhiXu/ACMP) is released!!!  
 4 | [News] The code for [ACMMP](https://github.com/GhiXu/ACMMP) is released!!!  
 5 | ## About
 6 | [ACMM](https://arxiv.org/abs/1904.08103) is a multi-scale geometric consistency guided multi-view stereo method for efficient and accurate depth map estimation. If you find this project useful for your research, please cite:  
 7 | ```
 8 | @article{Xu2019ACMM,  
 9 |   title={Multi-Scale Geometric Consistency Guided Multi-View Stereo}, 
10 |   author={Xu, Qingshan and Tao, Wenbing}, 
11 |   journal={Computer Vision and Pattern Recognition (CVPR)},
12 |   year={2019}
13 | }
14 | ```
15 | ## Dependencies
16 | The code has been tested on Ubuntu 14.04 with GTX Titan X.  
17 | * [Cuda](https://developer.nvidia.com/zh-cn/cuda-downloads) >= 6.0
18 | * [OpenCV](https://opencv.org/) >= 2.4
19 | * [cmake](https://cmake.org/)
20 | ## Usage
21 | * Compile ACMM
22 | ```  
23 | cmake .  
24 | make
25 | ```
26 | * Test 
27 | ``` 
28 | Use script colmap2mvsnet_acm.py to convert COLMAP SfM result to ACMM input   
29 | Run ./ACMM $data_folder to get reconstruction results
30 | ```
31 | ## Results on high-res ETH3D training dataset [2cm]
32 | | Mean   | courtyard | delivery_area | electro | facade | kicker | meadow | office | pipes  | playgroud | relief | relief_2 | terrace | terrains |
33 | |--------|-----------|---------------|---------|--------|--------|--------|--------|--------|-----------|--------|----------|---------|----------|
34 | | 78.87  | 87.47     | 83.70         | 86.82   | 70.79  | 77.51  | 66.41  | 64.88	 | 70.07  | 72.06     | 85.36  | 84.87    | 89.85	  | 85.52    |
35 | ## SfM Reconstructions for Tanks and Temples Dataset
36 | To ease comparison with other MVS methods with our method on [Tanks and Temples dataset](https://www.tanksandtemples.org/), we release our SfM reconstuctions on this dataset. They are obtained by [COLMAP](https://colmap.github.io/) and can be downloaded from [here](https://drive.google.com/open?id=1DTnnmJAOGt7WPXSLMysMvPTy4CUZt_TU).
37 | ## Acknowledgements
38 | This code largely benefits from the following repositories: [Gipuma](https://github.com/kysucix/gipuma) and [COLMAP](https://colmap.github.io/). Thanks to their authors for opening source of their excellent works.
39 | 


--------------------------------------------------------------------------------
/ACMM.h:
--------------------------------------------------------------------------------
  1 | #ifndef _ACMM_H_
  2 | #define _ACMM_H_
  3 | 
  4 | #include "main.h"
  5 | 
  6 | int readDepthDmb(const std::string file_path, cv::Mat_<float> &depth);
  7 | int readNormalDmb(const std::string file_path, cv::Mat_<cv::Vec3f> &normal);
  8 | int writeDepthDmb(const std::string file_path, const cv::Mat_<float> depth);
  9 | int writeNormalDmb(const std::string file_path, const cv::Mat_<cv::Vec3f> normal);
 10 | 
 11 | Camera ReadCamera(const std::string &cam_path);
 12 | void  RescaleImageAndCamera(cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst, cv::Mat_<float> &depth, Camera &camera);
 13 | float3 Get3DPointonWorld(const int x, const int y, const float depth, const Camera camera);
 14 | void ProjectonCamera(const float3 PointX, const Camera camera, float2 &point, float &depth);
 15 | float GetAngle(const cv::Vec3f &v1, const cv::Vec3f &v2);
 16 | void StoreColorPlyFileBinaryPointCloud (const std::string &plyFilePath, const std::vector<PointList> &pc);
 17 | 
 18 | void RunJBU(const cv::Mat_<float>  &scaled_image_float, const cv::Mat_<float> &src_depthmap, const std::string &dense_folder , const Problem &problem);
 19 | 
 20 | #define CUDA_SAFE_CALL(error) CudaSafeCall(error, __FILE__, __LINE__)
 21 | #define CUDA_CHECK_ERROR() CudaCheckError(__FILE__, __LINE__)
 22 | 
 23 | void CudaSafeCall(const cudaError_t error, const std::string& file, const int line);
 24 | void CudaCheckError(const char* file, const int line);
 25 | 
 26 | struct cudaTextureObjects {
 27 |     cudaTextureObject_t images[MAX_IMAGES];
 28 | };
 29 | 
 30 | struct PatchMatchParams {
 31 |     int max_iterations = 3;
 32 |     int patch_size = 11;
 33 |     int num_images = 5;
 34 |     int max_image_size=3200;
 35 |     int radius_increment = 2;
 36 |     float sigma_spatial = 5.0f;
 37 |     float sigma_color = 3.0f;
 38 |     int top_k = 4;
 39 |     float baseline = 0.54f;
 40 |     float depth_min = 0.0f;
 41 |     float depth_max = 1.0f;
 42 |     float disparity_min = 0.0f;
 43 |     float disparity_max = 1.0f;
 44 | 
 45 |     float scaled_cols;
 46 |     float scaled_rows;
 47 | 
 48 |     bool geom_consistency = false;
 49 |     bool multi_geometry = false;
 50 |     bool hierarchy = false;
 51 |     bool upsample = false;
 52 | };
 53 | 
 54 | class ACMM {
 55 | public:
 56 |     ACMM();
 57 |     ~ACMM();
 58 | 
 59 |     void InuputInitialization(const std::string &dense_folder, const std::vector<Problem> &problem, const int idx);
 60 |     void Colmap2MVS(const std::string &dense_folder, std::vector<Problem> &problems);
 61 |     void CudaSpaceInitialization(const std::string &dense_folder, const Problem &problem);
 62 |     void RunPatchMatch();
 63 |     void SetGeomConsistencyParams(bool multi_geometry);
 64 |     void SetHierarchyParams();
 65 | 
 66 |     int GetReferenceImageWidth();
 67 |     int GetReferenceImageHeight();
 68 |     cv::Mat GetReferenceImage();
 69 |     float4 GetPlaneHypothesis(const int index);
 70 |     float GetCost(const int index);
 71 | private:
 72 |     int num_images;
 73 |     std::vector<cv::Mat> images;
 74 |     std::vector<cv::Mat> depths;
 75 |     std::vector<Camera> cameras;
 76 |     cudaTextureObjects texture_objects_host;
 77 |     cudaTextureObjects texture_depths_host;
 78 |     float4 *plane_hypotheses_host;
 79 |     float4 *scaled_plane_hypotheses_host;
 80 |     float *costs_host;
 81 |     float *pre_costs_host;
 82 |     PatchMatchParams params;
 83 | 
 84 |     Camera *cameras_cuda;
 85 |     cudaArray *cuArray[MAX_IMAGES];
 86 |     cudaArray *cuDepthArray[MAX_IMAGES];
 87 |     cudaTextureObjects *texture_objects_cuda;
 88 |     cudaTextureObjects *texture_depths_cuda;
 89 |     float4 *plane_hypotheses_cuda;
 90 |     float4 *scaled_plane_hypotheses_cuda;
 91 |     float *costs_cuda;
 92 |     float *pre_costs_cuda;
 93 |     curandState *rand_states_cuda;
 94 |     unsigned int *selected_views_cuda;
 95 |     float *depths_cuda;
 96 | };
 97 | 
 98 | struct TexObj {
 99 |     cudaTextureObject_t imgs[MAX_IMAGES];
100 | };
101 | 
102 | struct JBUParameters {
103 |     int height;
104 |     int width;
105 |     int s_height;
106 |     int s_width;
107 |     int Imagescale;
108 | };
109 | 
110 | struct JBUTexObj {
111 |     cudaTextureObject_t imgs[JBU_NUM];
112 | };
113 | 
114 | class JBU {
115 | public:
116 |     JBU();
117 |     ~JBU();
118 | 
119 |     // Host Parameters
120 |     float *depth_h;
121 |     JBUTexObj jt_h;
122 |     JBUParameters jp_h;
123 | 
124 |     // Device Parameters
125 |     float *depth_d;
126 |     cudaArray *cuArray[JBU_NUM]; // The first for reference image, and the second for stereo depth image
127 |     JBUTexObj *jt_d;
128 |     JBUParameters *jp_d;
129 | 
130 |     void InitializeParameters(int n);
131 |     void CudaRun();
132 | };
133 | 
134 | #endif // _ACMM_H_
135 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
  1 | #include "main.h"
  2 | #include "ACMM.h"
  3 | 
  4 | void GenerateSampleList(const std::string &dense_folder, std::vector<Problem> &problems)
  5 | {
  6 |     std::string cluster_list_path = dense_folder + std::string("/pair.txt");
  7 | 
  8 |     problems.clear();
  9 | 
 10 |     std::ifstream file(cluster_list_path);
 11 | 
 12 |     int num_images;
 13 |     file >> num_images;
 14 | 
 15 |     for (int i = 0; i < num_images; ++i) {
 16 |         Problem problem;
 17 |         problem.src_image_ids.clear();
 18 |         file >> problem.ref_image_id;
 19 | 
 20 |         int num_src_images;
 21 |         file >> num_src_images;
 22 |         for (int j = 0; j < num_src_images; ++j) {
 23 |             int id;
 24 |             float score;
 25 |             file >> id >> score;
 26 |             if (score <= 0.0f) {
 27 |                 continue;
 28 |             }
 29 |             problem.src_image_ids.push_back(id);
 30 |         }
 31 |         problems.push_back(problem);
 32 |     }
 33 | }
 34 | 
 35 | int ComputeMultiScaleSettings(const std::string &dense_folder, std::vector<Problem> &problems)
 36 | {
 37 |     int max_num_downscale = -1;
 38 |     int size_bound = 1000;
 39 |     PatchMatchParams pmp;
 40 |     std::string image_folder = dense_folder + std::string("/images");
 41 | 
 42 |     size_t num_images = problems.size();
 43 | 
 44 |     for (size_t i = 0; i < num_images; ++i) {
 45 |         std::stringstream image_path;
 46 |         image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id << ".jpg";
 47 |         cv::Mat_<uint8_t> image_uint = cv::imread(image_path.str(), cv::IMREAD_GRAYSCALE);
 48 | 
 49 |         int rows = image_uint.rows;
 50 |         int cols = image_uint.cols;
 51 |         int max_size = std::max(rows, cols);
 52 |         if (max_size > pmp.max_image_size) {
 53 |             max_size = pmp.max_image_size;
 54 |         }
 55 |         problems[i].max_image_size = max_size;
 56 | 
 57 |         int k = 0;
 58 |         while (max_size > size_bound) {
 59 |             max_size /= 2;
 60 |             k++;
 61 |         }
 62 | 
 63 |         if (k > max_num_downscale) {
 64 |             max_num_downscale = k;
 65 |         }
 66 | 
 67 |         problems[i].num_downscale = k;
 68 |     }
 69 | 
 70 |     return max_num_downscale;
 71 | }
 72 | 
 73 | void ProcessProblem(const std::string &dense_folder, const std::vector<Problem> &problems, const int idx, bool geom_consistency, bool hierarchy, bool multi_geometrty=false)
 74 | {
 75 |     const Problem problem = problems[idx];
 76 |     std::cout << "Processing image " << std::setw(8) << std::setfill('0') << problem.ref_image_id << "..." << std::endl;
 77 |     cudaSetDevice(0);
 78 |     std::stringstream result_path;
 79 |     result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
 80 |     std::string result_folder = result_path.str();
 81 |     mkdir(result_folder.c_str(), 0777);
 82 | 
 83 |     ACMM acmm;
 84 |     if (geom_consistency) {
 85 |         acmm.SetGeomConsistencyParams(multi_geometrty);
 86 |     }
 87 |     if (hierarchy) {
 88 |         acmm.SetHierarchyParams();
 89 |     }
 90 | 
 91 |     acmm.InuputInitialization(dense_folder, problems, idx);
 92 | 
 93 |     acmm.CudaSpaceInitialization(dense_folder, problem);
 94 |     acmm.RunPatchMatch();
 95 | 
 96 |     const int width = acmm.GetReferenceImageWidth();
 97 |     const int height = acmm.GetReferenceImageHeight();
 98 | 
 99 |     cv::Mat_<float> depths = cv::Mat::zeros(height, width, CV_32FC1);
100 |     cv::Mat_<cv::Vec3f> normals = cv::Mat::zeros(height, width, CV_32FC3);
101 |     cv::Mat_<float> costs = cv::Mat::zeros(height, width, CV_32FC1);
102 | 
103 |     for (int col = 0; col < width; ++col) {
104 |         for (int row = 0; row < height; ++row) {
105 |             int center = row * width + col;
106 |             float4 plane_hypothesis = acmm.GetPlaneHypothesis(center);
107 |             depths(row, col) = plane_hypothesis.w;
108 |             normals(row, col) = cv::Vec3f(plane_hypothesis.x, plane_hypothesis.y, plane_hypothesis.z);
109 |             costs(row, col) = acmm.GetCost(center);
110 |         }
111 |     }
112 | 
113 |     std::string suffix = "/depths.dmb";
114 |     if (geom_consistency) {
115 |         suffix = "/depths_geom.dmb";
116 |     }
117 |     std::string depth_path = result_folder + suffix;
118 |     std::string normal_path = result_folder + "/normals.dmb";
119 |     std::string cost_path = result_folder + "/costs.dmb";
120 |     writeDepthDmb(depth_path, depths);
121 |     writeNormalDmb(normal_path, normals);
122 |     writeDepthDmb(cost_path, costs);
123 |     std::cout << "Processing image " << std::setw(8) << std::setfill('0') << problem.ref_image_id << " done!" << std::endl;
124 | }
125 | 
126 | void JointBilateralUpsampling(const std::string &dense_folder, const Problem &problem, int acmm_size)
127 | {
128 |     std::stringstream result_path;
129 |     result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
130 |     std::string result_folder = result_path.str();
131 |     std::string depth_path = result_folder + "/depths_geom.dmb";
132 |     cv::Mat_<float> ref_depth;
133 |     readDepthDmb(depth_path, ref_depth);
134 | 
135 |     std::string image_folder = dense_folder + std::string("/images");
136 |     std::stringstream image_path;
137 |     image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problem.ref_image_id << ".jpg";
138 |     cv::Mat_<uint8_t> image_uint = cv::imread(image_path.str(), cv::IMREAD_GRAYSCALE);
139 |     cv::Mat image_float;
140 |     image_uint.convertTo(image_float, CV_32FC1);
141 |     const float factor_x = static_cast<float>(acmm_size) / image_float.cols;
142 |     const float factor_y = static_cast<float>(acmm_size) / image_float.rows;
143 |     const float factor = std::min(factor_x, factor_y);
144 | 
145 |     const int new_cols = std::round(image_float.cols * factor);
146 |     const int new_rows = std::round(image_float.rows * factor);
147 |     cv::Mat scaled_image_float;
148 |     cv::resize(image_float, scaled_image_float, cv::Size(new_cols,new_rows), 0, 0, cv::INTER_LINEAR);
149 | 
150 |     std::cout << "Run JBU for image " << problem.ref_image_id <<  ".jpg" << std::endl;
151 |     RunJBU(scaled_image_float, ref_depth, dense_folder, problem );
152 | }
153 | 
154 | void RunFusion(std::string &dense_folder, const std::vector<Problem> &problems, bool geom_consistency)
155 | {
156 |     size_t num_images = problems.size();
157 |     std::string image_folder = dense_folder + std::string("/images");
158 |     std::string cam_folder = dense_folder + std::string("/cams");
159 | 
160 |     std::vector<cv::Mat> images;
161 |     std::vector<Camera> cameras;
162 |     std::vector<cv::Mat_<float>> depths;
163 |     std::vector<cv::Mat_<cv::Vec3f>> normals;
164 |     std::vector<cv::Mat> masks;
165 |     images.clear();
166 |     cameras.clear();
167 |     depths.clear();
168 |     normals.clear();
169 |     masks.clear();
170 |     
171 |     std::map<int, int> image_id_2_index;
172 | 
173 |     for (size_t i = 0; i < num_images; ++i) {
174 |         std::cout << "Reading image " << std::setw(8) << std::setfill('0') << i << "..." << std::endl;
175 |         image_id_2_index[problems[i].ref_image_id] = i;
176 |         std::stringstream image_path;
177 |         image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id << ".jpg";
178 |         cv::Mat_<cv::Vec3b> image = cv::imread (image_path.str(), cv::IMREAD_COLOR);
179 |         std::stringstream cam_path;
180 |         cam_path << cam_folder << "/" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id << "_cam.txt";
181 |         Camera camera = ReadCamera(cam_path.str());
182 | 
183 |         std::stringstream result_path;
184 |         result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id;
185 |         std::string result_folder = result_path.str();
186 |         std::string suffix = "/depths.dmb";
187 |         if (geom_consistency) {
188 |             suffix = "/depths_geom.dmb";
189 |         }
190 |         std::string depth_path = result_folder + suffix;
191 |         std::string normal_path = result_folder + "/normals.dmb";
192 |         cv::Mat_<float> depth;
193 |         cv::Mat_<cv::Vec3f> normal;
194 |         readDepthDmb(depth_path, depth);
195 |         readNormalDmb(normal_path, normal);
196 | 
197 |         cv::Mat_<cv::Vec3b> scaled_image;
198 |         RescaleImageAndCamera(image, scaled_image, depth, camera);
199 |         images.push_back(scaled_image);
200 |         cameras.push_back(camera);
201 |         depths.push_back(depth);
202 |         normals.push_back(normal);
203 |         cv::Mat mask = cv::Mat::zeros(depth.rows, depth.cols, CV_8UC1);
204 |         masks.push_back(mask);
205 |     }
206 | 
207 |     std::vector<PointList> PointCloud;
208 |     PointCloud.clear();
209 | 
210 |     for (size_t i = 0; i < num_images; ++i) {
211 |         std::cout << "Fusing image " << std::setw(8) << std::setfill('0') << i << "..." << std::endl;
212 |         const int cols = depths[i].cols;
213 |         const int rows = depths[i].rows;
214 |         int num_ngb = problems[i].src_image_ids.size();
215 |         std::vector<int2> used_list(num_ngb, make_int2(-1, -1));
216 |         for (int r =0; r < rows; ++r) {
217 |             for (int c = 0; c < cols; ++c) {
218 |                 if (masks[i].at<uchar>(r, c) == 1)
219 |                     continue;
220 |                 float ref_depth = depths[i].at<float>(r, c);
221 |                 cv::Vec3f ref_normal = normals[i].at<cv::Vec3f>(r, c);
222 | 
223 |                 if (ref_depth <= 0.0)
224 |                     continue;
225 | 
226 |                 float3 PointX = Get3DPointonWorld(c, r, ref_depth, cameras[i]);
227 |                 float3 consistent_Point = PointX;
228 |                 cv::Vec3f consistent_normal = ref_normal;
229 |                 float consistent_Color[3] = {(float)images[i].at<cv::Vec3b>(r, c)[0], (float)images[i].at<cv::Vec3b>(r, c)[1], (float)images[i].at<cv::Vec3b>(r, c)[2]};
230 |                 int num_consistent = 0;
231 | 
232 |                 for (int j = 0; j < num_ngb; ++j) {
233 |                     int src_id = image_id_2_index[problems[i].src_image_ids[j]];
234 |                     const int src_cols = depths[src_id].cols;
235 |                     const int src_rows = depths[src_id].rows;
236 |                     float2 point;
237 |                     float proj_depth;
238 |                     ProjectonCamera(PointX, cameras[src_id], point, proj_depth);
239 |                     int src_r = int(point.y + 0.5f);
240 |                     int src_c = int(point.x + 0.5f);
241 |                     if (src_c >= 0 && src_c < src_cols && src_r >= 0 && src_r < src_rows) {
242 |                         if (masks[src_id].at<uchar>(src_r, src_c) == 1)
243 |                             continue;
244 | 
245 |                         float src_depth = depths[src_id].at<float>(src_r, src_c);
246 |                         cv::Vec3f src_normal = normals[src_id].at<cv::Vec3f>(src_r, src_c);
247 |                         if (src_depth <= 0.0)
248 |                             continue;
249 | 
250 |                         float3 tmp_X = Get3DPointonWorld(src_c, src_r, src_depth, cameras[src_id]);
251 |                         float2 tmp_pt;
252 |                         ProjectonCamera(tmp_X, cameras[i], tmp_pt, proj_depth);
253 |                         float reproj_error = sqrt(pow(c - tmp_pt.x, 2) + pow(r - tmp_pt.y, 2));
254 |                         float relative_depth_diff = fabs(proj_depth - ref_depth) / ref_depth;
255 |                         float angle = GetAngle(ref_normal, src_normal);
256 | 
257 |                         if (reproj_error < 2.0f && relative_depth_diff < 0.01f && angle < 0.174533f) {
258 |                             consistent_Point.x += tmp_X.x;
259 |                             consistent_Point.y += tmp_X.y;
260 |                             consistent_Point.z += tmp_X.z;
261 |                             consistent_normal = consistent_normal + src_normal;
262 |                             consistent_Color[0] += images[src_id].at<cv::Vec3b>(src_r, src_c)[0];
263 |                             consistent_Color[1] += images[src_id].at<cv::Vec3b>(src_r, src_c)[1];
264 |                             consistent_Color[2] += images[src_id].at<cv::Vec3b>(src_r, src_c)[2];
265 | 
266 |                             used_list[j].x = src_c;
267 |                             used_list[j].y = src_r;
268 |                             num_consistent++;
269 |                         }
270 |                     }
271 |                 }
272 | 
273 |                 if (num_consistent >= 2) {
274 |                     consistent_Point.x /= (num_consistent + 1.0f);
275 |                     consistent_Point.y /= (num_consistent + 1.0f);
276 |                     consistent_Point.z /= (num_consistent + 1.0f);
277 |                     consistent_normal /= (num_consistent + 1.0f);
278 |                     consistent_Color[0] /= (num_consistent + 1.0f);
279 |                     consistent_Color[1] /= (num_consistent + 1.0f);
280 |                     consistent_Color[2] /= (num_consistent + 1.0f);
281 | 
282 |                     PointList point3D;
283 |                     point3D.coord = consistent_Point;
284 |                     point3D.normal = make_float3(consistent_normal[0], consistent_normal[1], consistent_normal[2]);
285 |                     point3D.color = make_float3(consistent_Color[0], consistent_Color[1], consistent_Color[2]);
286 |                     PointCloud.push_back(point3D);
287 | 
288 |                     for (int j = 0; j < num_ngb; ++j) {
289 |                         if (used_list[j].x == -1)
290 |                             continue;
291 |                         masks[image_id_2_index[problems[i].src_image_ids[j]]].at<uchar>(used_list[j].y, used_list[j].x) = 1;
292 |                     }
293 |                 }
294 |             }
295 |         }
296 |     }
297 | 
298 |     std::string ply_path = dense_folder + "/ACMM/ACMM_model.ply";
299 |     StoreColorPlyFileBinaryPointCloud (ply_path, PointCloud);
300 | }
301 | 
302 | int main(int argc, char** argv)
303 | {
304 |     if (argc < 2) {
305 |         std::cout << "USAGE: ACMM dense_folder" << std::endl;
306 |         return -1;
307 |     }
308 | 
309 |     std::string dense_folder = argv[1];
310 |     std::vector<Problem> problems;
311 |     GenerateSampleList(dense_folder, problems);
312 | 
313 |     std::string output_folder = dense_folder + std::string("/ACMM");
314 |     mkdir(output_folder.c_str(), 0777);
315 | 
316 |     size_t num_images = problems.size();
317 |     std::cout << "There are " << num_images << " problems needed to be processed!" << std::endl;
318 | 
319 |     int max_num_downscale = ComputeMultiScaleSettings(dense_folder, problems);
320 | 
321 |      int flag = 0;
322 |      int geom_iterations = 2;
323 |      bool geom_consistency = false;
324 |      bool hierarchy = false;
325 |      bool multi_geometry = false;
326 |      while (max_num_downscale >= 0) {
327 |         std::cout << "Scale: " << max_num_downscale << std::endl;
328 | 
329 |         for (size_t i = 0; i < num_images; ++i) {
330 |             if (problems[i].num_downscale >= 0) {
331 |                 problems[i].cur_image_size = problems[i].max_image_size / pow(2, problems[i].num_downscale);
332 |                 problems[i].num_downscale--;
333 |             }
334 |         }
335 | 
336 |         if (flag == 0) {
337 |             flag = 1;
338 |             geom_consistency = false;
339 |             for (size_t i = 0; i < num_images; ++i) {
340 |                 ProcessProblem(dense_folder, problems, i, geom_consistency ,hierarchy);
341 |             }
342 |             geom_consistency = true;
343 |             for (int geom_iter = 0; geom_iter < geom_iterations; ++geom_iter) {
344 |                 if (geom_iter == 0) {
345 |                     multi_geometry = false;
346 |                 }
347 |                 else {
348 |                     multi_geometry = true;
349 |                 }
350 |                 for (size_t i = 0; i < num_images; ++i) {
351 |                     ProcessProblem(dense_folder,  problems, i, geom_consistency, hierarchy, multi_geometry);
352 |                 }
353 |             }
354 |         }
355 |         else {
356 |             for (size_t i = 0; i < num_images; ++i) {
357 |                JointBilateralUpsampling(dense_folder, problems[i], problems[i].cur_image_size);
358 |              }
359 | 
360 |             hierarchy = true;
361 |             geom_consistency = false;
362 |             for (size_t i = 0; i < num_images; ++i) {
363 |                 ProcessProblem(dense_folder,  problems, i, geom_consistency, hierarchy);
364 |             }
365 |             hierarchy = false;
366 |             geom_consistency = true;
367 |             for (int geom_iter = 0; geom_iter < geom_iterations; ++geom_iter) {
368 |                 if (geom_iter == 0) {
369 |                     multi_geometry = false;
370 |                 }
371 |                 else {
372 |                     multi_geometry = true;
373 |                 }
374 |                 for (size_t i = 0; i < num_images; ++i) {
375 |                     ProcessProblem(dense_folder,  problems, i, geom_consistency, hierarchy, multi_geometry);
376 |                 }
377 |             }
378 |         }
379 | 
380 |         max_num_downscale--;
381 |     }
382 | 
383 |     geom_consistency = true;
384 |     RunFusion(dense_folder, problems, geom_consistency);
385 | 
386 |     return 0;
387 | }
388 | 


--------------------------------------------------------------------------------
/colmap2mvsnet_acm.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | """
  3 | Copyright 2019, Jingyang Zhang and Yao Yao, HKUST. Model reading is provided by COLMAP.
  4 | Preprocess script.
  5 | View selection is modified according to COLMAP's strategy, Qingshan Xu
  6 | """
  7 | 
  8 | from __future__ import print_function
  9 | import collections
 10 | import struct
 11 | import numpy as np
 12 | import multiprocessing as mp
 13 | from functools import partial
 14 | import os
 15 | import argparse
 16 | import shutil
 17 | import cv2
 18 | 
 19 | #============================ read_model.py ============================#
 20 | CameraModel = collections.namedtuple(
 21 |     "CameraModel", ["model_id", "model_name", "num_params"])
 22 | Camera = collections.namedtuple(
 23 |     "Camera", ["id", "model", "width", "height", "params"])
 24 | BaseImage = collections.namedtuple(
 25 |     "Image", ["id", "qvec", "tvec", "camera_id", "name", "xys", "point3D_ids"])
 26 | Point3D = collections.namedtuple(
 27 |     "Point3D", ["id", "xyz", "rgb", "error", "image_ids", "point2D_idxs"])
 28 | 
 29 | class Image(BaseImage):
 30 |     def qvec2rotmat(self):
 31 |         return qvec2rotmat(self.qvec)
 32 | 
 33 | CAMERA_MODELS = {
 34 |     CameraModel(model_id=0, model_name="SIMPLE_PINHOLE", num_params=3),
 35 |     CameraModel(model_id=1, model_name="PINHOLE", num_params=4),
 36 |     CameraModel(model_id=2, model_name="SIMPLE_RADIAL", num_params=4),
 37 |     CameraModel(model_id=3, model_name="RADIAL", num_params=5),
 38 |     CameraModel(model_id=4, model_name="OPENCV", num_params=8),
 39 |     CameraModel(model_id=5, model_name="OPENCV_FISHEYE", num_params=8),
 40 |     CameraModel(model_id=6, model_name="FULL_OPENCV", num_params=12),
 41 |     CameraModel(model_id=7, model_name="FOV", num_params=5),
 42 |     CameraModel(model_id=8, model_name="SIMPLE_RADIAL_FISHEYE", num_params=4),
 43 |     CameraModel(model_id=9, model_name="RADIAL_FISHEYE", num_params=5),
 44 |     CameraModel(model_id=10, model_name="THIN_PRISM_FISHEYE", num_params=12)
 45 | }
 46 | CAMERA_MODEL_IDS = dict([(camera_model.model_id, camera_model) \
 47 |                          for camera_model in CAMERA_MODELS])
 48 | 
 49 | 
 50 | def read_next_bytes(fid, num_bytes, format_char_sequence, endian_character="<"):
 51 |     """Read and unpack the next bytes from a binary file.
 52 |     :param fid:
 53 |     :param num_bytes: Sum of combination of {2, 4, 8}, e.g. 2, 6, 16, 30, etc.
 54 |     :param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}.
 55 |     :param endian_character: Any of {@, =, <, >, !}
 56 |     :return: Tuple of read and unpacked values.
 57 |     """
 58 |     data = fid.read(num_bytes)
 59 |     return struct.unpack(endian_character + format_char_sequence, data)
 60 | 
 61 | 
 62 | def read_cameras_text(path):
 63 |     """
 64 |     see: src/base/reconstruction.cc
 65 |         void Reconstruction::WriteCamerasText(const std::string& path)
 66 |         void Reconstruction::ReadCamerasText(const std::string& path)
 67 |     """
 68 |     cameras = {}
 69 |     with open(path, "r") as fid:
 70 |         while True:
 71 |             line = fid.readline()
 72 |             if not line:
 73 |                 break
 74 |             line = line.strip()
 75 |             if len(line) > 0 and line[0] != "#":
 76 |                 elems = line.split()
 77 |                 camera_id = int(elems[0])
 78 |                 model = elems[1]
 79 |                 width = int(elems[2])
 80 |                 height = int(elems[3])
 81 |                 params = np.array(tuple(map(float, elems[4:])))
 82 |                 cameras[camera_id] = Camera(id=camera_id, model=model,
 83 |                                             width=width, height=height,
 84 |                                             params=params)
 85 |     return cameras
 86 | 
 87 | 
 88 | def read_cameras_binary(path_to_model_file):
 89 |     """
 90 |     see: src/base/reconstruction.cc
 91 |         void Reconstruction::WriteCamerasBinary(const std::string& path)
 92 |         void Reconstruction::ReadCamerasBinary(const std::string& path)
 93 |     """
 94 |     cameras = {}
 95 |     with open(path_to_model_file, "rb") as fid:
 96 |         num_cameras = read_next_bytes(fid, 8, "Q")[0]
 97 |         for camera_line_index in range(num_cameras):
 98 |             camera_properties = read_next_bytes(
 99 |                 fid, num_bytes=24, format_char_sequence="iiQQ")
100 |             camera_id = camera_properties[0]
101 |             model_id = camera_properties[1]
102 |             model_name = CAMERA_MODEL_IDS[camera_properties[1]].model_name
103 |             width = camera_properties[2]
104 |             height = camera_properties[3]
105 |             num_params = CAMERA_MODEL_IDS[model_id].num_params
106 |             params = read_next_bytes(fid, num_bytes=8*num_params,
107 |                                      format_char_sequence="d"*num_params)
108 |             cameras[camera_id] = Camera(id=camera_id,
109 |                                         model=model_name,
110 |                                         width=width,
111 |                                         height=height,
112 |                                         params=np.array(params))
113 |         assert len(cameras) == num_cameras
114 |     return cameras
115 | 
116 | 
117 | def read_images_text(path):
118 |     """
119 |     see: src/base/reconstruction.cc
120 |         void Reconstruction::ReadImagesText(const std::string& path)
121 |         void Reconstruction::WriteImagesText(const std::string& path)
122 |     """
123 |     images = {}
124 |     with open(path, "r") as fid:
125 |         while True:
126 |             line = fid.readline()
127 |             if not line:
128 |                 break
129 |             line = line.strip()
130 |             if len(line) > 0 and line[0] != "#":
131 |                 elems = line.split()
132 |                 image_id = int(elems[0])
133 |                 qvec = np.array(tuple(map(float, elems[1:5])))
134 |                 tvec = np.array(tuple(map(float, elems[5:8])))
135 |                 camera_id = int(elems[8])
136 |                 image_name = elems[9]
137 |                 elems = fid.readline().split()
138 |                 xys = np.column_stack([tuple(map(float, elems[0::3])),
139 |                                        tuple(map(float, elems[1::3]))])
140 |                 point3D_ids = np.array(tuple(map(int, elems[2::3])))
141 |                 images[image_id] = Image(
142 |                     id=image_id, qvec=qvec, tvec=tvec,
143 |                     camera_id=camera_id, name=image_name,
144 |                     xys=xys, point3D_ids=point3D_ids)
145 |     return images
146 | 
147 | 
148 | def read_images_binary(path_to_model_file):
149 |     """
150 |     see: src/base/reconstruction.cc
151 |         void Reconstruction::ReadImagesBinary(const std::string& path)
152 |         void Reconstruction::WriteImagesBinary(const std::string& path)
153 |     """
154 |     images = {}
155 |     with open(path_to_model_file, "rb") as fid:
156 |         num_reg_images = read_next_bytes(fid, 8, "Q")[0]
157 |         for image_index in range(num_reg_images):
158 |             binary_image_properties = read_next_bytes(
159 |                 fid, num_bytes=64, format_char_sequence="idddddddi")
160 |             image_id = binary_image_properties[0]
161 |             qvec = np.array(binary_image_properties[1:5])
162 |             tvec = np.array(binary_image_properties[5:8])
163 |             camera_id = binary_image_properties[8]
164 |             image_name = ""
165 |             current_char = read_next_bytes(fid, 1, "c")[0]
166 |             while current_char != b"\x00":   # look for the ASCII 0 entry
167 |                 image_name += current_char.decode("utf-8")
168 |                 current_char = read_next_bytes(fid, 1, "c")[0]
169 |             num_points2D = read_next_bytes(fid, num_bytes=8,
170 |                                            format_char_sequence="Q")[0]
171 |             x_y_id_s = read_next_bytes(fid, num_bytes=24*num_points2D,
172 |                                        format_char_sequence="ddq"*num_points2D)
173 |             xys = np.column_stack([tuple(map(float, x_y_id_s[0::3])),
174 |                                    tuple(map(float, x_y_id_s[1::3]))])
175 |             point3D_ids = np.array(tuple(map(int, x_y_id_s[2::3])))
176 |             images[image_id] = Image(
177 |                 id=image_id, qvec=qvec, tvec=tvec,
178 |                 camera_id=camera_id, name=image_name,
179 |                 xys=xys, point3D_ids=point3D_ids)
180 |     return images
181 | 
182 | 
183 | def read_points3D_text(path):
184 |     """
185 |     see: src/base/reconstruction.cc
186 |         void Reconstruction::ReadPoints3DText(const std::string& path)
187 |         void Reconstruction::WritePoints3DText(const std::string& path)
188 |     """
189 |     points3D = {}
190 |     with open(path, "r") as fid:
191 |         while True:
192 |             line = fid.readline()
193 |             if not line:
194 |                 break
195 |             line = line.strip()
196 |             if len(line) > 0 and line[0] != "#":
197 |                 elems = line.split()
198 |                 point3D_id = int(elems[0])
199 |                 xyz = np.array(tuple(map(float, elems[1:4])))
200 |                 rgb = np.array(tuple(map(int, elems[4:7])))
201 |                 error = float(elems[7])
202 |                 image_ids = np.array(tuple(map(int, elems[8::2])))
203 |                 point2D_idxs = np.array(tuple(map(int, elems[9::2])))
204 |                 points3D[point3D_id] = Point3D(id=point3D_id, xyz=xyz, rgb=rgb,
205 |                                                error=error, image_ids=image_ids,
206 |                                                point2D_idxs=point2D_idxs)
207 |     return points3D
208 | 
209 | 
210 | def read_points3d_binary(path_to_model_file):
211 |     """
212 |     see: src/base/reconstruction.cc
213 |         void Reconstruction::ReadPoints3DBinary(const std::string& path)
214 |         void Reconstruction::WritePoints3DBinary(const std::string& path)
215 |     """
216 |     points3D = {}
217 |     with open(path_to_model_file, "rb") as fid:
218 |         num_points = read_next_bytes(fid, 8, "Q")[0]
219 |         for point_line_index in range(num_points):
220 |             binary_point_line_properties = read_next_bytes(
221 |                 fid, num_bytes=43, format_char_sequence="QdddBBBd")
222 |             point3D_id = binary_point_line_properties[0]
223 |             xyz = np.array(binary_point_line_properties[1:4])
224 |             rgb = np.array(binary_point_line_properties[4:7])
225 |             error = np.array(binary_point_line_properties[7])
226 |             track_length = read_next_bytes(
227 |                 fid, num_bytes=8, format_char_sequence="Q")[0]
228 |             track_elems = read_next_bytes(
229 |                 fid, num_bytes=8*track_length,
230 |                 format_char_sequence="ii"*track_length)
231 |             image_ids = np.array(tuple(map(int, track_elems[0::2])))
232 |             point2D_idxs = np.array(tuple(map(int, track_elems[1::2])))
233 |             points3D[point3D_id] = Point3D(
234 |                 id=point3D_id, xyz=xyz, rgb=rgb,
235 |                 error=error, image_ids=image_ids,
236 |                 point2D_idxs=point2D_idxs)
237 |     return points3D
238 | 
239 | 
240 | def read_model(path, ext):
241 |     if ext == ".txt":
242 |         cameras = read_cameras_text(os.path.join(path, "cameras" + ext))
243 |         images = read_images_text(os.path.join(path, "images" + ext))
244 |         points3D = read_points3D_text(os.path.join(path, "points3D") + ext)
245 |     else:
246 |         cameras = read_cameras_binary(os.path.join(path, "cameras" + ext))
247 |         images = read_images_binary(os.path.join(path, "images" + ext))
248 |         points3D = read_points3d_binary(os.path.join(path, "points3D") + ext)
249 |     return cameras, images, points3D
250 | 
251 | 
252 | def qvec2rotmat(qvec):
253 |     return np.array([
254 |         [1 - 2 * qvec[2]**2 - 2 * qvec[3]**2,
255 |          2 * qvec[1] * qvec[2] - 2 * qvec[0] * qvec[3],
256 |          2 * qvec[3] * qvec[1] + 2 * qvec[0] * qvec[2]],
257 |         [2 * qvec[1] * qvec[2] + 2 * qvec[0] * qvec[3],
258 |          1 - 2 * qvec[1]**2 - 2 * qvec[3]**2,
259 |          2 * qvec[2] * qvec[3] - 2 * qvec[0] * qvec[1]],
260 |         [2 * qvec[3] * qvec[1] - 2 * qvec[0] * qvec[2],
261 |          2 * qvec[2] * qvec[3] + 2 * qvec[0] * qvec[1],
262 |          1 - 2 * qvec[1]**2 - 2 * qvec[2]**2]])
263 | 
264 | 
265 | def rotmat2qvec(R):
266 |     Rxx, Ryx, Rzx, Rxy, Ryy, Rzy, Rxz, Ryz, Rzz = R.flat
267 |     K = np.array([
268 |         [Rxx - Ryy - Rzz, 0, 0, 0],
269 |         [Ryx + Rxy, Ryy - Rxx - Rzz, 0, 0],
270 |         [Rzx + Rxz, Rzy + Ryz, Rzz - Rxx - Ryy, 0],
271 |         [Ryz - Rzy, Rzx - Rxz, Rxy - Ryx, Rxx + Ryy + Rzz]]) / 3.0
272 |     eigvals, eigvecs = np.linalg.eigh(K)
273 |     qvec = eigvecs[[3, 0, 1, 2], np.argmax(eigvals)]
274 |     if qvec[0] < 0:
275 |         qvec *= -1
276 |     return qvec
277 | 
278 | 
279 | 
280 | def calc_score(inputs, images, points3d, extrinsic, args):
281 |     i, j = inputs
282 |     id_i = images[i+1].point3D_ids
283 |     id_j = images[j+1].point3D_ids
284 |     id_intersect = [it for it in id_i if it in id_j]
285 |     cam_center_i = -np.matmul(extrinsic[i+1][:3, :3].transpose(), extrinsic[i+1][:3, 3:4])[:, 0]
286 |     cam_center_j = -np.matmul(extrinsic[j+1][:3, :3].transpose(), extrinsic[j+1][:3, 3:4])[:, 0]
287 |     score = 0
288 |     angles = []
289 |     for pid in id_intersect:
290 |         if pid == -1:
291 |             continue
292 |         p = points3d[pid].xyz
293 |         theta = (180 / np.pi) * np.arccos(np.dot(cam_center_i - p, cam_center_j - p) / np.linalg.norm(cam_center_i - p) / np.linalg.norm(cam_center_j - p)) # triangulation angle
294 |         # score += np.exp(-(theta - args.theta0) * (theta - args.theta0) / (2 * (args.sigma1 if theta <= args.theta0 else args.sigma2) ** 2))
295 |         angles.append(theta)
296 |         score += 1
297 |     if len(angles) > 0:
298 |         angles_sorted = sorted(angles)
299 |         triangulationangle = angles_sorted[int(len(angles_sorted) * 0.75)]
300 |         if triangulationangle < 1:
301 |             score = 0.0
302 |     return i, j, score
303 | 
304 | def processing_single_scene(args):
305 | 
306 |     image_dir = os.path.join(args.dense_folder, 'images')
307 |     model_dir = os.path.join(args.dense_folder, 'sparse')
308 |     cam_dir = os.path.join(args.save_folder, 'cams')
309 |     image_converted_dir = os.path.join(args.save_folder, 'images')
310 | 
311 |     if os.path.exists(image_converted_dir):
312 |         print("remove:{}".format(image_converted_dir))
313 |         shutil.rmtree(image_converted_dir)
314 |     os.makedirs(image_converted_dir)
315 |     if os.path.exists(cam_dir):
316 |         print("remove:{}".format(cam_dir))
317 |         shutil.rmtree(cam_dir)
318 | 
319 |     cameras, images, points3d = read_model(model_dir, args.model_ext)
320 |     num_images = len(list(images.items()))
321 | 
322 |     param_type = {
323 |         'SIMPLE_PINHOLE': ['f', 'cx', 'cy'],
324 |         'PINHOLE': ['fx', 'fy', 'cx', 'cy'],
325 |         'SIMPLE_RADIAL': ['f', 'cx', 'cy', 'k'],
326 |         'SIMPLE_RADIAL_FISHEYE': ['f', 'cx', 'cy', 'k'],
327 |         'RADIAL': ['f', 'cx', 'cy', 'k1', 'k2'],
328 |         'RADIAL_FISHEYE': ['f', 'cx', 'cy', 'k1', 'k2'],
329 |         'OPENCV': ['fx', 'fy', 'cx', 'cy', 'k1', 'k2', 'p1', 'p2'],
330 |         'OPENCV_FISHEYE': ['fx', 'fy', 'cx', 'cy', 'k1', 'k2', 'k3', 'k4'],
331 |         'FULL_OPENCV': ['fx', 'fy', 'cx', 'cy', 'k1', 'k2', 'p1', 'p2', 'k3', 'k4', 'k5', 'k6'],
332 |         'FOV': ['fx', 'fy', 'cx', 'cy', 'omega'],
333 |         'THIN_PRISM_FISHEYE': ['fx', 'fy', 'cx', 'cy', 'k1', 'k2', 'p1', 'p2', 'k3', 'k4', 'sx1', 'sy1']
334 |     }
335 | 
336 |     # intrinsic
337 |     intrinsic = {}
338 |     for camera_id, cam in cameras.items():
339 |         params_dict = {key: value for key, value in zip(param_type[cam.model], cam.params)}
340 |         if 'f' in param_type[cam.model]:
341 |             params_dict['fx'] = params_dict['f']
342 |             params_dict['fy'] = params_dict['f']
343 |         i = np.array([
344 |             [params_dict['fx'], 0, params_dict['cx']],
345 |             [0, params_dict['fy'], params_dict['cy']],
346 |             [0, 0, 1]
347 |         ])
348 |         intrinsic[camera_id] = i
349 |     print('intrinsic\n', intrinsic, end='\n\n')
350 | 
351 |     new_images = {}
352 |     for i, image_id in enumerate(sorted(images.keys())):
353 |         new_images[i+1] = images[image_id]
354 |     images = new_images
355 | 
356 |     # extrinsic
357 |     extrinsic = {}
358 |     for image_id, image in images.items():
359 |         e = np.zeros((4, 4))
360 |         e[:3, :3] = qvec2rotmat(image.qvec)
361 |         e[:3, 3] = image.tvec
362 |         e[3, 3] = 1
363 |         extrinsic[image_id] = e
364 |     print('extrinsic[1]\n', extrinsic[1], end='\n\n')
365 | 
366 |     # depth range and interval
367 |     depth_ranges = {}
368 |     for i in range(num_images):
369 |         zs = []
370 |         for p3d_id in images[i+1].point3D_ids:
371 |             if p3d_id == -1:
372 |                 continue
373 |             transformed = np.matmul(extrinsic[i+1], [points3d[p3d_id].xyz[0], points3d[p3d_id].xyz[1], points3d[p3d_id].xyz[2], 1])
374 |             zs.append(np.asscalar(transformed[2]))
375 |         zs_sorted = sorted(zs)
376 |         # relaxed depth range
377 |         depth_min = zs_sorted[int(len(zs) * .01)] * 0.75
378 |         depth_max = zs_sorted[int(len(zs) * .99)] * 1.25
379 |         # determine depth number by inverse depth setting, see supplementary material
380 |         if args.max_d == 0:
381 |             image_int = intrinsic[images[i+1].camera_id]
382 |             image_ext = extrinsic[i+1]
383 |             image_r = image_ext[0:3, 0:3]
384 |             image_t = image_ext[0:3, 3]
385 |             p1 = [image_int[0, 2], image_int[1, 2], 1]
386 |             p2 = [image_int[0, 2] + 1, image_int[1, 2], 1]
387 |             P1 = np.matmul(np.linalg.inv(image_int), p1) * depth_min
388 |             P1 = np.matmul(np.linalg.inv(image_r), (P1 - image_t))
389 |             P2 = np.matmul(np.linalg.inv(image_int), p2) * depth_min
390 |             P2 = np.matmul(np.linalg.inv(image_r), (P2 - image_t))
391 |             depth_num = (1 / depth_min - 1 / depth_max) / (1 / depth_min - 1 / (depth_min + np.linalg.norm(P2 - P1)))
392 |         else:
393 |             depth_num = args.max_d
394 |         depth_interval = (depth_max - depth_min) / (depth_num - 1) / args.interval_scale
395 |         depth_ranges[i+1] = (depth_min, depth_interval, depth_num, depth_max)
396 |     print('depth_ranges[1]\n', depth_ranges[1], end='\n\n')
397 | 
398 |     # view selection
399 |     score = np.zeros((len(images), len(images)))
400 |     queue = []
401 |     for i in range(len(images)):
402 |         for j in range(i + 1, len(images)):
403 |             queue.append((i, j))
404 | 
405 |     p = mp.Pool(processes=mp.cpu_count())
406 |     func = partial(calc_score, images=images, points3d=points3d, args=args, extrinsic=extrinsic)
407 |     result = p.map(func, queue)
408 |     for i, j, s in result:
409 |         score[i, j] = s
410 |         score[j, i] = s
411 |     view_sel = []
412 |     num_view = min(20, len(images) - 1)
413 |     for i in range(len(images)):
414 |         sorted_score = np.argsort(score[i])[::-1]
415 |         view_sel.append([(k, score[i, k]) for k in sorted_score[:num_view]])
416 |     print('view_sel[0]\n', view_sel[0], end='\n\n')
417 | 
418 |     # write
419 |     try:
420 |         os.makedirs(cam_dir)
421 |     except os.error:
422 |         print(cam_dir + ' already exist.')
423 |     for i in range(num_images):
424 |         with open(os.path.join(cam_dir, '%08d_cam.txt' % i), 'w') as f:
425 |             f.write('extrinsic\n')
426 |             for j in range(4):
427 |                 for k in range(4):
428 |                     f.write(str(extrinsic[i+1][j, k]) + ' ')
429 |                 f.write('\n')
430 |             f.write('\nintrinsic\n')
431 |             for j in range(3):
432 |                 for k in range(3):
433 |                     f.write(str(intrinsic[images[i+1].camera_id][j, k]) + ' ')
434 |                 f.write('\n')
435 |             f.write('\n%f %f %f %f\n' % (depth_ranges[i+1][0], depth_ranges[i+1][1], depth_ranges[i+1][2], depth_ranges[i+1][3]))
436 |     with open(os.path.join(args.save_folder, 'pair.txt'), 'w') as f:
437 |         f.write('%d\n' % len(images))
438 |         for i, sorted_score in enumerate(view_sel):
439 |             f.write('%d\n%d ' % (i, len(sorted_score)))
440 |             for image_id, s in sorted_score:
441 |                 f.write('%d %d ' % (image_id, s))
442 |             f.write('\n')
443 | 
444 |     #convert to jpg
445 |     for i in range(num_images):
446 |         img_path = os.path.join(image_dir, images[i + 1].name)
447 |         if not img_path.endswith(".jpg"):
448 |             img = cv2.imread(img_path)
449 |             cv2.imwrite(os.path.join(image_converted_dir, '%08d.jpg' % i), img)
450 |         else:
451 |             shutil.copyfile(os.path.join(image_dir, images[i+1].name), os.path.join(image_converted_dir, '%08d.jpg' % i))
452 | 
453 | 
454 | if __name__ == '__main__':
455 |     parser = argparse.ArgumentParser(description='Convert colmap camera')
456 | 
457 |     parser.add_argument('--dense_folder', required=True, type=str, help='dense_folder.')
458 |     parser.add_argument('--save_folder', required=True, type=str, help='save_folder.')
459 | 
460 |     parser.add_argument('--max_d', type=int, default=192)
461 |     parser.add_argument('--interval_scale', type=float, default=1)
462 | 
463 |     parser.add_argument('--theta0', type=float, default=5)
464 |     parser.add_argument('--sigma1', type=float, default=1)
465 |     parser.add_argument('--sigma2', type=float, default=10)
466 |     parser.add_argument('--model_ext', type=str, default=".txt",  choices=[".txt", ".bin"], help='sparse model ext')
467 | 
468 |     args = parser.parse_args()
469 | 
470 |     os.makedirs(os.path.join(args.save_folder), exist_ok=True)
471 |     processing_single_scene(args)
472 | 


--------------------------------------------------------------------------------
/ACMM.cpp:
--------------------------------------------------------------------------------
  1 | #include "ACMM.h"
  2 | 
  3 | #include <cstdarg>
  4 | 
  5 | void StringAppendV(std::string* dst, const char* format, va_list ap) {
  6 |   // First try with a small fixed size buffer.
  7 |   static const int kFixedBufferSize = 1024;
  8 |   char fixed_buffer[kFixedBufferSize];
  9 | 
 10 |   // It is possible for methods that use a va_list to invalidate
 11 |   // the data in it upon use.  The fix is to make a copy
 12 |   // of the structure before using it and use that copy instead.
 13 |   va_list backup_ap;
 14 |   va_copy(backup_ap, ap);
 15 |   int result = vsnprintf(fixed_buffer, kFixedBufferSize, format, backup_ap);
 16 |   va_end(backup_ap);
 17 | 
 18 |   if (result < kFixedBufferSize) {
 19 |     if (result >= 0) {
 20 |       // Normal case - everything fits.
 21 |       dst->append(fixed_buffer, result);
 22 |       return;
 23 |     }
 24 | 
 25 | #ifdef _MSC_VER
 26 |     // Error or MSVC running out of space.  MSVC 8.0 and higher
 27 |     // can be asked about space needed with the special idiom below:
 28 |     va_copy(backup_ap, ap);
 29 |     result = vsnprintf(nullptr, 0, format, backup_ap);
 30 |     va_end(backup_ap);
 31 | #endif
 32 | 
 33 |     if (result < 0) {
 34 |       // Just an error.
 35 |       return;
 36 |     }
 37 |   }
 38 | 
 39 |   // Increase the buffer size to the size requested by vsnprintf,
 40 |   // plus one for the closing \0.
 41 |   const int variable_buffer_size = result + 1;
 42 |   std::unique_ptr<char> variable_buffer(new char[variable_buffer_size]);
 43 | 
 44 |   // Restore the va_list before we use it again.
 45 |   va_copy(backup_ap, ap);
 46 |   result =
 47 |       vsnprintf(variable_buffer.get(), variable_buffer_size, format, backup_ap);
 48 |   va_end(backup_ap);
 49 | 
 50 |   if (result >= 0 && result < variable_buffer_size) {
 51 |     dst->append(variable_buffer.get(), result);
 52 |   }
 53 | }
 54 | 
 55 | std::string StringPrintf(const char* format, ...) {
 56 |   va_list ap;
 57 |   va_start(ap, format);
 58 |   std::string result;
 59 |   StringAppendV(&result, format, ap);
 60 |   va_end(ap);
 61 |   return result;
 62 | }
 63 | 
 64 | void CudaSafeCall(const cudaError_t error, const std::string& file,
 65 |                   const int line) {
 66 |   if (error != cudaSuccess) {
 67 |     std::cerr << StringPrintf("%s in %s at line %i", cudaGetErrorString(error),
 68 |                               file.c_str(), line)
 69 |               << std::endl;
 70 |     exit(EXIT_FAILURE);
 71 |   }
 72 | }
 73 | 
 74 | void CudaCheckError(const char* file, const int line) {
 75 |   cudaError error = cudaGetLastError();
 76 |   if (error != cudaSuccess) {
 77 |     std::cerr << StringPrintf("cudaCheckError() failed at %s:%i : %s", file,
 78 |                               line, cudaGetErrorString(error))
 79 |               << std::endl;
 80 |     exit(EXIT_FAILURE);
 81 |   }
 82 | 
 83 |   // More careful checking. However, this will affect performance.
 84 |   // Comment away if needed.
 85 |   error = cudaDeviceSynchronize();
 86 |   if (cudaSuccess != error) {
 87 |     std::cerr << StringPrintf("cudaCheckError() with sync failed at %s:%i : %s",
 88 |                               file, line, cudaGetErrorString(error))
 89 |               << std::endl;
 90 |     std::cerr
 91 |         << "This error is likely caused by the graphics card timeout "
 92 |            "detection mechanism of your operating system. Please refer to "
 93 |            "the FAQ in the documentation on how to solve this problem."
 94 |         << std::endl;
 95 |     exit(EXIT_FAILURE);
 96 |   }
 97 | }
 98 | 
 99 | ACMM::ACMM() {}
100 | 
101 | ACMM::~ACMM()
102 | {
103 |     delete[] plane_hypotheses_host;
104 |     delete[] costs_host;
105 | 
106 |     for (int i = 0; i < num_images; ++i) {
107 |         cudaDestroyTextureObject(texture_objects_host.images[i]);
108 |         cudaFreeArray(cuArray[i]);
109 |     }
110 |     cudaFree(texture_objects_cuda);
111 |     cudaFree(cameras_cuda);
112 |     cudaFree(plane_hypotheses_cuda);
113 |     cudaFree(costs_cuda);
114 |     cudaFree(pre_costs_cuda);
115 |     cudaFree(rand_states_cuda);
116 |     cudaFree(selected_views_cuda);
117 |     cudaFree(depths_cuda);
118 | 
119 |     if (params.geom_consistency) {
120 |         for (int i = 0; i < num_images; ++i) {
121 |             cudaDestroyTextureObject(texture_depths_host.images[i]);
122 |             cudaFreeArray(cuDepthArray[i]);
123 |         }
124 |         cudaFree(texture_depths_cuda);
125 |     }
126 | 
127 |     if (params.hierarchy) {
128 |         delete[] scaled_plane_hypotheses_host;
129 |         delete[] pre_costs_host;
130 | 
131 |         cudaFree(scaled_plane_hypotheses_cuda);
132 |         cudaFree(pre_costs_cuda);
133 |     }
134 | 
135 | }
136 | 
137 | Camera ReadCamera(const std::string &cam_path)
138 | {
139 |     Camera camera;
140 |     std::ifstream file(cam_path);
141 | 
142 |     std::string line;
143 |     file >> line;
144 | 
145 |     for (int i = 0; i < 3; ++i) {
146 |         file >> camera.R[3 * i + 0] >> camera.R[3 * i + 1] >> camera.R[3 * i + 2] >> camera.t[i];
147 |     }
148 | 
149 |     float tmp[4];
150 |     file >> tmp[0] >> tmp[1] >> tmp[2] >> tmp[3];
151 |     file >> line;
152 | 
153 |     for (int i = 0; i < 3; ++i) {
154 |         file >> camera.K[3 * i + 0] >> camera.K[3 * i + 1] >> camera.K[3 * i + 2];
155 |     }
156 | 
157 |     float depth_num;
158 |     float interval;
159 |     file >> camera.depth_min >> interval >> depth_num >> camera.depth_max;
160 | 
161 |     return camera;
162 | }
163 | 
164 | void  RescaleImageAndCamera(cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst, cv::Mat_<float> &depth, Camera &camera)
165 | {
166 |     const int cols = depth.cols;
167 |     const int rows = depth.rows;
168 | 
169 |     if (cols == src.cols && rows == src.rows) {
170 |         dst = src.clone();
171 |         return;
172 |     }
173 | 
174 |     const float scale_x = cols / static_cast<float>(src.cols);
175 |     const float scale_y = rows / static_cast<float>(src.rows);
176 | 
177 |     cv::resize(src, dst, cv::Size(cols,rows), 0, 0, cv::INTER_LINEAR);
178 | 
179 |     camera.K[0] *= scale_x;
180 |     camera.K[2] *= scale_x;
181 |     camera.K[4] *= scale_y;
182 |     camera.K[5] *= scale_y;
183 |     camera.width = cols;
184 |     camera.height = rows;
185 | }
186 | 
187 | float3 Get3DPointonWorld(const int x, const int y, const float depth, const Camera camera)
188 | {
189 |     float3 pointX;
190 |     float3 tmpX;
191 |     // Reprojection
192 |     pointX.x = depth * (x - camera.K[2]) / camera.K[0];
193 |     pointX.y = depth * (y - camera.K[5]) / camera.K[4];
194 |     pointX.z = depth;
195 | 
196 |     // Rotation
197 |     tmpX.x = camera.R[0] * pointX.x + camera.R[3] * pointX.y + camera.R[6] * pointX.z;
198 |     tmpX.y = camera.R[1] * pointX.x + camera.R[4] * pointX.y + camera.R[7] * pointX.z;
199 |     tmpX.z = camera.R[2] * pointX.x + camera.R[5] * pointX.y + camera.R[8] * pointX.z;
200 | 
201 |     // Transformation
202 |     float3 C;
203 |     C.x = -(camera.R[0] * camera.t[0] + camera.R[3] * camera.t[1] + camera.R[6] * camera.t[2]);
204 |     C.y = -(camera.R[1] * camera.t[0] + camera.R[4] * camera.t[1] + camera.R[7] * camera.t[2]);
205 |     C.z = -(camera.R[2] * camera.t[0] + camera.R[5] * camera.t[1] + camera.R[8] * camera.t[2]);
206 |     pointX.x = tmpX.x + C.x;
207 |     pointX.y = tmpX.y + C.y;
208 |     pointX.z = tmpX.z + C.z;
209 | 
210 |     return pointX;
211 | }
212 | 
213 | void ProjectonCamera(const float3 PointX, const Camera camera, float2 &point, float &depth)
214 | {
215 |     float3 tmp;
216 |     tmp.x = camera.R[0] * PointX.x + camera.R[1] * PointX.y + camera.R[2] * PointX.z + camera.t[0];
217 |     tmp.y = camera.R[3] * PointX.x + camera.R[4] * PointX.y + camera.R[5] * PointX.z + camera.t[1];
218 |     tmp.z = camera.R[6] * PointX.x + camera.R[7] * PointX.y + camera.R[8] * PointX.z + camera.t[2];
219 | 
220 |     depth = camera.K[6] * tmp.x + camera.K[7] * tmp.y + camera.K[8] * tmp.z;
221 |     point.x = (camera.K[0] * tmp.x + camera.K[1] * tmp.y + camera.K[2] * tmp.z) / depth;
222 |     point.y = (camera.K[3] * tmp.x + camera.K[4] * tmp.y + camera.K[5] * tmp.z) / depth;
223 | }
224 | 
225 | float GetAngle( const cv::Vec3f &v1, const cv::Vec3f &v2 )
226 | {
227 |     float dot_product = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
228 |     float angle = acosf(dot_product);
229 |     //if angle is not a number the dot product was 1 and thus the two vectors should be identical --> return 0
230 |     if ( angle != angle )
231 |         return 0.0f;
232 | 
233 |     return angle;
234 | }
235 | 
236 | int readDepthDmb(const std::string file_path, cv::Mat_<float> &depth)
237 | {
238 |     FILE *inimage;
239 |     inimage = fopen(file_path.c_str(), "rb");
240 |     if (!inimage){
241 |         std::cout << "Error opening file " << file_path << std::endl;
242 |         return -1;
243 |     }
244 | 
245 |     int32_t type, h, w, nb;
246 | 
247 |     type = -1;
248 | 
249 |     fread(&type,sizeof(int32_t),1,inimage);
250 |     fread(&h,sizeof(int32_t),1,inimage);
251 |     fread(&w,sizeof(int32_t),1,inimage);
252 |     fread(&nb,sizeof(int32_t),1,inimage);
253 | 
254 |     if (type != 1) {
255 |         fclose(inimage);
256 |         return -1;
257 |     }
258 | 
259 |     int32_t dataSize = h*w*nb;
260 | 
261 |     depth = cv::Mat::zeros(h,w,CV_32F);
262 |     fread(depth.data,sizeof(float),dataSize,inimage);
263 | 
264 |     fclose(inimage);
265 |     return 0;
266 | }
267 | 
268 | int writeDepthDmb(const std::string file_path, const cv::Mat_<float> depth)
269 | {
270 |     FILE *outimage;
271 |     outimage = fopen(file_path.c_str(), "wb");
272 |     if (!outimage) {
273 |         std::cout << "Error opening file " << file_path << std::endl;
274 |     }
275 | 
276 |     int32_t type = 1;
277 |     int32_t h = depth.rows;
278 |     int32_t w = depth.cols;
279 |     int32_t nb = 1;
280 | 
281 |     fwrite(&type,sizeof(int32_t),1,outimage);
282 |     fwrite(&h,sizeof(int32_t),1,outimage);
283 |     fwrite(&w,sizeof(int32_t),1,outimage);
284 |     fwrite(&nb,sizeof(int32_t),1,outimage);
285 | 
286 |     float* data = (float*)depth.data;
287 | 
288 |     int32_t datasize = w*h*nb;
289 |     fwrite(data,sizeof(float),datasize,outimage);
290 | 
291 |     fclose(outimage);
292 |     return 0;
293 | }
294 | 
295 | int readNormalDmb (const std::string file_path, cv::Mat_<cv::Vec3f> &normal)
296 | {
297 |     FILE *inimage;
298 |     inimage = fopen(file_path.c_str(), "rb");
299 |     if (!inimage) {
300 |         std::cout << "Error opening file " << file_path << std::endl;
301 |         return -1;
302 |     }
303 | 
304 |     int32_t type, h, w, nb;
305 | 
306 |     type = -1;
307 | 
308 |     fread(&type,sizeof(int32_t),1,inimage);
309 |     fread(&h,sizeof(int32_t),1,inimage);
310 |     fread(&w,sizeof(int32_t),1,inimage);
311 |     fread(&nb,sizeof(int32_t),1,inimage);
312 | 
313 |     if (type != 1) {
314 |         fclose(inimage);
315 |         return -1;
316 |     }
317 | 
318 |     int32_t dataSize = h*w*nb;
319 | 
320 |     normal = cv::Mat::zeros(h,w,CV_32FC3);
321 |     fread(normal.data,sizeof(float),dataSize,inimage);
322 | 
323 |     fclose(inimage);
324 |     return 0;
325 | }
326 | 
327 | int writeNormalDmb(const std::string file_path, const cv::Mat_<cv::Vec3f> normal)
328 | {
329 |     FILE *outimage;
330 |     outimage = fopen(file_path.c_str(), "wb");
331 |     if (!outimage) {
332 |         std::cout << "Error opening file " << file_path << std::endl;
333 |     }
334 | 
335 |     int32_t type = 1; //float
336 |     int32_t h = normal.rows;
337 |     int32_t w = normal.cols;
338 |     int32_t nb = 3;
339 | 
340 |     fwrite(&type,sizeof(int32_t),1,outimage);
341 |     fwrite(&h,sizeof(int32_t),1,outimage);
342 |     fwrite(&w,sizeof(int32_t),1,outimage);
343 |     fwrite(&nb,sizeof(int32_t),1,outimage);
344 | 
345 |     float* data = (float*)normal.data;
346 | 
347 |     int32_t datasize = w*h*nb;
348 |     fwrite(data,sizeof(float),datasize,outimage);
349 | 
350 |     fclose(outimage);
351 |     return 0;
352 | }
353 | 
354 | void StoreColorPlyFileBinaryPointCloud (const std::string &plyFilePath, const std::vector<PointList> &pc)
355 | {
356 |     std::cout << "store 3D points to ply file" << std::endl;
357 | 
358 |     FILE *outputPly;
359 |     outputPly=fopen(plyFilePath.c_str(), "wb");
360 | 
361 |     /*write header*/
362 |     fprintf(outputPly, "ply\n");
363 |     fprintf(outputPly, "format binary_little_endian 1.0\n");
364 |     fprintf(outputPly, "element vertex %d\n",pc.size());
365 |     fprintf(outputPly, "property float x\n");
366 |     fprintf(outputPly, "property float y\n");
367 |     fprintf(outputPly, "property float z\n");
368 |     fprintf(outputPly, "property float nx\n");
369 |     fprintf(outputPly, "property float ny\n");
370 |     fprintf(outputPly, "property float nz\n");
371 |     fprintf(outputPly, "property uchar red\n");
372 |     fprintf(outputPly, "property uchar green\n");
373 |     fprintf(outputPly, "property uchar blue\n");
374 |     fprintf(outputPly, "end_header\n");
375 | 
376 |     //write data
377 | #pragma omp parallel for
378 |     for(size_t i = 0; i < pc.size(); i++) {
379 |         const PointList &p = pc[i];
380 |         float3 X = p.coord;
381 |         const float3 normal = p.normal;
382 |         const float3 color = p.color;
383 |         const char b_color = (int)color.x;
384 |         const char g_color = (int)color.y;
385 |         const char r_color = (int)color.z;
386 | 
387 |         if(!(X.x < FLT_MAX && X.x > -FLT_MAX) || !(X.y < FLT_MAX && X.y > -FLT_MAX) || !(X.z < FLT_MAX && X.z >= -FLT_MAX)){
388 |             X.x = 0.0f;
389 |             X.y = 0.0f;
390 |             X.z = 0.0f;
391 |         }
392 | #pragma omp critical
393 |         {
394 |             fwrite(&X.x,      sizeof(X.x), 1, outputPly);
395 |             fwrite(&X.y,      sizeof(X.y), 1, outputPly);
396 |             fwrite(&X.z,      sizeof(X.z), 1, outputPly);
397 |             fwrite(&normal.x, sizeof(normal.x), 1, outputPly);
398 |             fwrite(&normal.y, sizeof(normal.y), 1, outputPly);
399 |             fwrite(&normal.z, sizeof(normal.z), 1, outputPly);
400 |             fwrite(&r_color,  sizeof(char), 1, outputPly);
401 |             fwrite(&g_color,  sizeof(char), 1, outputPly);
402 |             fwrite(&b_color,  sizeof(char), 1, outputPly);
403 |         }
404 | 
405 |     }
406 |     fclose(outputPly);
407 | }
408 | 
409 | static float GetDisparity(const Camera &camera, const int2 &p, const float &depth)
410 | {
411 |     float point3D[3];
412 |     point3D[0] = depth * (p.x - camera.K[2]) / camera.K[0];
413 |     point3D[1] = depth * (p.y - camera.K[5]) / camera.K[4];
414 |     point3D[2] = depth;
415 | 
416 |     return std::sqrt(point3D[0] * point3D[0] + point3D[1] * point3D[1] + point3D[2] * point3D[2]);
417 | }
418 | 
419 | void ACMM::SetGeomConsistencyParams(bool multi_gemetry=false)
420 | {
421 |     params.geom_consistency = true;
422 |     params.max_iterations = 2;
423 |     if (multi_gemetry) {
424 |        params.multi_geometry = true;
425 |     }
426 | }
427 | 
428 | void ACMM::SetHierarchyParams()
429 | {
430 |     params.hierarchy = true;
431 | }
432 | 
433 | void ACMM::InuputInitialization(const std::string &dense_folder, const std::vector<Problem> &problems, const int idx)
434 | {
435 |     images.clear();
436 |     cameras.clear();
437 |     const Problem problem = problems[idx];
438 | 
439 |     std::string image_folder = dense_folder + std::string("/images");
440 |     std::string cam_folder = dense_folder + std::string("/cams");
441 | 
442 |     std::stringstream image_path;
443 |     image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problem.ref_image_id << ".jpg";
444 |     cv::Mat_<uint8_t> image_uint = cv::imread(image_path.str(), cv::IMREAD_GRAYSCALE);
445 |     cv::Mat image_float;
446 |     image_uint.convertTo(image_float, CV_32FC1);
447 |     images.push_back(image_float);
448 |     std::stringstream cam_path;
449 |     cam_path << cam_folder << "/" << std::setw(8) << std::setfill('0') << problem.ref_image_id << "_cam.txt";
450 |     Camera camera = ReadCamera(cam_path.str());
451 |     camera.height = image_float.rows;
452 |     camera.width = image_float.cols;
453 |     cameras.push_back(camera);
454 | 
455 |     size_t num_src_images = problem.src_image_ids.size();
456 |     for (size_t i = 0; i < num_src_images; ++i) {
457 |         std::stringstream image_path;
458 |         image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problem.src_image_ids[i] << ".jpg";
459 |         cv::Mat_<uint8_t> image_uint = cv::imread(image_path.str(), cv::IMREAD_GRAYSCALE);
460 |         cv::Mat image_float;
461 |         image_uint.convertTo(image_float, CV_32FC1);
462 |         images.push_back(image_float);
463 |         std::stringstream cam_path;
464 |         cam_path << cam_folder << "/" << std::setw(8) << std::setfill('0') << problem.src_image_ids[i] << "_cam.txt";
465 |         Camera camera = ReadCamera(cam_path.str());
466 |         camera.height = image_float.rows;
467 |         camera.width = image_float.cols;
468 |         cameras.push_back(camera);
469 |     }
470 | 
471 |     // Scale cameras and images
472 |     int max_image_size = problems[idx].cur_image_size;
473 |     for (size_t i = 0; i < images.size(); ++i) {
474 |         if (i > 0) {
475 |             max_image_size = problems[problem.src_image_ids[i - 1]].cur_image_size;
476 |         }
477 | 
478 |         if (images[i].cols <= max_image_size && images[i].rows <= max_image_size) {
479 |             continue;
480 |         }
481 | 
482 |         const float factor_x = static_cast<float>(max_image_size) / images[i].cols;
483 |         const float factor_y = static_cast<float>(max_image_size) / images[i].rows;
484 |         const float factor = std::min(factor_x, factor_y);
485 | 
486 |         const int new_cols = std::round(images[i].cols * factor);
487 |         const int new_rows = std::round(images[i].rows * factor);
488 | 
489 |         const float scale_x = new_cols / static_cast<float>(images[i].cols);
490 |         const float scale_y = new_rows / static_cast<float>(images[i].rows);
491 | 
492 |         cv::Mat_<float> scaled_image_float;
493 |         cv::resize(images[i], scaled_image_float, cv::Size(new_cols,new_rows), 0, 0, cv::INTER_LINEAR);
494 |         images[i] = scaled_image_float.clone();
495 | 
496 |         cameras[i].K[0] *= scale_x;
497 |         cameras[i].K[2] *= scale_x;
498 |         cameras[i].K[4] *= scale_y;
499 |         cameras[i].K[5] *= scale_y;
500 |         cameras[i].height = scaled_image_float.rows;
501 |         cameras[i].width = scaled_image_float.cols;
502 |     }
503 | 
504 |     params.depth_min = cameras[0].depth_min * 0.6f;
505 |     params.depth_max = cameras[0].depth_max * 1.2f;
506 |     std::cout << "depthe range: " << params.depth_min << " " << params.depth_max << std::endl;
507 |     params.num_images = (int)images.size();
508 |     std::cout << "num images: " << params.num_images << std::endl;
509 |     params.disparity_min = cameras[0].K[0] * params.baseline / params.depth_max;
510 |     params.disparity_max = cameras[0].K[0] * params.baseline / params.depth_min;
511 | 
512 |     if (params.geom_consistency) {
513 |         depths.clear();
514 | 
515 |         std::stringstream result_path;
516 |         result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
517 |         std::string result_folder = result_path.str();
518 |         std::string suffix = "/depths.dmb";
519 |         if (params.multi_geometry) {
520 |             suffix = "/depths_geom.dmb";
521 |         }
522 |         std::string depth_path = result_folder + suffix;
523 |         cv::Mat_<float> ref_depth;
524 |         readDepthDmb(depth_path, ref_depth);
525 |         depths.push_back(ref_depth);
526 | 
527 |         size_t num_src_images = problem.src_image_ids.size();
528 |         for (size_t i = 0; i < num_src_images; ++i) {
529 |             std::stringstream result_path;
530 |             result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problem.src_image_ids[i];
531 |             std::string result_folder = result_path.str();
532 |             std::string depth_path = result_folder + suffix;
533 |             cv::Mat_<float> depth;
534 |             readDepthDmb(depth_path, depth);
535 |             depths.push_back(depth);
536 |         }
537 |     }
538 | }
539 | 
540 | void ACMM::CudaSpaceInitialization(const std::string &dense_folder, const Problem &problem)
541 | {
542 |     num_images = (int)images.size();
543 | 
544 |     for (int i = 0; i < num_images; ++i) {
545 |         int rows = images[i].rows;
546 |         int cols = images[i].cols;
547 | 
548 |         cudaChannelFormatDesc channelDesc = cudaCreateChannelDesc(32, 0, 0, 0, cudaChannelFormatKindFloat);
549 |         cudaMallocArray(&cuArray[i], &channelDesc, cols, rows);
550 |         cudaMemcpy2DToArray (cuArray[i], 0, 0, images[i].ptr<float>(), images[i].step[0], cols*sizeof(float), rows, cudaMemcpyHostToDevice);
551 | 
552 |         struct cudaResourceDesc resDesc;
553 |         memset(&resDesc, 0, sizeof(cudaResourceDesc));
554 |         resDesc.resType = cudaResourceTypeArray;
555 |         resDesc.res.array.array = cuArray[i];
556 | 
557 |         struct cudaTextureDesc texDesc;
558 |         memset(&texDesc, 0, sizeof(cudaTextureDesc));
559 |         texDesc.addressMode[0] = cudaAddressModeWrap;
560 |         texDesc.addressMode[1] = cudaAddressModeWrap;
561 |         texDesc.filterMode = cudaFilterModeLinear;
562 |         texDesc.readMode  = cudaReadModeElementType;
563 |         texDesc.normalizedCoords = 0;
564 | 
565 |         cudaCreateTextureObject(&(texture_objects_host.images[i]), &resDesc, &texDesc, NULL);
566 |     }
567 |     cudaMalloc((void**)&texture_objects_cuda, sizeof(cudaTextureObjects));
568 |     cudaMemcpy(texture_objects_cuda, &texture_objects_host, sizeof(cudaTextureObjects), cudaMemcpyHostToDevice);
569 | 
570 |     cudaMalloc((void**)&cameras_cuda, sizeof(Camera) * (num_images));
571 |     cudaMemcpy(cameras_cuda, &cameras[0], sizeof(Camera) * (num_images), cudaMemcpyHostToDevice);
572 | 
573 |     plane_hypotheses_host = new float4[cameras[0].height * cameras[0].width];
574 |     cudaMalloc((void**)&plane_hypotheses_cuda, sizeof(float4) * (cameras[0].height * cameras[0].width));
575 | 
576 |     costs_host = new float[cameras[0].height * cameras[0].width];
577 |     cudaMalloc((void**)&costs_cuda, sizeof(float) * (cameras[0].height * cameras[0].width));
578 |     cudaMalloc((void**)&pre_costs_cuda, sizeof(float) * (cameras[0].height * cameras[0].width));
579 | 
580 |     cudaMalloc((void**)&rand_states_cuda, sizeof(curandState) * (cameras[0].height * cameras[0].width));
581 |     cudaMalloc((void**)&selected_views_cuda, sizeof(unsigned int) * (cameras[0].height * cameras[0].width));
582 | 
583 |     cudaMalloc((void**)&depths_cuda, sizeof(float) * (cameras[0].height * cameras[0].width));
584 | 
585 |     if (params.geom_consistency) {
586 |         for (int i = 0; i < num_images; ++i) {
587 |             int rows = depths[i].rows;
588 |             int cols = depths[i].cols;
589 | 
590 |             cudaChannelFormatDesc channelDesc = cudaCreateChannelDesc(32, 0, 0, 0, cudaChannelFormatKindFloat);
591 |             cudaMallocArray(&cuDepthArray[i], &channelDesc, cols, rows);
592 |             cudaMemcpy2DToArray (cuDepthArray[i], 0, 0, depths[i].ptr<float>(), depths[i].step[0], cols*sizeof(float), rows, cudaMemcpyHostToDevice);
593 | 
594 |             struct cudaResourceDesc resDesc;
595 |             memset(&resDesc, 0, sizeof(cudaResourceDesc));
596 |             resDesc.resType = cudaResourceTypeArray;
597 |             resDesc.res.array.array = cuDepthArray[i];
598 | 
599 |             struct cudaTextureDesc texDesc;
600 |             memset(&texDesc, 0, sizeof(cudaTextureDesc));
601 |             texDesc.addressMode[0] = cudaAddressModeWrap;
602 |             texDesc.addressMode[1] = cudaAddressModeWrap;
603 |             texDesc.filterMode = cudaFilterModeLinear;
604 |             texDesc.readMode  = cudaReadModeElementType;
605 |             texDesc.normalizedCoords = 0;
606 | 
607 |             cudaCreateTextureObject(&(texture_depths_host.images[i]), &resDesc, &texDesc, NULL);
608 |         }
609 |         cudaMalloc((void**)&texture_depths_cuda, sizeof(cudaTextureObjects));
610 |         cudaMemcpy(texture_depths_cuda, &texture_depths_host, sizeof(cudaTextureObjects), cudaMemcpyHostToDevice);
611 | 
612 |         std::stringstream result_path;
613 |         result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
614 |         std::string result_folder = result_path.str();
615 |         std::string suffix = "/depths.dmb";
616 |         if (params.multi_geometry) {
617 |             suffix = "/depths_geom.dmb";
618 |         }
619 |         std::string depth_path = result_folder + suffix;
620 |         std::string normal_path = result_folder + "/normals.dmb";
621 |         std::string cost_path = result_folder + "/costs.dmb";
622 |         cv::Mat_<float> ref_depth;
623 |         cv::Mat_<cv::Vec3f> ref_normal;
624 |         cv::Mat_<float> ref_cost;
625 |         readDepthDmb(depth_path, ref_depth);
626 |         depths.push_back(ref_depth);
627 |         readNormalDmb(normal_path, ref_normal);
628 |         readDepthDmb(cost_path, ref_cost);
629 |         int width = ref_depth.cols;
630 |         int height = ref_depth.rows;
631 |         for (int col = 0; col < width; ++col) {
632 |             for (int row = 0; row < height; ++row) {
633 |                 int center = row * width + col;
634 |                 float4 plane_hypothesis;
635 |                 plane_hypothesis.x = ref_normal(row, col)[0];
636 |                 plane_hypothesis.y = ref_normal(row, col)[1];
637 |                 plane_hypothesis.z = ref_normal(row, col)[2];
638 |                 plane_hypothesis.w = ref_depth(row, col);
639 |                 plane_hypotheses_host[center] = plane_hypothesis;
640 |                 costs_host[center] = ref_cost(row, col);
641 |             }
642 |         }
643 |         cudaMemcpy(plane_hypotheses_cuda, plane_hypotheses_host, sizeof(float4) * width * height, cudaMemcpyHostToDevice);
644 |         cudaMemcpy(costs_cuda, costs_host, sizeof(float) * width * height, cudaMemcpyHostToDevice);
645 |     }
646 | 
647 |     if (params.hierarchy) {
648 |         std::stringstream result_path;
649 |         result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
650 |         std::string result_folder = result_path.str();
651 |         std::string depth_path = result_folder + "/depths.dmb";
652 |         std::string normal_path = result_folder + "/normals.dmb";
653 |         std::string cost_path = result_folder + "/costs.dmb";
654 |         cv::Mat_<float> ref_depth;
655 |         cv::Mat_<cv::Vec3f> ref_normal;
656 |         cv::Mat_<float> ref_cost;
657 |         readDepthDmb(depth_path, ref_depth);
658 |         depths.push_back(ref_depth);
659 |         readNormalDmb(normal_path, ref_normal);
660 |         readDepthDmb(cost_path, ref_cost);
661 |         int width = ref_normal.cols;
662 |         int height = ref_normal.rows;
663 |         scaled_plane_hypotheses_host= new float4[height * width];
664 |         cudaMalloc((void**)&scaled_plane_hypotheses_cuda, sizeof(float4) * height * width);
665 |         pre_costs_host = new float[height * width];
666 |         cudaMalloc((void**)&pre_costs_cuda, sizeof(float) * cameras[0].height * cameras[0].width);
667 |         if (width !=images[0]. rows || height != images[0].cols) {
668 |             params.upsample = true;
669 |             params.scaled_cols = width;
670 |             params.scaled_rows = height;
671 |         }
672 |         else {
673 |             params.upsample = false;
674 |         }
675 |         for (int col = 0; col < width; ++col) {
676 |             for (int row = 0; row < height; ++row) {
677 |                 int center = row * width + col;
678 |                 float4 plane_hypothesis;
679 |                 plane_hypothesis.x = ref_normal(row, col)[0];
680 |                 plane_hypothesis.y = ref_normal(row, col)[1];
681 |                 plane_hypothesis.z = ref_normal(row, col)[2];
682 |                 if (params.upsample) {
683 |                     plane_hypothesis.w = ref_cost(row, col);
684 |                 }
685 |                 else {
686 |                     plane_hypothesis.w = ref_depth(row, col);
687 |                 }
688 |                 scaled_plane_hypotheses_host[center] = plane_hypothesis;
689 |             }
690 |         }
691 | 
692 |         for (int col = 0; col < cameras[0].width; ++col) {
693 |             for (int row = 0; row < cameras[0].height; ++row) {
694 |                 int center = row * cameras[0].width + col;
695 |                 float4 plane_hypothesis;
696 |                 plane_hypothesis.w = ref_depth(row, col);
697 |                 plane_hypotheses_host[center] = plane_hypothesis;
698 |             }
699 |         }
700 | 
701 |         cudaMemcpy(scaled_plane_hypotheses_cuda, scaled_plane_hypotheses_host, sizeof(float4) * height * width, cudaMemcpyHostToDevice);
702 |         cudaMemcpy(plane_hypotheses_cuda, plane_hypotheses_host, sizeof(float4) * cameras[0].width * cameras[0].height, cudaMemcpyHostToDevice);
703 |     }
704 | }
705 | 
706 | int ACMM::GetReferenceImageWidth()
707 | {
708 |     return cameras[0].width;
709 | }
710 | 
711 | int ACMM::GetReferenceImageHeight()
712 | {
713 |     return cameras[0].height;
714 | }
715 | 
716 | cv::Mat ACMM::GetReferenceImage()
717 | {
718 |     return images[0];
719 | }
720 | 
721 | float4 ACMM::GetPlaneHypothesis(const int index)
722 | {
723 |     return plane_hypotheses_host[index];
724 | }
725 | 
726 | float ACMM::GetCost(const int index)
727 | {
728 |     return costs_host[index];
729 | }
730 | 
731 |  void JBUAddImageToTextureFloatGray ( std::vector<cv::Mat_<float>>  &imgs, cudaTextureObject_t texs[], cudaArray *cuArray[], const int &numSelViews)
732 | {
733 |     for (int i=0; i<numSelViews; i++) {
734 |         int index = i;
735 |         int rows = imgs[index].rows;
736 |         int cols = imgs[index].cols;
737 |         // Create channel with floating point type
738 |         cudaChannelFormatDesc channelDesc = cudaCreateChannelDesc (32, 0, 0,  0, cudaChannelFormatKindFloat);
739 |         // Allocate array with correct size and number of channels
740 |         cudaMallocArray(&cuArray[i], &channelDesc, cols, rows);
741 |         cudaMemcpy2DToArray (cuArray[i], 0, 0, imgs[index].ptr<float>(), imgs[index].step[0], cols*sizeof(float), rows, cudaMemcpyHostToDevice);
742 | 
743 |         // Specify texture
744 |         struct cudaResourceDesc resDesc;
745 |         memset(&resDesc, 0, sizeof(cudaResourceDesc));
746 |         resDesc.resType         = cudaResourceTypeArray;
747 |         resDesc.res.array.array = cuArray[i];
748 | 
749 |         // Specify texture object parameters
750 |         struct cudaTextureDesc texDesc;
751 |         memset(&texDesc, 0, sizeof(cudaTextureDesc));
752 |         texDesc.addressMode[0]   = cudaAddressModeWrap;
753 |         texDesc.addressMode[1]   = cudaAddressModeWrap;
754 |         texDesc.filterMode       = cudaFilterModeLinear;
755 |         texDesc.readMode         = cudaReadModeElementType;
756 |         texDesc.normalizedCoords = 0;
757 | 
758 |         // Create texture object
759 |         cudaCreateTextureObject(&(texs[i]), &resDesc, &texDesc, NULL);
760 |     }
761 |     return;
762 | }
763 | 
764 |  JBU::JBU(){}
765 | 
766 |  JBU::~JBU()
767 |  {
768 |      free(depth_h);
769 | 
770 |      cudaFree(depth_d);
771 |      cudaFree(jp_d);
772 |      cudaFree(jt_d);
773 |  }
774 | 
775 |  void JBU::InitializeParameters(int n)
776 |  {
777 |      depth_h = (float*)malloc(sizeof(float) * n);
778 | 
779 |      cudaMalloc ((void**)&depth_d,  sizeof(float) * n);
780 | 
781 |      cudaMalloc((void**)&jp_d, sizeof(JBUParameters) * 1);
782 |      cudaMemcpy(jp_d, &jp_h, sizeof(JBUParameters) * 1, cudaMemcpyHostToDevice);
783 | 
784 |      cudaMalloc((void**)&jt_d, sizeof(JBUTexObj) * 1);
785 |      cudaMemcpy(jt_d, &jt_h, sizeof(JBUTexObj) * 1, cudaMemcpyHostToDevice);
786 |      cudaDeviceSynchronize();
787 |  }
788 | 
789 | void RunJBU(const cv::Mat_<float>  &scaled_image_float, const cv::Mat_<float> &src_depthmap, const std::string &dense_folder , const Problem &problem)
790 | {
791 |     uint32_t rows = scaled_image_float.rows;
792 |     uint32_t cols = scaled_image_float.cols;
793 |     int Imagescale = std::max(scaled_image_float.rows / src_depthmap.rows, scaled_image_float.cols / src_depthmap.cols);
794 | 
795 |     if (Imagescale == 1) {
796 |         std::cout << "Image.rows = Depthmap.rows" << std::endl;
797 |         return;
798 |     }
799 | 
800 |     std::vector<cv::Mat_<float> > imgs(JBU_NUM);
801 |     imgs[0] = scaled_image_float.clone();
802 |     imgs[1] = src_depthmap.clone();
803 | 
804 |     JBU jbu;
805 |     jbu.jp_h.height = rows;
806 |     jbu.jp_h.width = cols;
807 |     jbu.jp_h.s_height = src_depthmap.rows;
808 |     jbu.jp_h.s_width = src_depthmap.cols;
809 |     jbu.jp_h.Imagescale = Imagescale;
810 |     JBUAddImageToTextureFloatGray(imgs, jbu.jt_h.imgs, jbu.cuArray, JBU_NUM);
811 | 
812 |     jbu.InitializeParameters(rows * cols);
813 |     jbu.CudaRun();
814 | 
815 |     cv::Mat_<float> depthmap = cv::Mat::zeros( rows, cols, CV_32FC1 );
816 | 
817 |     for (uint32_t i = 0; i < cols; ++i) {
818 |         for(uint32_t j = 0; j < rows; ++j) {
819 |             int center = i + cols * j;
820 |             if (jbu.depth_h[center] != jbu.depth_h[center]) {
821 |                 std::cout << "wrong!" << std::endl;
822 |             }
823 |             depthmap (j, i) = jbu.depth_h[center];
824 |         }
825 |     }
826 | 
827 |     cv::Mat_<float> disp0 = depthmap.clone();
828 |     std::stringstream result_path;
829 |     result_path << dense_folder << "/ACMM" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
830 |     std::string result_folder = result_path.str();
831 |     mkdir(result_folder.c_str(), 777);
832 |     std::string depth_path = result_folder + "/depths.dmb";
833 |     writeDepthDmb ( depth_path, disp0 );
834 | 
835 |     for (int i=0; i < JBU_NUM; i++) {
836 |         CUDA_SAFE_CALL( cudaDestroyTextureObject(jbu.jt_h.imgs[i]) );
837 |         CUDA_SAFE_CALL( cudaFreeArray(jbu.cuArray[i]) );
838 |     }
839 |     cudaDeviceSynchronize();
840 | }
841 | 


--------------------------------------------------------------------------------
/ACMM.cu:
--------------------------------------------------------------------------------
   1 | #include "ACMM.h"
   2 | 
   3 | #define mul4(v,k) { \
   4 |     v->x = v->x * k; \
   5 |     v->y = v->y * k; \
   6 |     v->z = v->z * k;\
   7 | }
   8 | 
   9 | #define vecdiv4(v,k) { \
  10 |     v->x = v->x / k; \
  11 |     v->y = v->y / k; \
  12 |     v->z = v->z / k;\
  13 | }
  14 | 
  15 | __device__  void sort_small(float *d, const int n)
  16 | {
  17 |     int j;
  18 |     for (int i = 1; i < n; i++) {
  19 |         float tmp = d[i];
  20 |         for (j = i; j >= 1 && tmp < d[j-1]; j--)
  21 |             d[j] = d[j-1];
  22 |         d[j] = tmp;
  23 |     }
  24 | }
  25 | 
  26 | __device__ void sort_small_weighted(float *d, float *w, int n)
  27 | {
  28 |     int j;
  29 |     for (int i = 1; i < n; i++) {
  30 |         float tmp = d[i];
  31 |         float tmp_w = w[i];
  32 |         for (j = i; j >= 1 && tmp < d[j - 1]; j--) {
  33 |             d[j] = d[j - 1];
  34 |             w[j] = w[j - 1];
  35 |         }
  36 |         d[j] = tmp;
  37 |         w[j] = tmp_w;
  38 |     }
  39 | }
  40 | 
  41 | __device__ int FindMinCostIndex(const float *costs, const int n)
  42 | {
  43 |     float min_cost = costs[0];
  44 |     int min_cost_idx = 0;
  45 |     for (int idx = 1; idx < n; ++idx) {
  46 |         if (costs[idx] <= min_cost) {
  47 |             min_cost = costs[idx];
  48 |             min_cost_idx = idx;
  49 |         }
  50 |     }
  51 |     return min_cost_idx;
  52 | }
  53 | 
  54 | __device__  void setBit(unsigned int &input, const unsigned int n)
  55 | {
  56 |     input |= (unsigned int)(1 << n);
  57 | }
  58 | 
  59 | __device__  int isSet(unsigned int input, const unsigned int n)
  60 | {
  61 |     return (input >> n) & 1;
  62 | }
  63 | 
  64 | __device__ void Mat33DotVec3(const float mat[9], const float4 vec, float4 *result)
  65 | {
  66 |   result->x = mat[0] * vec.x + mat[1] * vec.y + mat[2] * vec.z;
  67 |   result->y = mat[3] * vec.x + mat[4] * vec.y + mat[5] * vec.z;
  68 |   result->z = mat[6] * vec.x + mat[7] * vec.y + mat[8] * vec.z;
  69 | }
  70 | 
  71 | __device__ float Vec3DotVec3(const float4 vec1, const float4 vec2)
  72 | {
  73 |     return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
  74 | }
  75 | 
  76 | __device__ void NormalizeVec3 (float4 *vec)
  77 | {
  78 |     const float normSquared = vec->x * vec->x + vec->y * vec->y + vec->z * vec->z;
  79 |     const float inverse_sqrt = rsqrtf (normSquared);
  80 |     vec->x *= inverse_sqrt;
  81 |     vec->y *= inverse_sqrt;
  82 |     vec->z *= inverse_sqrt;
  83 | }
  84 | 
  85 | __device__ void TransformPDFToCDF(float* probs, const int num_probs)
  86 | {
  87 |     float prob_sum = 0.0f;
  88 |     for (int i = 0; i < num_probs; ++i) {
  89 |         prob_sum += probs[i];
  90 |     }
  91 |     const float inv_prob_sum = 1.0f / prob_sum;
  92 | 
  93 |     float cum_prob = 0.0f;
  94 |     for (int i = 0; i < num_probs; ++i) {
  95 |         const float prob = probs[i] * inv_prob_sum;
  96 |         cum_prob += prob;
  97 |         probs[i] = cum_prob;
  98 |     }
  99 | }
 100 | 
 101 | __device__ void Get3DPoint(const Camera camera, const int2 p, const float depth, float *X)
 102 | {
 103 |     X[0] = depth * (p.x - camera.K[2]) / camera.K[0];
 104 |     X[1] = depth * (p.y - camera.K[5]) / camera.K[4];
 105 |     X[2] = depth;
 106 | }
 107 | 
 108 | __device__ float4 GetViewDirection(const Camera camera, const int2 p, const float depth)
 109 | {
 110 |     float X[3];
 111 |     Get3DPoint(camera, p, depth, X);
 112 |     float norm = sqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2]);
 113 | 
 114 |     float4 view_direction;
 115 |     view_direction.x = X[0] / norm;
 116 |     view_direction.y = X[1] / norm;
 117 |     view_direction.z =  X[2] / norm;
 118 |     view_direction.w = 0;
 119 |     return view_direction;
 120 | }
 121 | 
 122 | __device__ float GetDistance2Origin(const Camera camera, const int2 p, const float depth, const float4 normal)
 123 | {
 124 |     float X[3];
 125 |     Get3DPoint(camera, p, depth, X);
 126 |     return -(normal.x * X[0] + normal.y * X[1] + normal.z * X[2]);
 127 | }
 128 | 
 129 | __device__   float SpatialGauss(float x1, float y1, float x2, float y2, float sigma, float mu = 0.0)
 130 | {
 131 |     float dis = pow(x1 - x2, 2) + pow(y1 - y2, 2) - mu;
 132 |     return exp(-1.0 * dis / (2 * sigma * sigma));
 133 | }
 134 | 
 135 | __device__  float RangeGauss(float x, float sigma, float mu = 0.0)
 136 | {
 137 |     float x_p = x - mu;
 138 |     return exp(-1.0 * (x_p * x_p) / (2 * sigma * sigma));
 139 | }
 140 | 
 141 | __device__ float ComputeDepthfromPlaneHypothesis(const Camera camera, const float4 plane_hypothesis, const int2 p)
 142 | {
 143 |     return -plane_hypothesis.w * camera.K[0] / ((p.x - camera.K[2]) * plane_hypothesis.x + (camera.K[0] / camera.K[4]) * (p.y - camera.K[5]) * plane_hypothesis.y + camera.K[0] * plane_hypothesis.z);
 144 | }
 145 | 
 146 | __device__ float4 GenerateRandomNormal(const Camera camera, const int2 p, curandState *rand_state, const float depth)
 147 | {
 148 |     float4 normal;
 149 |     float q1 = 1.0f;
 150 |     float q2 = 1.0f;
 151 |     float s = 2.0f;
 152 |     while (s >= 1.0f) {
 153 |         q1 = 2.0f * curand_uniform(rand_state) -1.0f;
 154 |         q2 = 2.0f * curand_uniform(rand_state) - 1.0f;
 155 |         s = q1 * q1 + q2 * q2;
 156 |     }
 157 |     const float sq = sqrt(1.0f - s);
 158 |     normal.x = 2.0f * q1 * sq;
 159 |     normal.y = 2.0f * q2 * sq;
 160 |     normal.z = 1.0f - 2.0f * s;
 161 |     normal.w = 0;
 162 | 
 163 |     float4 view_direction = GetViewDirection(camera, p, depth);
 164 |     float dot_product = normal.x * view_direction.x + normal.y * view_direction.y + normal.z * view_direction.z;
 165 |     if (dot_product > 0.0f) {
 166 |         normal.x = -normal.x;
 167 |         normal.y = -normal.y;
 168 |         normal.z = - normal.z;
 169 |     }
 170 |     NormalizeVec3(&normal);
 171 |     return normal;
 172 | }
 173 | 
 174 | __device__ float4 GeneratePerturbedNormal(const Camera camera, const int2 p, const float4 normal, curandState *rand_state, const float perturbation)
 175 | {
 176 |     float4 view_direction = GetViewDirection(camera, p, 1.0f);
 177 | 
 178 |     const float a1 = (curand_uniform(rand_state) - 0.5f) * perturbation;
 179 |     const float a2 = (curand_uniform(rand_state) - 0.5f) * perturbation;
 180 |     const float a3 = (curand_uniform(rand_state) - 0.5f) * perturbation;
 181 | 
 182 |     const float sin_a1 = sin(a1);
 183 |     const float sin_a2 = sin(a2);
 184 |     const float sin_a3 = sin(a3);
 185 |     const float cos_a1 = cos(a1);
 186 |     const float cos_a2 = cos(a2);
 187 |     const float cos_a3 = cos(a3);
 188 | 
 189 |     float R[9];
 190 |     R[0] = cos_a2 * cos_a3;
 191 |     R[1] = cos_a3 * sin_a1 * sin_a2 - cos_a1 * sin_a3;
 192 |     R[2] = sin_a1 * sin_a3 + cos_a1 * cos_a3 * sin_a2;
 193 |     R[3] = cos_a2 * sin_a3;
 194 |     R[4] = cos_a1 * cos_a3 + sin_a1 * sin_a2 * sin_a3;
 195 |     R[5] = cos_a1 * sin_a2 * sin_a3 - cos_a3 * sin_a1;
 196 |     R[6] = -sin_a2;
 197 |     R[7] = cos_a2 * sin_a1;
 198 |     R[8] = cos_a1 * cos_a2;
 199 | 
 200 |     float4 normal_perturbed;
 201 |     Mat33DotVec3(R, normal, &normal_perturbed);
 202 | 
 203 |     if (Vec3DotVec3(normal_perturbed, view_direction) >= 0.0f) {
 204 |         normal_perturbed = normal;
 205 |     }
 206 | 
 207 |     NormalizeVec3(&normal_perturbed);
 208 |     return normal_perturbed;
 209 | }
 210 | 
 211 | __device__ float4 GenerateRandomPlaneHypothesis(const Camera camera, const int2 p, curandState *rand_state, const float depth_min, const float depth_max)
 212 | {
 213 |     float depth = curand_uniform(rand_state) * (depth_max - depth_min) + depth_min;
 214 |     float4 plane_hypothesis = GenerateRandomNormal(camera, p, rand_state, depth);
 215 |     plane_hypothesis.w = GetDistance2Origin(camera, p, depth, plane_hypothesis);
 216 |     return plane_hypothesis;
 217 | }
 218 | 
 219 | __device__ float4 GeneratePertubedPlaneHypothesis(const Camera camera, const int2 p, curandState *rand_state, const float perturbation, const float4 plane_hypothesis_now, const float depth_now, const float depth_min, const float depth_max)
 220 | {
 221 |     float depth_perturbed = depth_now;
 222 | 
 223 |     float dist_perturbed = plane_hypothesis_now.w;
 224 |     const float dist_min_perturbed = (1 - perturbation) * dist_perturbed;
 225 |     const float dist_max_perturbed = (1 + perturbation) * dist_perturbed;
 226 |     float4 plane_hypothesis_temp = plane_hypothesis_now;
 227 |     do {
 228 |         dist_perturbed = curand_uniform(rand_state) * (dist_max_perturbed - dist_min_perturbed) + dist_min_perturbed;
 229 |         plane_hypothesis_temp.w = dist_perturbed;
 230 |         depth_perturbed = ComputeDepthfromPlaneHypothesis(camera, plane_hypothesis_temp, p);
 231 |     } while (depth_perturbed < depth_min && depth_perturbed > depth_max);
 232 | 
 233 |     float4 plane_hypothesis = GeneratePerturbedNormal(camera, p, plane_hypothesis_now, rand_state, perturbation * M_PI);
 234 |     plane_hypothesis.w = dist_perturbed;
 235 |     return plane_hypothesis;
 236 | }
 237 | 
 238 | __device__ void ComputeHomography(const Camera ref_camera, const Camera src_camera, const float4 plane_hypothesis, float *H)
 239 | {
 240 |     float ref_C[3];
 241 |     float src_C[3];
 242 |     ref_C[0] = -(ref_camera.R[0] * ref_camera.t[0] + ref_camera.R[3] * ref_camera.t[1] + ref_camera.R[6] * ref_camera.t[2]);
 243 |     ref_C[1] = -(ref_camera.R[1] * ref_camera.t[0] + ref_camera.R[4] * ref_camera.t[1] + ref_camera.R[7] * ref_camera.t[2]);
 244 |     ref_C[2] = -(ref_camera.R[2] * ref_camera.t[0] + ref_camera.R[5] * ref_camera.t[1] + ref_camera.R[8] * ref_camera.t[2]);
 245 |     src_C[0] = -(src_camera.R[0] * src_camera.t[0] + src_camera.R[3] * src_camera.t[1] + src_camera.R[6] * src_camera.t[2]);
 246 |     src_C[1] = -(src_camera.R[1] * src_camera.t[0] + src_camera.R[4] * src_camera.t[1] + src_camera.R[7] * src_camera.t[2]);
 247 |     src_C[2] = -(src_camera.R[2] * src_camera.t[0] + src_camera.R[5] * src_camera.t[1] + src_camera.R[8] * src_camera.t[2]);
 248 | 
 249 |     float R_relative[9];
 250 |     float C_relative[3];
 251 |     float t_relative[3];
 252 |     R_relative[0] = src_camera.R[0] * ref_camera.R[0] + src_camera.R[1] * ref_camera.R[1] + src_camera.R[2] *ref_camera.R[2];
 253 |     R_relative[1] = src_camera.R[0] * ref_camera.R[3] + src_camera.R[1] * ref_camera.R[4] + src_camera.R[2] *ref_camera.R[5];
 254 |     R_relative[2] = src_camera.R[0] * ref_camera.R[6] + src_camera.R[1] * ref_camera.R[7] + src_camera.R[2] *ref_camera.R[8];
 255 |     R_relative[3] = src_camera.R[3] * ref_camera.R[0] + src_camera.R[4] * ref_camera.R[1] + src_camera.R[5] *ref_camera.R[2];
 256 |     R_relative[4] = src_camera.R[3] * ref_camera.R[3] + src_camera.R[4] * ref_camera.R[4] + src_camera.R[5] *ref_camera.R[5];
 257 |     R_relative[5] = src_camera.R[3] * ref_camera.R[6] + src_camera.R[4] * ref_camera.R[7] + src_camera.R[5] *ref_camera.R[8];
 258 |     R_relative[6] = src_camera.R[6] * ref_camera.R[0] + src_camera.R[7] * ref_camera.R[1] + src_camera.R[8] *ref_camera.R[2];
 259 |     R_relative[7] = src_camera.R[6] * ref_camera.R[3] + src_camera.R[7] * ref_camera.R[4] + src_camera.R[8] *ref_camera.R[5];
 260 |     R_relative[8] = src_camera.R[6] * ref_camera.R[6] + src_camera.R[7] * ref_camera.R[7] + src_camera.R[8] *ref_camera.R[8];
 261 |     C_relative[0] = (ref_C[0] - src_C[0]);
 262 |     C_relative[1] = (ref_C[1] - src_C[1]);
 263 |     C_relative[2] = (ref_C[2] - src_C[2]);
 264 |     t_relative[0] = src_camera.R[0] * C_relative[0] + src_camera.R[1] * C_relative[1] + src_camera.R[2] * C_relative[2];
 265 |     t_relative[1] = src_camera.R[3] * C_relative[0] + src_camera.R[4] * C_relative[1] + src_camera.R[5] * C_relative[2];
 266 |     t_relative[2] = src_camera.R[6] * C_relative[0] + src_camera.R[7] * C_relative[1] + src_camera.R[8] * C_relative[2];
 267 | 
 268 |     H[0] = R_relative[0] - t_relative[0] * plane_hypothesis.x / plane_hypothesis.w;
 269 |     H[1] = R_relative[1] - t_relative[0] * plane_hypothesis.y / plane_hypothesis.w;
 270 |     H[2] = R_relative[2] - t_relative[0] * plane_hypothesis.z / plane_hypothesis.w;
 271 |     H[3] = R_relative[3] - t_relative[1] * plane_hypothesis.x / plane_hypothesis.w;
 272 |     H[4] = R_relative[4] - t_relative[1] * plane_hypothesis.y / plane_hypothesis.w;
 273 |     H[5] = R_relative[5] - t_relative[1] * plane_hypothesis.z / plane_hypothesis.w;
 274 |     H[6] = R_relative[6] - t_relative[2] * plane_hypothesis.x / plane_hypothesis.w;
 275 |     H[7] = R_relative[7] - t_relative[2] * plane_hypothesis.y / plane_hypothesis.w;
 276 |     H[8] = R_relative[8] - t_relative[2] * plane_hypothesis.z / plane_hypothesis.w;
 277 | 
 278 |     float tmp[9];
 279 |     tmp[0] = H[0] / ref_camera.K[0];
 280 |     tmp[1] = H[1] / ref_camera.K[4];
 281 |     tmp[2] = -H[0] * ref_camera.K[2] / ref_camera.K[0] - H[1] * ref_camera.K[5] / ref_camera.K[4] + H[2];
 282 |     tmp[3] = H[3] / ref_camera.K[0];
 283 |     tmp[4] = H[4] / ref_camera.K[4];
 284 |     tmp[5] = -H[3] * ref_camera.K[2] / ref_camera.K[0] - H[4] * ref_camera.K[5] / ref_camera.K[4] + H[5];
 285 |     tmp[6] = H[6] / ref_camera.K[0];
 286 |     tmp[7] = H[7] / ref_camera.K[4];
 287 |     tmp[8] = -H[6] * ref_camera.K[2] / ref_camera.K[0] - H[7] * ref_camera.K[5] / ref_camera.K[4] + H[8];
 288 | 
 289 |     H[0] = src_camera.K[0] * tmp[0] + src_camera.K[2] * tmp[6];
 290 |     H[1] = src_camera.K[0] * tmp[1] + src_camera.K[2] * tmp[7];
 291 |     H[2] = src_camera.K[0] * tmp[2] + src_camera.K[2] * tmp[8];
 292 |     H[3] = src_camera.K[4] * tmp[3] + src_camera.K[5] * tmp[6];
 293 |     H[4] = src_camera.K[4] * tmp[4] + src_camera.K[5] * tmp[7];
 294 |     H[5] = src_camera.K[4] * tmp[5] + src_camera.K[5] * tmp[8];
 295 |     H[6] = src_camera.K[8] * tmp[6];
 296 |     H[7] = src_camera.K[8] * tmp[7];
 297 |     H[8] = src_camera.K[8] * tmp[8];
 298 | }
 299 | 
 300 | __device__ float2 ComputeCorrespondingPoint(const float *H, const int2 p)
 301 | {
 302 |     float3 pt;
 303 |     pt.x = H[0] * p.x + H[1] * p.y + H[2];
 304 |     pt.y = H[3] * p.x + H[4] * p.y + H[5];
 305 |     pt.z = H[6] * p.x + H[7] * p.y + H[8];
 306 |     return make_float2(pt.x / pt.z, pt.y / pt.z);
 307 | }
 308 | 
 309 | __device__ float4 TransformNormal(const Camera camera, float4 plane_hypothesis)
 310 | {
 311 |     float4 transformed_normal;
 312 |     transformed_normal.x = camera.R[0] * plane_hypothesis.x + camera.R[3] * plane_hypothesis.y + camera.R[6] * plane_hypothesis.z;
 313 |     transformed_normal.y = camera.R[1] * plane_hypothesis.x + camera.R[4] * plane_hypothesis.y + camera.R[7] * plane_hypothesis.z;
 314 |     transformed_normal.z = camera.R[2] * plane_hypothesis.x + camera.R[5] * plane_hypothesis.y + camera.R[8] * plane_hypothesis.z;
 315 |     transformed_normal.w = plane_hypothesis.w;
 316 |     return transformed_normal;
 317 | }
 318 | 
 319 | __device__ float4 TransformNormal2RefCam(const Camera camera, float4 plane_hypothesis)
 320 | {
 321 |     float4 transformed_normal;
 322 |     transformed_normal.x = camera.R[0] * plane_hypothesis.x + camera.R[1] * plane_hypothesis.y + camera.R[2] * plane_hypothesis.z;
 323 |     transformed_normal.y = camera.R[3] * plane_hypothesis.x + camera.R[4] * plane_hypothesis.y + camera.R[5] * plane_hypothesis.z;
 324 |     transformed_normal.z = camera.R[6] * plane_hypothesis.x + camera.R[7] * plane_hypothesis.y + camera.R[8] * plane_hypothesis.z;
 325 |     transformed_normal.w = plane_hypothesis.w;
 326 |     return transformed_normal;
 327 | }
 328 | 
 329 | __device__ float ComputeBilateralWeight(const float x_dist, const float y_dist, const float pix, const float center_pix, const float sigma_spatial, const float sigma_color)
 330 | {
 331 |     const float spatial_dist = sqrt(x_dist * x_dist + y_dist * y_dist);
 332 |     const float color_dist = fabs(pix - center_pix);
 333 |     return exp(-spatial_dist / (2.0f * sigma_spatial* sigma_spatial) - color_dist / (2.0f * sigma_color * sigma_color));
 334 | }
 335 | 
 336 | __device__ float ComputeBilateralNCC(const cudaTextureObject_t ref_image, const Camera ref_camera, const cudaTextureObject_t src_image, const Camera src_camera, const int2 p, const float4 plane_hypothesis, const PatchMatchParams params)
 337 | {
 338 |     const float cost_max = 2.0f;
 339 |     int radius = params.patch_size / 2;
 340 | 
 341 |     float H[9];
 342 |     ComputeHomography(ref_camera, src_camera, plane_hypothesis, H);
 343 |     float2 pt = ComputeCorrespondingPoint(H, p);
 344 |     if (pt.x >= src_camera.width || pt.x < 0.0f || pt.y >= src_camera.height || pt.y < 0.0f) {
 345 |         return cost_max;
 346 |     }
 347 | 
 348 |     float cost = 0.0f;
 349 |     {
 350 |         float sum_ref = 0.0f;
 351 |         float sum_ref_ref = 0.0f;
 352 |         float sum_src = 0.0f;
 353 |         float sum_src_src = 0.0f;
 354 |         float sum_ref_src = 0.0f;
 355 |         float bilateral_weight_sum = 0.0f;
 356 |         const float ref_center_pix = tex2D<float>(ref_image, p.x + 0.5f, p.y + 0.5f);
 357 | 
 358 |         for (int i = -radius; i < radius + 1; i += params.radius_increment) {
 359 |             float sum_ref_row = 0.0f;
 360 |             float sum_src_row = 0.0f;
 361 |             float sum_ref_ref_row = 0.0f;
 362 |             float sum_src_src_row = 0.0f;
 363 |             float sum_ref_src_row = 0.0f;
 364 |             float bilateral_weight_sum_row = 0.0f;
 365 | 
 366 |             for (int j = -radius; j < radius + 1; j += params.radius_increment) {
 367 |                 const int2 ref_pt = make_int2(p.x + i, p.y + j);
 368 |                 const float ref_pix = tex2D<float>(ref_image, ref_pt.x + 0.5f, ref_pt.y + 0.5f);
 369 |                 float2 src_pt = ComputeCorrespondingPoint(H, ref_pt);
 370 |                 const float src_pix = tex2D<float>(src_image, src_pt.x + 0.5f, src_pt.y + 0.5f);
 371 | 
 372 |                 float weight = ComputeBilateralWeight(i, j, ref_pix, ref_center_pix, params.sigma_spatial, params.sigma_color);
 373 | 
 374 |                 sum_ref_row += weight * ref_pix;
 375 |                 sum_ref_ref_row += weight * ref_pix * ref_pix;
 376 |                 sum_src_row += weight * src_pix;
 377 |                 sum_src_src_row += weight * src_pix * src_pix;
 378 |                 sum_ref_src_row += weight * ref_pix * src_pix;
 379 |                 bilateral_weight_sum_row += weight;
 380 |             }
 381 | 
 382 |             sum_ref += sum_ref_row;
 383 |             sum_ref_ref += sum_ref_ref_row;
 384 |             sum_src += sum_src_row;
 385 |             sum_src_src += sum_src_src_row;
 386 |             sum_ref_src += sum_ref_src_row;
 387 |             bilateral_weight_sum += bilateral_weight_sum_row;
 388 |         }
 389 |         const float inv_bilateral_weight_sum = 1.0f / bilateral_weight_sum;
 390 |         sum_ref *= inv_bilateral_weight_sum;
 391 |         sum_ref_ref *= inv_bilateral_weight_sum;
 392 |         sum_src *= inv_bilateral_weight_sum;
 393 |         sum_src_src *= inv_bilateral_weight_sum;
 394 |         sum_ref_src *= inv_bilateral_weight_sum;
 395 | 
 396 |         const float var_ref = sum_ref_ref - sum_ref * sum_ref;
 397 |         const float var_src = sum_src_src - sum_src * sum_src;
 398 | 
 399 |         const float kMinVar = 1e-5f;
 400 |         if (var_ref < kMinVar || var_src < kMinVar) {
 401 |             return cost = cost_max;
 402 |         } else {
 403 |             const float covar_src_ref = sum_ref_src - sum_ref * sum_src;
 404 |             const float var_ref_src = sqrt(var_ref * var_src);
 405 |             return cost = max(0.0f, min(cost_max, 1.0f - covar_src_ref / var_ref_src));
 406 |         }
 407 |     }
 408 | }
 409 | 
 410 | __device__ float ComputeMultiViewInitialCostandSelectedViews(const cudaTextureObject_t *images, const Camera *cameras, const int2 p, const float4 plane_hypothesis, unsigned int *selected_views, const PatchMatchParams params)
 411 | {
 412 |     float cost_max = 2.0f;
 413 |     float cost_vector[32] = {2.0f};
 414 |     float cost_vector_copy[32] = {2.0f};
 415 |     int cost_count = 0;
 416 |     int num_valid_views = 0;
 417 | 
 418 |     for (int i = 1; i < params.num_images; ++i) {
 419 |         float c = ComputeBilateralNCC(images[0], cameras[0], images[i], cameras[i], p, plane_hypothesis, params);
 420 |         cost_vector[i - 1] = c;
 421 |         cost_vector_copy[i - 1] = c;
 422 |         cost_count++;
 423 |         if (c < cost_max) {
 424 |             num_valid_views++;
 425 |         }
 426 |     }
 427 | 
 428 |     sort_small(cost_vector, cost_count);
 429 |     *selected_views = 0;
 430 | 
 431 |     int top_k = min(num_valid_views, params.top_k);
 432 |     if (top_k > 0) {
 433 |         float cost = 0.0f;
 434 |         for (int i = 0; i < top_k; ++i) {
 435 |             cost += cost_vector[i];
 436 |         }
 437 |         float cost_threshold = cost_vector[top_k - 1];
 438 |         for (int i = 0; i < params.num_images - 1; ++i) {
 439 |             if (cost_vector_copy[i] <= cost_threshold) {
 440 |                 setBit(*selected_views, i);
 441 |             }
 442 |         }
 443 |         return cost / top_k;
 444 |     } else {
 445 |         return cost_max;
 446 |     }
 447 | }
 448 | 
 449 | __device__ void ComputeMultiViewCostVector(const cudaTextureObject_t *images, const Camera *cameras, const int2 p, const float4 plane_hypothesis, float *cost_vector, const PatchMatchParams params)
 450 | {
 451 |     for (int i = 1; i < params.num_images; ++i) {
 452 |         cost_vector[i - 1] = ComputeBilateralNCC(images[0], cameras[0], images[i], cameras[i], p, plane_hypothesis, params);
 453 |     }
 454 | }
 455 | 
 456 | __device__ float3 Get3DPointonWorld_cu(const float x, const float y, const float depth, const Camera camera)
 457 | {
 458 |     float3 pointX;
 459 |     float3 tmpX;
 460 |     // Reprojection
 461 |     pointX.x = depth * (x - camera.K[2]) / camera.K[0];
 462 |     pointX.y = depth * (y - camera.K[5]) / camera.K[4];
 463 |     pointX.z = depth;
 464 | 
 465 |     // Rotation
 466 |     tmpX.x = camera.R[0] * pointX.x + camera.R[3] * pointX.y + camera.R[6] * pointX.z;
 467 |     tmpX.y = camera.R[1] * pointX.x + camera.R[4] * pointX.y + camera.R[7] * pointX.z;
 468 |     tmpX.z = camera.R[2] * pointX.x + camera.R[5] * pointX.y + camera.R[8] * pointX.z;
 469 | 
 470 |     // Transformation
 471 |     float3 C;
 472 |     C.x = -(camera.R[0] * camera.t[0] + camera.R[3] * camera.t[1] + camera.R[6] * camera.t[2]);
 473 |     C.y = -(camera.R[1] * camera.t[0] + camera.R[4] * camera.t[1] + camera.R[7] * camera.t[2]);
 474 |     C.z = -(camera.R[2] * camera.t[0] + camera.R[5] * camera.t[1] + camera.R[8] * camera.t[2]);
 475 |     pointX.x = tmpX.x + C.x;
 476 |     pointX.y = tmpX.y + C.y;
 477 |     pointX.z = tmpX.z + C.z;
 478 | 
 479 |     return pointX;
 480 | }
 481 | 
 482 | __device__ void ProjectonCamera_cu(const float3 PointX, const Camera camera, float2 &point, float &depth)
 483 | {
 484 |     float3 tmp;
 485 |     tmp.x = camera.R[0] * PointX.x + camera.R[1] * PointX.y + camera.R[2] * PointX.z + camera.t[0];
 486 |     tmp.y = camera.R[3] * PointX.x + camera.R[4] * PointX.y + camera.R[5] * PointX.z + camera.t[1];
 487 |     tmp.z = camera.R[6] * PointX.x + camera.R[7] * PointX.y + camera.R[8] * PointX.z + camera.t[2];
 488 | 
 489 |     depth = camera.K[6] * tmp.x + camera.K[7] * tmp.y + camera.K[8] * tmp.z;
 490 |     point.x = (camera.K[0] * tmp.x + camera.K[1] * tmp.y + camera.K[2] * tmp.z) / depth;
 491 |     point.y = (camera.K[3] * tmp.x + camera.K[4] * tmp.y + camera.K[5] * tmp.z) / depth;
 492 | }
 493 | 
 494 | __device__ float ComputeGeomConsistencyCost(const cudaTextureObject_t depth_image, const Camera ref_camera, const Camera src_camera, const float4 plane_hypothesis, const int2 p)
 495 | {
 496 |     const float max_cost = 3.0f;
 497 | 
 498 |     float depth = ComputeDepthfromPlaneHypothesis(ref_camera, plane_hypothesis, p);
 499 |     float3 forward_point = Get3DPointonWorld_cu(p.x, p.y, depth, ref_camera);
 500 | 
 501 |     float2 src_pt;
 502 |     float src_d;
 503 |     ProjectonCamera_cu(forward_point, src_camera, src_pt, src_d);
 504 |     const float src_depth = tex2D<float>(depth_image,  (int)src_pt.x + 0.5f, (int)src_pt.y + 0.5f);
 505 | 
 506 |     if (src_depth == 0.0f) {
 507 |         return max_cost;
 508 |     }
 509 | 
 510 |     float3 src_3D_pt = Get3DPointonWorld_cu(src_pt.x, src_pt.y, src_depth, src_camera);
 511 | 
 512 |     float2 backward_point;
 513 |     float ref_d;
 514 |     ProjectonCamera_cu(src_3D_pt, ref_camera, backward_point, ref_d);
 515 | 
 516 |     const float diff_col = p.x - backward_point.x;
 517 |     const float diff_row = p.y - backward_point.y;
 518 |     return min(max_cost, sqrt(diff_col * diff_col + diff_row * diff_row));
 519 | }
 520 | 
 521 | __global__ void RandomInitialization(cudaTextureObjects *texture_objects, Camera *cameras, float4 *plane_hypotheses,  float4 *scaled_plane_hypotheses, float *costs,  float *pre_costs,  curandState *rand_states, unsigned int *selected_views, const PatchMatchParams params)
 522 | {
 523 |     const int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
 524 |     int width = cameras[0].width;
 525 |     int height = cameras[0].height;
 526 | 
 527 |     if (p.x >= width || p.y >= height) {
 528 |         return;
 529 |     }
 530 | 
 531 |     const int center = p.y * width + p.x;
 532 |     curand_init(clock64(), p.y, p.x, &rand_states[center]);
 533 | 
 534 |     if (!params.geom_consistency && !params.hierarchy ) {
 535 |         plane_hypotheses[center] = GenerateRandomPlaneHypothesis(cameras[0], p, &rand_states[center], params.depth_min, params.depth_max);
 536 |         costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
 537 |     }
 538 |     else {
 539 |         if(params.upsample) {
 540 |             const float scale = 1.0 * params.scaled_cols / width;
 541 |             const float sigmad = 0.50;
 542 |             const float sigmar = 25.5;
 543 |             const int Imagescale = max(width / params.scaled_cols , height / params.scaled_rows);
 544 |             const int WinWidth =Imagescale * Imagescale + 1;
 545 |             int num_neighbors = WinWidth / 2;
 546 | 
 547 |             const float o_y = p.y * scale;
 548 |             const float o_x = p.x * scale;
 549 |             const float refPix = tex2D<float>(texture_objects[0].images[0], p.x + 0.5f, p.y + 0.5f);
 550 |             int r_y = 0;
 551 |             int r_ys = 0;
 552 |             int r_x = 0;
 553 |             int r_xs = 0;
 554 |             float sgauss = 0.0, rgauss = 0.0, totalgauss = 0.0;
 555 |             float c_total_val = 0.0, normalizing_factor = 0.0;
 556 |             float  srcPix = 0, neighborPix = 0;
 557 |             float4 srcNorm;
 558 |             float4 n_total_val;
 559 |             n_total_val.x = 0; n_total_val.y = 0; n_total_val.z = 0; n_total_val.w = 0;
 560 |      \
 561 |             for (int j = -num_neighbors; j <= num_neighbors; ++j) {
 562 |                 // source
 563 |                 r_y = o_y + j;
 564 |                 r_y = (r_y > 0 ? (r_y < params.scaled_rows ? r_y : params.scaled_rows - 1) : 0) ;
 565 |                 // reference
 566 |                 r_ys = p.y + j;
 567 |                 for (int i = -num_neighbors; i <= num_neighbors; ++i) {
 568 |                     // source
 569 |                     r_x = o_x + i;
 570 |                     r_x = (r_x > 0 ? (r_x < params.scaled_cols? r_x : params.scaled_cols - 1) : 0);
 571 |                     const int s_center = r_y*params.scaled_cols+r_x;
 572 |                     if (s_center >=  params.scaled_rows * params.scaled_cols) {
 573 |                         printf("Illegal: %d, %d, %f, %f (%d, %d)\n", r_x, r_y, o_x, o_y, params.scaled_cols,  params.scaled_rows);
 574 |                     }
 575 |                     srcPix = scaled_plane_hypotheses[s_center].w;
 576 |                     srcNorm = scaled_plane_hypotheses[s_center];
 577 |                     // refIm
 578 |                     r_xs = p.x + i;
 579 |                     neighborPix = tex2D<float>(texture_objects[0].images[0], r_xs + 0.5f, r_ys + 0.5f);
 580 | 
 581 |                     sgauss = SpatialGauss(o_x, o_y, r_x, r_y, sigmad);
 582 |                     rgauss = RangeGauss(fabs(refPix - neighborPix), sigmar);
 583 |                     totalgauss = sgauss * rgauss;
 584 |                     normalizing_factor += totalgauss;
 585 |                     c_total_val += srcPix * totalgauss;
 586 |                     mul4((&srcNorm), totalgauss);
 587 |                     n_total_val.x  = n_total_val.x + srcNorm.x;
 588 |                     n_total_val.y  = n_total_val.y + srcNorm.y;
 589 |                     n_total_val.z  = n_total_val.z + srcNorm.z;
 590 |                 }
 591 |             }
 592 |             costs[center] = c_total_val / normalizing_factor;
 593 |             vecdiv4((&n_total_val), normalizing_factor);
 594 |             NormalizeVec3(&n_total_val);
 595 | 
 596 |              costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
 597 |             pre_costs[center] = costs[center];
 598 | 
 599 |             float4 plane_hypothesis = n_total_val;
 600 |             plane_hypothesis = TransformNormal2RefCam(cameras[0], plane_hypothesis);
 601 |             float depth = plane_hypotheses[center].w;
 602 |             plane_hypothesis.w = GetDistance2Origin(cameras[0], p, depth, plane_hypothesis);
 603 |             plane_hypotheses[center] = plane_hypothesis;
 604 |             costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
 605 |          }
 606 |          else {
 607 |              float4 plane_hypothesis;
 608 |              if (params.hierarchy) {
 609 |                  plane_hypothesis = scaled_plane_hypotheses[center];
 610 |              }
 611 |              else {
 612 |                  plane_hypothesis = plane_hypotheses[center];
 613 |              }
 614 |              plane_hypothesis = TransformNormal2RefCam(cameras[0], plane_hypothesis);
 615 |              float depth = plane_hypothesis.w;
 616 |              plane_hypothesis.w = GetDistance2Origin(cameras[0], p, depth, plane_hypothesis);
 617 |              plane_hypotheses[center] = plane_hypothesis;
 618 |              costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
 619 |          }
 620 |     }
 621 | }
 622 | 
 623 | __device__ void PlaneHypothesisRefinement(const cudaTextureObject_t *images, const cudaTextureObject_t *depth_images, const Camera *cameras, float4 *plane_hypothesis, float *depth, float *cost, curandState *rand_state, const float *view_weights, const float weight_norm, const int2 p, const PatchMatchParams params)
 624 | {
 625 |     float perturbation = 0.02f;
 626 | 
 627 |     float depth_rand = curand_uniform(rand_state) * (params.depth_max - params.depth_min) + params.depth_min;
 628 |     float4 plane_hypothesis_rand = GenerateRandomNormal(cameras[0], p, rand_state, *depth);
 629 |     float depth_perturbed = *depth;
 630 |     const float depth_min_perturbed = (1 - perturbation) * depth_perturbed;
 631 |     const float depth_max_perturbed = (1 + perturbation) * depth_perturbed;
 632 |     do {
 633 |         depth_perturbed = curand_uniform(rand_state) * (depth_max_perturbed - depth_min_perturbed) + depth_min_perturbed;
 634 |     } while (depth_perturbed < params.depth_min && depth_perturbed > params.depth_max);
 635 |     float4 plane_hypothesis_perturbed = GeneratePerturbedNormal(cameras[0], p, *plane_hypothesis, rand_state, perturbation * M_PI);
 636 | 
 637 |     const int num_planes = 5;
 638 |     float depths[num_planes] = {depth_rand, *depth, depth_rand, *depth, depth_perturbed};
 639 |     float4 normals[num_planes] = {*plane_hypothesis, plane_hypothesis_rand, plane_hypothesis_rand, plane_hypothesis_perturbed, *plane_hypothesis};
 640 | 
 641 |     for (int i = 0; i < num_planes; ++i) {
 642 |         float cost_vector[32] = {2.0f};
 643 |         float4 temp_plane_hypothesis = normals[i];
 644 |         temp_plane_hypothesis.w = GetDistance2Origin(cameras[0], p, depths[i], temp_plane_hypothesis);
 645 |         ComputeMultiViewCostVector(images, cameras, p, temp_plane_hypothesis, cost_vector, params);
 646 | 
 647 |         float temp_cost = 0.0f;
 648 |         for (int j = 0; j < params.num_images - 1; ++j) {
 649 |             if (view_weights[j] > 0) {
 650 |                 if (params.geom_consistency) {
 651 |                     temp_cost += view_weights[j] * (cost_vector[j] + 0.2f * ComputeGeomConsistencyCost(depth_images[j+1], cameras[0], cameras[j+1], temp_plane_hypothesis, p));
 652 |                 }
 653 |                 else {
 654 |                     temp_cost += view_weights[j] * cost_vector[j];
 655 |                 }
 656 |             }
 657 |         }
 658 |         temp_cost /= weight_norm;
 659 | 
 660 |         float depth_before = ComputeDepthfromPlaneHypothesis(cameras[0], temp_plane_hypothesis, p);
 661 |         if (depth_before >= params.depth_min && depth_before <= params.depth_max && temp_cost < *cost) {
 662 |             *depth = depth_before;
 663 |             *plane_hypothesis = temp_plane_hypothesis;
 664 |             *cost = temp_cost;
 665 |         }
 666 |     }
 667 | }
 668 | 
 669 | __device__ void CheckerboardPropagation(const cudaTextureObject_t *images, const cudaTextureObject_t *depths, const Camera *cameras, float4 *plane_hypotheses, float *costs, float *pre_costs, curandState *rand_states, unsigned int *selected_views, const int2 p, const PatchMatchParams params, const int iter)
 670 | {
 671 |     int width = cameras[0].width;
 672 |     int height = cameras[0].height;
 673 |     if (p.x >= width || p.y >= height) {
 674 |         return;
 675 |     }
 676 | 
 677 |     const int center = p.y * width + p.x;
 678 |     int left_near = center - 1;
 679 |     int left_far = center - 3;
 680 |     int right_near = center + 1;
 681 |     int right_far = center + 3;
 682 |     int up_near = center - width;
 683 |     int up_far = center - 3 * width;
 684 |     int down_near = center + width;
 685 |     int down_far = center + 3 * width;
 686 | 
 687 |     // Adaptive Checkerboard Sampling
 688 |     float cost_array[8][32] = {2.0f};
 689 |     // 0 -- up_near, 1 -- up_far, 2 -- down_near, 3 -- down_far, 4 -- left_near, 5 -- left_far, 6 -- right_near, 7 -- right_far
 690 |     bool flag[8] = {false};
 691 |     int num_valid_pixels = 0;
 692 | 
 693 |     float costMin;
 694 |     int costMinPoint;
 695 | 
 696 |     // up_far
 697 |     if (p.y > 2) {
 698 |         flag[1] = true;
 699 |         num_valid_pixels++;
 700 |         costMin = costs[up_far];
 701 |         costMinPoint = up_far;
 702 |         for (int i = 1; i < 11; ++i) {
 703 |             if (p.y > 2 + 2 * i) {
 704 |                 int pointTemp = up_far - 2 * i * width;
 705 |                 if (costs[pointTemp] < costMin) {
 706 |                     costMin = costs[pointTemp];
 707 |                     costMinPoint = pointTemp;
 708 |                 }
 709 |             }
 710 |         }
 711 |         up_far = costMinPoint;
 712 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[up_far], cost_array[1], params);
 713 |     }
 714 | 
 715 |     // dwon_far
 716 |     if (p.y < height - 3) {
 717 |         flag[3] = true;
 718 |         num_valid_pixels++;
 719 |         costMin = costs[down_far];
 720 |         costMinPoint = down_far;
 721 |         for (int i = 1; i < 11; ++i) {
 722 |             if (p.y < height - 3 - 2 * i) {
 723 |                 int pointTemp = down_far + 2 * i * width;
 724 |                 if (costs[pointTemp] < costMin) {
 725 |                     costMin = costs[pointTemp];
 726 |                     costMinPoint = pointTemp;
 727 |                 }
 728 |             }
 729 |         }
 730 |         down_far = costMinPoint;
 731 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[down_far], cost_array[3], params);
 732 |     }
 733 | 
 734 |     // left_far
 735 |     if (p.x > 2) {
 736 |         flag[5] = true;
 737 |         num_valid_pixels++;
 738 |         costMin = costs[left_far];
 739 |         costMinPoint = left_far;
 740 |         for (int i = 1; i < 11; ++i) {
 741 |             if (p.x > 2 + 2 * i) {
 742 |                 int pointTemp = left_far - 2 * i;
 743 |                 if (costs[pointTemp] < costMin) {
 744 |                     costMin = costs[pointTemp];
 745 |                     costMinPoint = pointTemp;
 746 |                 }
 747 |             }
 748 |         }
 749 |         left_far = costMinPoint;
 750 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_far], cost_array[5], params);
 751 |     }
 752 | 
 753 |     // right_far
 754 |     if (p.x < width - 3) {
 755 |         flag[7] = true;
 756 |         num_valid_pixels++;
 757 |         costMin = costs[right_far];
 758 |         costMinPoint = right_far;
 759 |         for (int i = 1; i < 11; ++i) {
 760 |             if (p.x < width - 3 - 2 * i) {
 761 |                 int pointTemp = right_far + 2 * i;
 762 |                 if (costMin < costs[pointTemp]) {
 763 |                     costMin = costs[pointTemp];
 764 |                     costMinPoint = pointTemp;
 765 |                 }
 766 |             }
 767 |         }
 768 |         right_far = costMinPoint;
 769 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_far], cost_array[7], params);
 770 |     }
 771 | 
 772 |     // up_near
 773 |     if (p.y > 0) {
 774 |         flag[0] = true;
 775 |         num_valid_pixels++;
 776 |         costMin = costs[up_near];
 777 |         costMinPoint = up_near;
 778 |         for (int i = 0; i < 3; ++i) {
 779 |             if (p.y > 1 + i && p.x > i) {
 780 |                 int pointTemp = up_near - (1 + i) * width - i;
 781 |                 if (costs[pointTemp] < costMin) {
 782 |                     costMin = costs[pointTemp];
 783 |                     costMinPoint = pointTemp;
 784 |                 }
 785 |             }
 786 |             if (p.y > 1 + i && p.x < width - 1 - i) {
 787 |                 int pointTemp = up_near - (1 + i) * width + i;
 788 |                 if (costs[pointTemp] < costMin) {
 789 |                     costMin = costs[pointTemp];
 790 |                     costMinPoint = pointTemp;
 791 |                 }
 792 |             }
 793 |         }
 794 |         up_near = costMinPoint;
 795 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[up_near], cost_array[0], params);
 796 |     }
 797 | 
 798 |     // down_near
 799 |     if (p.y < height - 1) {
 800 |         flag[2] = true;
 801 |         num_valid_pixels++;
 802 |         costMin = costs[down_near];
 803 |         costMinPoint = down_near;
 804 |         for (int i = 0; i < 3; ++i) {
 805 |             if (p.y < height - 2 - i && p.x > i) {
 806 |                 int pointTemp = down_near + (1 + i) * width - i;
 807 |                 if (costs[pointTemp] < costMin) {
 808 |                     costMin = costs[pointTemp];
 809 |                     costMinPoint = pointTemp;
 810 |                 }
 811 |             }
 812 |             if (p.y < height - 2 - i && p.x < width - 1 - i) {
 813 |                 int pointTemp = down_near + (1 + i) * width + i;
 814 |                 if (costs[pointTemp] < costMin) {
 815 |                     costMin = costs[pointTemp];
 816 |                     costMinPoint = pointTemp;
 817 |                 }
 818 |             }
 819 |         }
 820 |         down_near = costMinPoint;
 821 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[down_near], cost_array[2], params);
 822 |     }
 823 | 
 824 |     // left_near
 825 |     if (p.x > 0) {
 826 |         flag[4] = true;
 827 |         num_valid_pixels++;
 828 |         costMin = costs[left_near];
 829 |         costMinPoint = left_near;
 830 |         for (int i = 0; i < 3; ++i) {
 831 |             if (p.x > 1 + i && p.y > i) {
 832 |                 int pointTemp = left_near - (1 + i) - i * width;
 833 |                 if (costs[pointTemp] < costMin) {
 834 |                     costMin = costs[pointTemp];
 835 |                     costMinPoint = pointTemp;
 836 |                 }
 837 |             }
 838 |             if (p.x > 1 + i && p.y < height - 1 - i) {
 839 |                 int pointTemp = left_near - (1 + i) + i * width;
 840 |                 if (costs[pointTemp] < costMin) {
 841 |                     costMin = costs[pointTemp];
 842 |                     costMinPoint = pointTemp;
 843 |                 }
 844 |             }
 845 |         }
 846 |         left_near = costMinPoint;
 847 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_near], cost_array[4], params);
 848 |     }
 849 | 
 850 |     // right_near
 851 |     if (p.x < width - 1) {
 852 |         flag[6] = true;
 853 |         num_valid_pixels++;
 854 |         costMin = costs[right_near];
 855 |         costMinPoint = right_near;
 856 |         for (int i = 0; i < 3; ++i) {
 857 |             if (p.x < width - 2 - i && p.y > i) {
 858 |                 int pointTemp = right_near + (1 + i) - i * width;
 859 |                 if (costs[pointTemp] < costMin) {
 860 |                     costMin = costs[pointTemp];
 861 |                     costMinPoint = pointTemp;
 862 |                 }
 863 |             }
 864 |             if (p.x < width - 2 - i && p.y < height - 1- i) {
 865 |                 int pointTemp = right_near + (1 + i) + i * width;
 866 |                 if (costs[pointTemp] < costMin) {
 867 |                     costMin = costs[pointTemp];
 868 |                     costMinPoint = pointTemp;
 869 |                 }
 870 |             }
 871 |         }
 872 |         right_near = costMinPoint;
 873 |         ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_near], cost_array[6], params);
 874 |     }
 875 |     const int positions[8] = {up_near, up_far, down_near, down_far, left_near, left_far, right_near, right_far};
 876 | 
 877 |     // Multi-hypothesis Joint View Selection
 878 |     float view_weights[32] = {0.0f};
 879 |     float view_selection_priors[32] = {0.0f};
 880 |     int neighbor_positions[4] = {center - width, center + width, center - 1, center + 1};
 881 |     for (int i = 0; i < 4; ++i) {
 882 |         if (flag[2 * i]) {
 883 |             for (int j = 0; j < params.num_images - 1; ++j) {
 884 |                 if (isSet(selected_views[neighbor_positions[i]], j) == 1) {
 885 |                     view_selection_priors[j] += 0.9f;
 886 |                 } else {
 887 |                     view_selection_priors[j] += 0.1f;
 888 |                 }
 889 |             }
 890 |         }
 891 |     }
 892 | 
 893 |     float sampling_probs[32] = {0.0f};
 894 |     float cost_threshold = 0.8 * expf((iter) * (iter) / (-90.0f));
 895 |     for (int i = 0; i < params.num_images - 1; i++) {
 896 |         float count = 0;
 897 |         int count_false = 0;
 898 |         float tmpw = 0;
 899 |         for (int j = 0; j < 8; j++) {
 900 |             if (cost_array[j][i] < cost_threshold) {
 901 |                 tmpw += expf(cost_array[j][i] * cost_array[j][i] / (-0.18f));
 902 |                 count++;
 903 |             }
 904 |             if (cost_array[j][i] > 1.2f) {
 905 |                 count_false++;
 906 |             }
 907 |         }
 908 |         if (count > 2 && count_false < 3) {
 909 |             sampling_probs[i] = tmpw / count;
 910 |         }
 911 |         else if (count_false < 3) {
 912 |             sampling_probs[i] = expf(cost_threshold * cost_threshold / (-0.32f));
 913 |         }
 914 |         sampling_probs[i] = sampling_probs[i] * view_selection_priors[i];
 915 |     }
 916 | 
 917 |     TransformPDFToCDF(sampling_probs, params.num_images - 1);
 918 |     for (int sample = 0; sample < 15; ++sample) {
 919 |         const float rand_prob = curand_uniform(&rand_states[center]) - FLT_EPSILON;
 920 | 
 921 |         for (int image_id = 0; image_id < params.num_images - 1; ++image_id) {
 922 |             const float prob = sampling_probs[image_id];
 923 |             if (prob > rand_prob) {
 924 |                 view_weights[image_id] += 1.0f;
 925 |                 break;
 926 |             }
 927 |         }
 928 |     }
 929 | 
 930 |     unsigned int temp_selected_views = 0;
 931 |     int num_selected_view = 0;
 932 |     float weight_norm = 0;
 933 |     for (int i = 0; i < params.num_images - 1; ++i) {
 934 |         if (view_weights[i] > 0) {
 935 |             setBit(temp_selected_views, i);
 936 |             weight_norm += view_weights[i];
 937 |             num_selected_view++;
 938 |         }
 939 |     }
 940 | 
 941 |     float final_costs[8] = {0.0f};
 942 |     for (int i = 0; i < 8; ++i) {
 943 |         for (int j = 0; j < params.num_images - 1; ++j) {
 944 |             if (view_weights[j] > 0) {
 945 |                 if (params.geom_consistency) {
 946 |                     if (flag[i]) {
 947 |                         final_costs[i] += view_weights[j] * (cost_array[i][j] + 0.2f * ComputeGeomConsistencyCost(depths[j+1], cameras[0], cameras[j+1], plane_hypotheses[positions[i]], p));
 948 |                     }
 949 |                     else {
 950 |                         final_costs[i] += view_weights[j] * (cost_array[i][j] + 0.1f * 3.0f);
 951 |                     }
 952 |                 }
 953 |                 else {
 954 |                     final_costs[i] += view_weights[j] * cost_array[i][j];
 955 |                 }
 956 |             }
 957 |         }
 958 |         final_costs[i] /= weight_norm;
 959 |     }
 960 | 
 961 |     const int min_cost_idx = FindMinCostIndex(final_costs, 8);
 962 | 
 963 |     float cost_vector_now[32] = {2.0f};
 964 |     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[center], cost_vector_now, params);
 965 |     float cost_now = 0.0f;
 966 |     for (int i = 0; i < params.num_images - 1; ++i) {
 967 |         if (params.geom_consistency) {
 968 |             cost_now += view_weights[i] * (cost_vector_now[i] + 0.2f * ComputeGeomConsistencyCost(depths[i+1], cameras[0], cameras[i+1], plane_hypotheses[center], p));
 969 |         }
 970 |         else {
 971 |             cost_now += view_weights[i] * cost_vector_now[i];
 972 |         }
 973 |     }
 974 |     cost_now /= weight_norm;
 975 |     costs[center] = cost_now;
 976 |     float depth_now = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[center], p);
 977 |     float4 plane_hypotheses_now;
 978 |     if (flag[min_cost_idx]) {
 979 |         float depth_before = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[positions[min_cost_idx]], p);
 980 | 
 981 |         if (depth_before >= params.depth_min && depth_before <= params.depth_max && final_costs[min_cost_idx] < cost_now) {
 982 |             depth_now = depth_before;
 983 |            plane_hypotheses_now = plane_hypotheses[positions[min_cost_idx]];
 984 |            cost_now = final_costs[min_cost_idx];
 985 |            selected_views[center] = temp_selected_views;
 986 |         }
 987 |     }
 988 | 
 989 |     PlaneHypothesisRefinement(images, depths, cameras, &plane_hypotheses_now, &depth_now, &cost_now, &rand_states[center], view_weights, weight_norm, p, params);
 990 | 
 991 |     if (params.hierarchy) {
 992 |         if (cost_now < pre_costs[center] - 0.1f) {
 993 |             costs[center] = cost_now;
 994 |             plane_hypotheses[center] = plane_hypotheses_now;
 995 |         }
 996 |     }
 997 |     else {
 998 |         costs[center] = cost_now;
 999 |         plane_hypotheses[center] = plane_hypotheses_now;
1000 |     }
1001 | }
1002 | 
1003 | __global__ void BlackPixelUpdate(cudaTextureObjects *texture_objects, cudaTextureObjects *texture_depths, Camera *cameras, float4 *plane_hypotheses, float *costs,  float *pre_costs,  curandState *rand_states, unsigned int *selected_views, const PatchMatchParams params, const int iter)
1004 | {
1005 |     int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
1006 |     if (threadIdx.x % 2 == 0) {
1007 |         p.y = p.y * 2;
1008 |     } else {
1009 |         p.y = p.y * 2 + 1;
1010 |     }
1011 | 
1012 |     CheckerboardPropagation(texture_objects[0].images, texture_depths[0].images, cameras, plane_hypotheses, costs, pre_costs,  rand_states, selected_views, p, params, iter);
1013 | }
1014 | 
1015 | __global__ void RedPixelUpdate(cudaTextureObjects *texture_objects, cudaTextureObjects *texture_depths, Camera *cameras, float4 *plane_hypotheses, float *costs,  float *pre_costs, curandState *rand_states, unsigned int *selected_views, const PatchMatchParams params, const int iter)
1016 | {
1017 |     int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
1018 |     if (threadIdx.x % 2 == 0) {
1019 |         p.y = p.y * 2 + 1;
1020 |     } else {
1021 |         p.y = p.y * 2;
1022 |     }
1023 | 
1024 |     CheckerboardPropagation(texture_objects[0].images, texture_depths[0].images, cameras, plane_hypotheses, costs, pre_costs, rand_states, selected_views, p, params, iter);
1025 | }
1026 | 
1027 | __global__ void GetDepthandNormal(Camera *cameras, float4 *plane_hypotheses, const PatchMatchParams params)
1028 | {
1029 |     const int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
1030 |     const int width = cameras[0].width;
1031 |     const int height = cameras[0].height;
1032 | 
1033 |     if (p.x >= width || p.y >= height) {
1034 |         return;
1035 |     }
1036 | 
1037 |     const int center = p.y * width + p.x;
1038 |     plane_hypotheses[center].w = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[center], p);
1039 |     plane_hypotheses[center] = TransformNormal(cameras[0], plane_hypotheses[center]);
1040 | }
1041 | 
1042 | __device__ void CheckerboardFilter(const Camera *cameras, float4 *plane_hypotheses, float *costs, const int2 p)
1043 | {
1044 |     int width = cameras[0].width;
1045 |     int height = cameras[0].height;
1046 |     if (p.x >= width || p.y >= height) {
1047 |         return;
1048 |     }
1049 | 
1050 |     const int center = p.y * width + p.x;
1051 | 
1052 |     float filter[21];
1053 |     int index = 0;
1054 | 
1055 |     filter[index++] = plane_hypotheses[center].w;
1056 | 
1057 |     // Left
1058 |     const int left = center - 1;
1059 |     const int leftleft = center - 3;
1060 | 
1061 |     // Up
1062 |     const int up = center - width;
1063 |     const int upup = center - 3 * width;
1064 | 
1065 |     // Down
1066 |     const int down = center + width;
1067 |     const int downdown = center + 3 * width;
1068 | 
1069 |     // Right
1070 |     const int right = center + 1;
1071 |     const int rightright = center + 3;
1072 | 
1073 |     if (costs[center] < 0.001f) {
1074 |         return;
1075 |     }
1076 | 
1077 |     if (p.y>0) {
1078 |         filter[index++] = plane_hypotheses[up].w;
1079 |     }
1080 |     if (p.y>2) {
1081 |         filter[index++] = plane_hypotheses[upup].w;
1082 |     }
1083 |     if (p.y>4) {
1084 |         filter[index++] = plane_hypotheses[upup-width*2].w;
1085 |     }
1086 |     if (p.y<height-1) {
1087 |         filter[index++] = plane_hypotheses[down].w;
1088 |     }
1089 |     if (p.y<height-3) {
1090 |         filter[index++] = plane_hypotheses[downdown].w;
1091 |     }
1092 |     if (p.y<height-5) {
1093 |         filter[index++] = plane_hypotheses[downdown+width*2].w;
1094 |     }
1095 |     if (p.x>0) {
1096 |         filter[index++] = plane_hypotheses[left].w;
1097 |     }
1098 |     if (p.x>2) {
1099 |         filter[index++] = plane_hypotheses[leftleft].w;
1100 |     }
1101 |     if (p.x>4) {
1102 |         filter[index++] = plane_hypotheses[leftleft-2].w;
1103 |     }
1104 |     if (p.x<width-1) {
1105 |         filter[index++] = plane_hypotheses[right].w;
1106 |     }
1107 |     if (p.x<width-3) {
1108 |         filter[index++] = plane_hypotheses[rightright].w;
1109 |     }
1110 |     if (p.x<width-5) {
1111 |         filter[index++] = plane_hypotheses[rightright+2].w;
1112 |     }
1113 |     if (p.y>0 &&
1114 |         p.x<width-2) {
1115 |         filter[index++] = plane_hypotheses[up+2].w;
1116 |     }
1117 |     if (p.y< height-1 &&
1118 |         p.x<width-2) {
1119 |         filter[index++] = plane_hypotheses[down+2].w;
1120 |     }
1121 |     if (p.y>0 &&
1122 |         p.x>1)
1123 |     {
1124 |         filter[index++] = plane_hypotheses[up-2].w;
1125 |     }
1126 |     if (p.y<height-1 &&
1127 |         p.x>1) {
1128 |         filter[index++] = plane_hypotheses[down-2].w;
1129 |     }
1130 |     if (p.x>0 &&
1131 |         p.y>2)
1132 |     {
1133 |         filter[index++] = plane_hypotheses[left  - width*2].w;
1134 |     }
1135 |     if (p.x<width-1 &&
1136 |         p.y>2)
1137 |     {
1138 |         filter[index++] = plane_hypotheses[right - width*2].w;
1139 |     }
1140 |     if (p.x>0 &&
1141 |         p.y<height-2) {
1142 |         filter[index++] = plane_hypotheses[left  + width*2].w;
1143 |     }
1144 |     if (p.x<width-1 &&
1145 |         p.y<height-2) {
1146 |         filter[index++] = plane_hypotheses[right + width*2].w;
1147 |     }
1148 | 
1149 |     sort_small(filter,index);
1150 |     int median_index = index / 2;
1151 |     if (index % 2 == 0) {
1152 |         plane_hypotheses[center].w = (filter[median_index-1] + filter[median_index]) / 2;
1153 |     } else {
1154 |         plane_hypotheses[center].w = filter[median_index];
1155 |     }
1156 | }
1157 | 
1158 | __global__ void BlackPixelFilter(const Camera *cameras, float4 *plane_hypotheses, float *costs)
1159 | {
1160 |     int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
1161 |     if (threadIdx.x % 2 == 0) {
1162 |         p.y = p.y * 2;
1163 |     } else {
1164 |         p.y = p.y * 2 + 1;
1165 |     }
1166 | 
1167 |     CheckerboardFilter(cameras, plane_hypotheses, costs, p);
1168 | }
1169 | 
1170 | __global__ void RedPixelFilter(const Camera *cameras, float4 *plane_hypotheses, float *costs)
1171 | {
1172 |     int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
1173 |     if (threadIdx.x % 2 == 0) {
1174 |         p.y = p.y * 2 + 1;
1175 |     } else {
1176 |         p.y = p.y * 2;
1177 |     }
1178 | 
1179 |     CheckerboardFilter(cameras, plane_hypotheses, costs, p);
1180 | }
1181 | 
1182 | void ACMM::RunPatchMatch()
1183 | {
1184 |     const int width = cameras[0].width;
1185 |     const int height = cameras[0].height;
1186 | 
1187 |     int BLOCK_W = 32;
1188 |     int BLOCK_H = (BLOCK_W / 2);
1189 | 
1190 |     dim3 grid_size_randinit;
1191 |     grid_size_randinit.x = (width + 16 - 1) / 16;
1192 |     grid_size_randinit.y= (height + 16 - 1) / 16;
1193 |     grid_size_randinit.z = 1;
1194 |     dim3 block_size_randinit;
1195 |     block_size_randinit.x = 16;
1196 |     block_size_randinit.y = 16;
1197 |     block_size_randinit.z = 1;
1198 | 
1199 |     dim3 grid_size_checkerboard;
1200 |     grid_size_checkerboard.x = (width + BLOCK_W - 1) / BLOCK_W;
1201 |     grid_size_checkerboard.y= ( (height / 2) + BLOCK_H - 1) / BLOCK_H;
1202 |     grid_size_checkerboard.z = 1;
1203 |     dim3 block_size_checkerboard;
1204 |     block_size_checkerboard.x = BLOCK_W;
1205 |     block_size_checkerboard.y = BLOCK_H;
1206 |     block_size_checkerboard.z = 1;
1207 | 
1208 |     int max_iterations = params.max_iterations;
1209 | 
1210 |     RandomInitialization<<<grid_size_randinit, block_size_randinit>>>(texture_objects_cuda, cameras_cuda, plane_hypotheses_cuda, scaled_plane_hypotheses_cuda, costs_cuda, pre_costs_cuda, rand_states_cuda, selected_views_cuda, params);
1211 |     CUDA_SAFE_CALL(cudaDeviceSynchronize());
1212 | 
1213 |     for (int i = 0; i < max_iterations; ++i) {
1214 |         BlackPixelUpdate<<<grid_size_checkerboard, block_size_checkerboard>>>(texture_objects_cuda, texture_depths_cuda, cameras_cuda, plane_hypotheses_cuda, costs_cuda, pre_costs_cuda, rand_states_cuda, selected_views_cuda, params, i);
1215 |         CUDA_SAFE_CALL(cudaDeviceSynchronize());
1216 |         RedPixelUpdate<<<grid_size_checkerboard, block_size_checkerboard>>>(texture_objects_cuda, texture_depths_cuda, cameras_cuda, plane_hypotheses_cuda, costs_cuda, pre_costs_cuda,rand_states_cuda, selected_views_cuda, params, i);
1217 |         CUDA_SAFE_CALL(cudaDeviceSynchronize());
1218 |         printf("iteration: %d\n", i);
1219 |     }
1220 | 
1221 |     GetDepthandNormal<<<grid_size_randinit, block_size_randinit>>>(cameras_cuda, plane_hypotheses_cuda, params);
1222 |     CUDA_SAFE_CALL(cudaDeviceSynchronize());
1223 | 
1224 |     BlackPixelFilter<<<grid_size_checkerboard, block_size_checkerboard>>>(cameras_cuda, plane_hypotheses_cuda, costs_cuda);
1225 |     CUDA_SAFE_CALL(cudaDeviceSynchronize());
1226 |     RedPixelFilter<<<grid_size_checkerboard, block_size_checkerboard>>>(cameras_cuda, plane_hypotheses_cuda, costs_cuda);
1227 |     CUDA_SAFE_CALL(cudaDeviceSynchronize());
1228 | 
1229 |     cudaMemcpy(plane_hypotheses_host, plane_hypotheses_cuda, sizeof(float4) * width * height, cudaMemcpyDeviceToHost);
1230 |     cudaMemcpy(costs_host, costs_cuda, sizeof(float) * width * height, cudaMemcpyDeviceToHost);
1231 |     CUDA_SAFE_CALL(cudaDeviceSynchronize());
1232 | }
1233 | 
1234 | __global__ void JBU_cu(JBUParameters *jp, JBUTexObj *jt, float *depth)
1235 | {
1236 |     const int2 p = make_int2 ( blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y );
1237 |     const int rows = jp[0].height;
1238 |     const int cols = jp[0].width;
1239 |     const int center = p.y * cols + p.x;
1240 | 
1241 |     if (p.x >= cols) {
1242 |         return;
1243 |     }
1244 |     if (p.y >= rows) {
1245 |         return;
1246 |     }
1247 | 
1248 |     const float scale  = 1.0 * jp[0].s_width / jp[0].width;
1249 |     const float sigmad = 0.50;
1250 |     const float sigmar = 25.5;
1251 |     const int WinWidth = jp[0].Imagescale * jp[0].Imagescale + 1;
1252 |     int num_neighbors = WinWidth / 2;
1253 | 
1254 |     const float o_y = p.y * scale;
1255 |     const float o_x = p.x * scale;
1256 |     const float refPix = tex2D<float>(jt[0].imgs[0], p.x + 0.5f, p.y + 0.5f);
1257 |     int r_y = 0;
1258 |     int r_ys = 0;
1259 |     int r_x = 0;
1260 |     int r_xs = 0;
1261 |     float sgauss = 0.0, rgauss = 0.0, totalgauss = 0.0;
1262 |     float total_val = 0.0, normalizing_factor = 0.0;
1263 |     float  srcPix = 0, neighborPix = 0;
1264 | 
1265 |     for (int j = -num_neighbors; j <= num_neighbors; ++j) {
1266 |         // source
1267 |         r_y = o_y + j;
1268 |         r_y = (r_y > 0 ? (r_y < jp[0].s_height ? r_y :jp[0].s_height - 1) : 0) ;
1269 |         // reference
1270 |         r_ys = p.y + j;
1271 |         r_ys = (r_ys > 0 ? (r_ys < jp[0].height ? r_ys :jp[0].height - 1) : 0) ;
1272 |         for (int i = -num_neighbors; i <= num_neighbors; ++i) {
1273 |             // source
1274 |             r_x = o_x + i;
1275 |             r_x = (r_x > 0 ? (r_x < jp[0].s_width ? r_x : jp[0].s_width - 1) : 0);
1276 |            srcPix = tex2D<float>(jt[0].imgs[1], r_x + 0.5f, r_y + 0.5f);
1277 |             // refIm
1278 |             r_xs = p.x + i;
1279 |             r_xs = (r_xs > 0 ? (r_xs < jp[0].width ? r_xs :jp[0].width - 1) : 0) ;
1280 |             neighborPix = tex2D<float>(jt[0].imgs[0], r_xs + 0.5f, r_ys + 0.5f);
1281 | 
1282 |             sgauss = SpatialGauss(o_x, o_y, r_x, r_y, sigmad);
1283 |             rgauss = RangeGauss(fabs(refPix - neighborPix), sigmar);
1284 |             totalgauss = sgauss * rgauss;
1285 |             normalizing_factor += totalgauss;
1286 |             total_val += srcPix * totalgauss;
1287 |         }
1288 |     }
1289 | 
1290 |     depth[center] = total_val / normalizing_factor;
1291 | 
1292 | }
1293 | void JBU::CudaRun()
1294 | {
1295 |     int rows = jp_h.height;
1296 |     int cols = jp_h.width;
1297 | 
1298 |     dim3 grid_size_initrand;
1299 |     grid_size_initrand.x= (cols + 16 - 1) / 16;
1300 |     grid_size_initrand.y=(rows + 16 - 1) / 16;
1301 |     grid_size_initrand.z= 1;
1302 |     dim3 block_size_initrand;
1303 |     block_size_initrand.x = 16;
1304 |     block_size_initrand.y = 16;
1305 |     block_size_initrand.z = 1;
1306 | 
1307 |     cudaEvent_t start, stop;
1308 |     cudaEventCreate(&start);
1309 |     cudaEventCreate(&stop);
1310 | 
1311 |     cudaEventRecord(start);
1312 | 
1313 |     cudaDeviceSynchronize();
1314 |     JBU_cu<<< grid_size_initrand, block_size_initrand>>>(jp_d, jt_d, depth_d);
1315 |     cudaDeviceSynchronize();
1316 | 
1317 |     cudaMemcpy(depth_h, depth_d, sizeof(float) * rows * cols, cudaMemcpyDeviceToHost);
1318 |     cudaDeviceSynchronize();
1319 | 
1320 |     cudaEventRecord(stop);
1321 |     cudaEventSynchronize(stop);
1322 |     float milliseconds = 0;
1323 |     cudaEventElapsedTime(&milliseconds, start, stop);
1324 |     printf("Total time needed for computation: %f seconds\n", milliseconds / 1000.f);
1325 | }
1326 | 


--------------------------------------------------------------------------------