1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 //     * Redistributions of source code must retain the above copyright
8 //       notice, this list of conditions and the following disclaimer.
9 //
10 //     * Redistributions in binary form must reproduce the above copyright
11 //       notice, this list of conditions and the following disclaimer in the
12 //       documentation and/or other materials provided with the distribution.
13 //
14 //     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
15 //       its contributors may be used to endorse or promote products derived
16 //       from this software without specific prior written permission.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 // POSSIBILITY OF SUCH DAMAGE.
29 //
30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
31 
32 #include "mvs/model.h"
33 
34 #include "base/camera_models.h"
35 #include "base/pose.h"
36 #include "base/projection.h"
37 #include "base/reconstruction.h"
38 #include "base/triangulation.h"
39 #include "util/misc.h"
40 
41 namespace colmap {
42 namespace mvs {
43 
Read(const std::string & path,const std::string & format)44 void Model::Read(const std::string& path, const std::string& format) {
45   auto format_lower_case = format;
46   StringToLower(&format_lower_case);
47   if (format_lower_case == "colmap") {
48     ReadFromCOLMAP(path);
49   } else if (format_lower_case == "pmvs") {
50     ReadFromPMVS(path);
51   } else {
52     LOG(FATAL) << "Invalid input format";
53   }
54 }
55 
ReadFromCOLMAP(const std::string & path)56 void Model::ReadFromCOLMAP(const std::string& path) {
57   Reconstruction reconstruction;
58   reconstruction.Read(JoinPaths(path, "sparse"));
59 
60   images.reserve(reconstruction.NumRegImages());
61   std::unordered_map<image_t, size_t> image_id_to_idx;
62   for (size_t i = 0; i < reconstruction.NumRegImages(); ++i) {
63     const auto image_id = reconstruction.RegImageIds()[i];
64     const auto& image = reconstruction.Image(image_id);
65     const auto& camera = reconstruction.Camera(image.CameraId());
66 
67     CHECK_EQ(camera.ModelId(), PinholeCameraModel::model_id);
68 
69     const std::string image_path = JoinPaths(path, "images", image.Name());
70     const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> K =
71         camera.CalibrationMatrix().cast<float>();
72     const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> R =
73         QuaternionToRotationMatrix(image.Qvec()).cast<float>();
74     const Eigen::Vector3f T = image.Tvec().cast<float>();
75 
76     images.emplace_back(image_path, camera.Width(), camera.Height(), K.data(),
77                         R.data(), T.data());
78     image_id_to_idx.emplace(image_id, i);
79     image_names_.push_back(image.Name());
80     image_name_to_idx_.emplace(image.Name(), i);
81   }
82 
83   points.reserve(reconstruction.NumPoints3D());
84   for (const auto& point3D : reconstruction.Points3D()) {
85     Point point;
86     point.x = point3D.second.X();
87     point.y = point3D.second.Y();
88     point.z = point3D.second.Z();
89     point.track.reserve(point3D.second.Track().Length());
90     for (const auto& track_el : point3D.second.Track().Elements()) {
91       point.track.push_back(image_id_to_idx.at(track_el.image_id));
92     }
93     points.push_back(point);
94   }
95 }
96 
ReadFromPMVS(const std::string & path)97 void Model::ReadFromPMVS(const std::string& path) {
98   if (ReadFromBundlerPMVS(path)) {
99     return;
100   } else if (ReadFromRawPMVS(path)) {
101     return;
102   } else {
103     LOG(FATAL) << "Invalid PMVS format";
104   }
105 }
106 
GetImageIdx(const std::string & name) const107 int Model::GetImageIdx(const std::string& name) const {
108   CHECK_GT(image_name_to_idx_.count(name), 0)
109       << "Image with name `" << name << "` does not exist";
110   return image_name_to_idx_.at(name);
111 }
112 
GetImageName(const int image_idx) const113 std::string Model::GetImageName(const int image_idx) const {
114   CHECK_GE(image_idx, 0);
115   CHECK_LT(image_idx, image_names_.size());
116   return image_names_.at(image_idx);
117 }
118 
GetMaxOverlappingImages(const size_t num_images,const double min_triangulation_angle) const119 std::vector<std::vector<int>> Model::GetMaxOverlappingImages(
120     const size_t num_images, const double min_triangulation_angle) const {
121   std::vector<std::vector<int>> overlapping_images(images.size());
122 
123   const float min_triangulation_angle_rad = DegToRad(min_triangulation_angle);
124 
125   const auto shared_num_points = ComputeSharedPoints();
126 
127   const float kTriangulationAnglePercentile = 75;
128   const auto triangulation_angles =
129       ComputeTriangulationAngles(kTriangulationAnglePercentile);
130 
131   for (size_t image_idx = 0; image_idx < images.size(); ++image_idx) {
132     const auto& shared_images = shared_num_points.at(image_idx);
133     const auto& overlapping_triangulation_angles =
134         triangulation_angles.at(image_idx);
135 
136     std::vector<std::pair<int, int>> ordered_images;
137     ordered_images.reserve(shared_images.size());
138     for (const auto& image : shared_images) {
139       if (overlapping_triangulation_angles.at(image.first) >=
140           min_triangulation_angle_rad) {
141         ordered_images.emplace_back(image.first, image.second);
142       }
143     }
144 
145     const size_t eff_num_images = std::min(ordered_images.size(), num_images);
146     if (eff_num_images < shared_images.size()) {
147       std::partial_sort(ordered_images.begin(),
148                         ordered_images.begin() + eff_num_images,
149                         ordered_images.end(),
150                         [](const std::pair<int, int> image1,
151                            const std::pair<int, int> image2) {
152                           return image1.second > image2.second;
153                         });
154     } else {
155       std::sort(ordered_images.begin(), ordered_images.end(),
156                 [](const std::pair<int, int> image1,
157                    const std::pair<int, int> image2) {
158                   return image1.second > image2.second;
159                 });
160     }
161 
162     overlapping_images[image_idx].reserve(eff_num_images);
163     for (size_t i = 0; i < eff_num_images; ++i) {
164       overlapping_images[image_idx].push_back(ordered_images[i].first);
165     }
166   }
167 
168   return overlapping_images;
169 }
170 
GetMaxOverlappingImagesFromPMVS() const171 const std::vector<std::vector<int>>& Model::GetMaxOverlappingImagesFromPMVS()
172     const {
173   return pmvs_vis_dat_;
174 }
175 
ComputeDepthRanges() const176 std::vector<std::pair<float, float>> Model::ComputeDepthRanges() const {
177   std::vector<std::vector<float>> depths(images.size());
178   for (const auto& point : points) {
179     const Eigen::Vector3f X(point.x, point.y, point.z);
180     for (const auto& image_idx : point.track) {
181       const auto& image = images.at(image_idx);
182       const float depth =
183           Eigen::Map<const Eigen::Vector3f>(&image.GetR()[6]).dot(X) +
184           image.GetT()[2];
185       if (depth > 0) {
186         depths[image_idx].push_back(depth);
187       }
188     }
189   }
190 
191   std::vector<std::pair<float, float>> depth_ranges(depths.size());
192   for (size_t image_idx = 0; image_idx < depth_ranges.size(); ++image_idx) {
193     auto& depth_range = depth_ranges[image_idx];
194 
195     auto& image_depths = depths[image_idx];
196 
197     if (image_depths.empty()) {
198       depth_range.first = -1.0f;
199       depth_range.second = -1.0f;
200       continue;
201     }
202 
203     std::sort(image_depths.begin(), image_depths.end());
204 
205     const float kMinPercentile = 0.01f;
206     const float kMaxPercentile = 0.99f;
207     depth_range.first = image_depths[image_depths.size() * kMinPercentile];
208     depth_range.second = image_depths[image_depths.size() * kMaxPercentile];
209 
210     const float kStretchRatio = 0.25f;
211     depth_range.first *= (1.0f - kStretchRatio);
212     depth_range.second *= (1.0f + kStretchRatio);
213   }
214 
215   return depth_ranges;
216 }
217 
ComputeSharedPoints() const218 std::vector<std::map<int, int>> Model::ComputeSharedPoints() const {
219   std::vector<std::map<int, int>> shared_points(images.size());
220   for (const auto& point : points) {
221     for (size_t i = 0; i < point.track.size(); ++i) {
222       const int image_idx1 = point.track[i];
223       for (size_t j = 0; j < i; ++j) {
224         const int image_idx2 = point.track[j];
225         if (image_idx1 != image_idx2) {
226           shared_points.at(image_idx1)[image_idx2] += 1;
227           shared_points.at(image_idx2)[image_idx1] += 1;
228         }
229       }
230     }
231   }
232   return shared_points;
233 }
234 
ComputeTriangulationAngles(const float percentile) const235 std::vector<std::map<int, float>> Model::ComputeTriangulationAngles(
236     const float percentile) const {
237   std::vector<Eigen::Vector3d> proj_centers(images.size());
238   for (size_t image_idx = 0; image_idx < images.size(); ++image_idx) {
239     const auto& image = images[image_idx];
240     Eigen::Vector3f C;
241     ComputeProjectionCenter(image.GetR(), image.GetT(), C.data());
242     proj_centers[image_idx] = C.cast<double>();
243   }
244 
245   std::vector<std::map<int, std::vector<float>>> all_triangulation_angles(
246       images.size());
247   for (const auto& point : points) {
248     for (size_t i = 0; i < point.track.size(); ++i) {
249       const int image_idx1 = point.track[i];
250       for (size_t j = 0; j < i; ++j) {
251         const int image_idx2 = point.track[j];
252         if (image_idx1 != image_idx2) {
253           const float angle = CalculateTriangulationAngle(
254               proj_centers.at(image_idx1), proj_centers.at(image_idx2),
255               Eigen::Vector3d(point.x, point.y, point.z));
256           all_triangulation_angles.at(image_idx1)[image_idx2].push_back(angle);
257           all_triangulation_angles.at(image_idx2)[image_idx1].push_back(angle);
258         }
259       }
260     }
261   }
262 
263   std::vector<std::map<int, float>> triangulation_angles(images.size());
264   for (size_t image_idx = 0; image_idx < all_triangulation_angles.size();
265        ++image_idx) {
266     const auto& overlapping_images = all_triangulation_angles[image_idx];
267     for (const auto& image : overlapping_images) {
268       triangulation_angles[image_idx].emplace(
269           image.first, Percentile(image.second, percentile));
270     }
271   }
272 
273   return triangulation_angles;
274 }
275 
ReadFromBundlerPMVS(const std::string & path)276 bool Model::ReadFromBundlerPMVS(const std::string& path) {
277   const std::string bundle_file_path = JoinPaths(path, "bundle.rd.out");
278 
279   if (!ExistsFile(bundle_file_path)) {
280     return false;
281   }
282 
283   std::ifstream file(bundle_file_path);
284   CHECK(file.is_open()) << bundle_file_path;
285 
286   // Header line.
287   std::string header;
288   std::getline(file, header);
289 
290   int num_images, num_points;
291   file >> num_images >> num_points;
292 
293   images.reserve(num_images);
294   for (int image_idx = 0; image_idx < num_images; ++image_idx) {
295     const std::string image_name = StringPrintf("%08d.jpg", image_idx);
296     const std::string image_path = JoinPaths(path, "visualize", image_name);
297 
298     float K[9] = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
299     file >> K[0];
300     K[4] = K[0];
301 
302     Bitmap bitmap;
303     CHECK(bitmap.Read(image_path));
304     K[2] = bitmap.Width() / 2.0f;
305     K[5] = bitmap.Height() / 2.0f;
306 
307     float k1, k2;
308     file >> k1 >> k2;
309     CHECK_EQ(k1, 0.0f);
310     CHECK_EQ(k2, 0.0f);
311 
312     float R[9];
313     for (size_t i = 0; i < 9; ++i) {
314       file >> R[i];
315     }
316     for (size_t i = 3; i < 9; ++i) {
317       R[i] = -R[i];
318     }
319 
320     float T[3];
321     file >> T[0] >> T[1] >> T[2];
322     T[1] = -T[1];
323     T[2] = -T[2];
324 
325     images.emplace_back(image_path, bitmap.Width(), bitmap.Height(), K, R, T);
326     image_names_.push_back(image_name);
327     image_name_to_idx_.emplace(image_name, image_idx);
328   }
329 
330   points.resize(num_points);
331   for (int point_id = 0; point_id < num_points; ++point_id) {
332     auto& point = points[point_id];
333 
334     file >> point.x >> point.y >> point.z;
335 
336     int color[3];
337     file >> color[0] >> color[1] >> color[2];
338 
339     int track_len;
340     file >> track_len;
341     point.track.resize(track_len);
342 
343     for (int i = 0; i < track_len; ++i) {
344       int feature_idx;
345       float imx, imy;
346       file >> point.track[i] >> feature_idx >> imx >> imy;
347       CHECK_LT(point.track[i], images.size());
348     }
349   }
350 
351   return true;
352 }
353 
ReadFromRawPMVS(const std::string & path)354 bool Model::ReadFromRawPMVS(const std::string& path) {
355   const std::string vis_dat_path = JoinPaths(path, "vis.dat");
356   if (!ExistsFile(vis_dat_path)) {
357     return false;
358   }
359 
360   for (int image_idx = 0;; ++image_idx) {
361     const std::string image_name = StringPrintf("%08d.jpg", image_idx);
362     const std::string image_path = JoinPaths(path, "visualize", image_name);
363 
364     if (!ExistsFile(image_path)) {
365       break;
366     }
367 
368     Bitmap bitmap;
369     CHECK(bitmap.Read(image_path));
370 
371     const std::string proj_matrix_path =
372         JoinPaths(path, "txt", StringPrintf("%08d.txt", image_idx));
373 
374     std::ifstream proj_matrix_file(proj_matrix_path);
375     CHECK(proj_matrix_file.is_open()) << proj_matrix_path;
376 
377     std::string contour;
378     proj_matrix_file >> contour;
379     CHECK_EQ(contour, "CONTOUR");
380 
381     Eigen::Matrix3x4d P;
382     for (int i = 0; i < 3; ++i) {
383       proj_matrix_file >> P(i, 0) >> P(i, 1) >> P(i, 2) >> P(i, 3);
384     }
385 
386     Eigen::Matrix3d K;
387     Eigen::Matrix3d R;
388     Eigen::Vector3d T;
389     DecomposeProjectionMatrix(P, &K, &R, &T);
390 
391     // The COLMAP patch match algorithm requires that there is no skew.
392     K(0, 1) = 0.0f;
393     K(1, 0) = 0.0f;
394     K(2, 0) = 0.0f;
395     K(2, 1) = 0.0f;
396     K(2, 2) = 1.0f;
397 
398     const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> K_float = K.cast<float>();
399     const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> R_float = R.cast<float>();
400     const Eigen::Vector3f T_float = T.cast<float>();
401 
402     images.emplace_back(image_path, bitmap.Width(), bitmap.Height(),
403                         K_float.data(), R_float.data(), T_float.data());
404     image_names_.push_back(image_name);
405     image_name_to_idx_.emplace(image_name, image_idx);
406   }
407 
408   std::ifstream vis_dat_file(vis_dat_path);
409   CHECK(vis_dat_file.is_open()) << vis_dat_path;
410 
411   std::string visdata;
412   vis_dat_file >> visdata;
413   CHECK_EQ(visdata, "VISDATA");
414 
415   int num_images;
416   vis_dat_file >> num_images;
417   CHECK_GE(num_images, 0);
418   CHECK_EQ(num_images, images.size());
419 
420   pmvs_vis_dat_.resize(num_images);
421   for (int i = 0; i < num_images; ++i) {
422     int image_idx;
423     vis_dat_file >> image_idx;
424     CHECK_GE(image_idx, 0);
425     CHECK_LT(image_idx, num_images);
426 
427     int num_visible_images;
428     vis_dat_file >> num_visible_images;
429 
430     auto& visible_image_idxs = pmvs_vis_dat_[image_idx];
431     visible_image_idxs.reserve(num_visible_images);
432 
433     for (int j = 0; j < num_visible_images; ++j) {
434       int visible_image_idx;
435       vis_dat_file >> visible_image_idx;
436       CHECK_GE(visible_image_idx, 0);
437       CHECK_LT(visible_image_idx, num_images);
438       if (visible_image_idx != image_idx) {
439         visible_image_idxs.push_back(visible_image_idx);
440       }
441     }
442   }
443 
444   return true;
445 }
446 
447 }  // namespace mvs
448 }  // namespace colmap
449