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 "base/reconstruction.h"
33
34 #include <fstream>
35
36 #include "base/database_cache.h"
37 #include "base/pose.h"
38 #include "base/projection.h"
39 #include "base/similarity_transform.h"
40 #include "base/triangulation.h"
41 #include "estimators/similarity_transform.h"
42 #include "optim/loransac.h"
43 #include "util/bitmap.h"
44 #include "util/misc.h"
45 #include "util/ply.h"
46
47 namespace colmap {
48
Reconstruction()49 Reconstruction::Reconstruction()
50 : correspondence_graph_(nullptr), num_added_points3D_(0) {}
51
Point3DIds() const52 std::unordered_set<point3D_t> Reconstruction::Point3DIds() const {
53 std::unordered_set<point3D_t> point3D_ids;
54 point3D_ids.reserve(points3D_.size());
55
56 for (const auto& point3D : points3D_) {
57 point3D_ids.insert(point3D.first);
58 }
59
60 return point3D_ids;
61 }
62
Load(const DatabaseCache & database_cache)63 void Reconstruction::Load(const DatabaseCache& database_cache) {
64 correspondence_graph_ = nullptr;
65
66 // Add cameras.
67 cameras_.reserve(database_cache.NumCameras());
68 for (const auto& camera : database_cache.Cameras()) {
69 if (!ExistsCamera(camera.first)) {
70 AddCamera(camera.second);
71 }
72 // Else: camera was added before, e.g. with `ReadAllCameras`.
73 }
74
75 // Add images.
76 images_.reserve(database_cache.NumImages());
77
78 for (const auto& image : database_cache.Images()) {
79 if (ExistsImage(image.second.ImageId())) {
80 class Image& existing_image = Image(image.second.ImageId());
81 CHECK_EQ(existing_image.Name(), image.second.Name());
82 if (existing_image.NumPoints2D() == 0) {
83 existing_image.SetPoints2D(image.second.Points2D());
84 } else {
85 CHECK_EQ(image.second.NumPoints2D(), existing_image.NumPoints2D());
86 }
87 existing_image.SetNumObservations(image.second.NumObservations());
88 existing_image.SetNumCorrespondences(image.second.NumCorrespondences());
89 } else {
90 AddImage(image.second);
91 }
92 }
93
94 // Add image pairs.
95 for (const auto& image_pair :
96 database_cache.CorrespondenceGraph().NumCorrespondencesBetweenImages()) {
97 ImagePairStat image_pair_stat;
98 image_pair_stat.num_total_corrs = image_pair.second;
99 image_pair_stats_.emplace(image_pair.first, image_pair_stat);
100 }
101 }
102
SetUp(const CorrespondenceGraph * correspondence_graph)103 void Reconstruction::SetUp(const CorrespondenceGraph* correspondence_graph) {
104 CHECK_NOTNULL(correspondence_graph);
105 for (auto& image : images_) {
106 image.second.SetUp(Camera(image.second.CameraId()));
107 }
108 correspondence_graph_ = correspondence_graph;
109
110 // If an existing model was loaded from disk and there were already images
111 // registered previously, we need to set observations as triangulated.
112 for (const auto image_id : reg_image_ids_) {
113 const class Image& image = Image(image_id);
114 for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
115 ++point2D_idx) {
116 if (image.Point2D(point2D_idx).HasPoint3D()) {
117 const bool kIsContinuedPoint3D = false;
118 SetObservationAsTriangulated(image_id, point2D_idx,
119 kIsContinuedPoint3D);
120 }
121 }
122 }
123 }
124
TearDown()125 void Reconstruction::TearDown() {
126 correspondence_graph_ = nullptr;
127
128 // Remove all not yet registered images.
129 std::unordered_set<camera_t> keep_camera_ids;
130 for (auto it = images_.begin(); it != images_.end();) {
131 if (it->second.IsRegistered()) {
132 keep_camera_ids.insert(it->second.CameraId());
133 it->second.TearDown();
134 ++it;
135 } else {
136 it = images_.erase(it);
137 }
138 }
139
140 // Remove all unused cameras.
141 for (auto it = cameras_.begin(); it != cameras_.end();) {
142 if (keep_camera_ids.count(it->first) == 0) {
143 it = cameras_.erase(it);
144 } else {
145 ++it;
146 }
147 }
148
149 // Compress tracks.
150 for (auto& point3D : points3D_) {
151 point3D.second.Track().Compress();
152 }
153 }
154
AddCamera(const class Camera & camera)155 void Reconstruction::AddCamera(const class Camera& camera) {
156 CHECK(!ExistsCamera(camera.CameraId()));
157 CHECK(camera.VerifyParams());
158 cameras_.emplace(camera.CameraId(), camera);
159 }
160
AddImage(const class Image & image)161 void Reconstruction::AddImage(const class Image& image) {
162 CHECK(!ExistsImage(image.ImageId()));
163 images_[image.ImageId()] = image;
164 }
165
AddPoint3D(const Eigen::Vector3d & xyz,const Track & track,const Eigen::Vector3ub & color)166 point3D_t Reconstruction::AddPoint3D(const Eigen::Vector3d& xyz,
167 const Track& track,
168 const Eigen::Vector3ub& color) {
169 const point3D_t point3D_id = ++num_added_points3D_;
170 CHECK(!ExistsPoint3D(point3D_id));
171
172 class Point3D& point3D = points3D_[point3D_id];
173
174 point3D.SetXYZ(xyz);
175 point3D.SetTrack(track);
176 point3D.SetColor(color);
177
178 for (const auto& track_el : track.Elements()) {
179 class Image& image = Image(track_el.image_id);
180 CHECK(!image.Point2D(track_el.point2D_idx).HasPoint3D());
181 image.SetPoint3DForPoint2D(track_el.point2D_idx, point3D_id);
182 CHECK_LE(image.NumPoints3D(), image.NumPoints2D());
183 }
184
185 const bool kIsContinuedPoint3D = false;
186
187 for (const auto& track_el : track.Elements()) {
188 SetObservationAsTriangulated(track_el.image_id, track_el.point2D_idx,
189 kIsContinuedPoint3D);
190 }
191
192 return point3D_id;
193 }
194
AddObservation(const point3D_t point3D_id,const TrackElement & track_el)195 void Reconstruction::AddObservation(const point3D_t point3D_id,
196 const TrackElement& track_el) {
197 class Image& image = Image(track_el.image_id);
198 CHECK(!image.Point2D(track_el.point2D_idx).HasPoint3D());
199
200 image.SetPoint3DForPoint2D(track_el.point2D_idx, point3D_id);
201 CHECK_LE(image.NumPoints3D(), image.NumPoints2D());
202
203 class Point3D& point3D = Point3D(point3D_id);
204 point3D.Track().AddElement(track_el);
205
206 const bool kIsContinuedPoint3D = true;
207 SetObservationAsTriangulated(track_el.image_id, track_el.point2D_idx,
208 kIsContinuedPoint3D);
209 }
210
MergePoints3D(const point3D_t point3D_id1,const point3D_t point3D_id2)211 point3D_t Reconstruction::MergePoints3D(const point3D_t point3D_id1,
212 const point3D_t point3D_id2) {
213 const class Point3D& point3D1 = Point3D(point3D_id1);
214 const class Point3D& point3D2 = Point3D(point3D_id2);
215
216 const Eigen::Vector3d merged_xyz =
217 (point3D1.Track().Length() * point3D1.XYZ() +
218 point3D2.Track().Length() * point3D2.XYZ()) /
219 (point3D1.Track().Length() + point3D2.Track().Length());
220 const Eigen::Vector3d merged_rgb =
221 (point3D1.Track().Length() * point3D1.Color().cast<double>() +
222 point3D2.Track().Length() * point3D2.Color().cast<double>()) /
223 (point3D1.Track().Length() + point3D2.Track().Length());
224
225 Track merged_track;
226 merged_track.Reserve(point3D1.Track().Length() + point3D2.Track().Length());
227 merged_track.AddElements(point3D1.Track().Elements());
228 merged_track.AddElements(point3D2.Track().Elements());
229
230 DeletePoint3D(point3D_id1);
231 DeletePoint3D(point3D_id2);
232
233 const point3D_t merged_point3D_id =
234 AddPoint3D(merged_xyz, merged_track, merged_rgb.cast<uint8_t>());
235
236 return merged_point3D_id;
237 }
238
DeletePoint3D(const point3D_t point3D_id)239 void Reconstruction::DeletePoint3D(const point3D_t point3D_id) {
240 // Note: Do not change order of these instructions, especially with respect to
241 // `Reconstruction::ResetTriObservations`
242
243 const class Track& track = Point3D(point3D_id).Track();
244
245 const bool kIsDeletedPoint3D = true;
246
247 for (const auto& track_el : track.Elements()) {
248 ResetTriObservations(track_el.image_id, track_el.point2D_idx,
249 kIsDeletedPoint3D);
250 }
251
252 for (const auto& track_el : track.Elements()) {
253 class Image& image = Image(track_el.image_id);
254 image.ResetPoint3DForPoint2D(track_el.point2D_idx);
255 }
256
257 points3D_.erase(point3D_id);
258 }
259
DeleteObservation(const image_t image_id,const point2D_t point2D_idx)260 void Reconstruction::DeleteObservation(const image_t image_id,
261 const point2D_t point2D_idx) {
262 // Note: Do not change order of these instructions, especially with respect to
263 // `Reconstruction::ResetTriObservations`
264
265 class Image& image = Image(image_id);
266 const point3D_t point3D_id = image.Point2D(point2D_idx).Point3DId();
267 class Point3D& point3D = Point3D(point3D_id);
268
269 if (point3D.Track().Length() <= 2) {
270 DeletePoint3D(point3D_id);
271 return;
272 }
273
274 point3D.Track().DeleteElement(image_id, point2D_idx);
275
276 const bool kIsDeletedPoint3D = false;
277 ResetTriObservations(image_id, point2D_idx, kIsDeletedPoint3D);
278
279 image.ResetPoint3DForPoint2D(point2D_idx);
280 }
281
DeleteAllPoints2DAndPoints3D()282 void Reconstruction::DeleteAllPoints2DAndPoints3D() {
283 points3D_.clear();
284 for (auto& image : images_) {
285 class Image new_image;
286 new_image.SetImageId(image.second.ImageId());
287 new_image.SetName(image.second.Name());
288 new_image.SetCameraId(image.second.CameraId());
289 new_image.SetRegistered(image.second.IsRegistered());
290 new_image.SetNumCorrespondences(image.second.NumCorrespondences());
291 new_image.SetQvec(image.second.Qvec());
292 new_image.SetQvecPrior(image.second.QvecPrior());
293 new_image.SetTvec(image.second.Tvec());
294 new_image.SetTvecPrior(image.second.TvecPrior());
295 image.second = new_image;
296 }
297 }
298
RegisterImage(const image_t image_id)299 void Reconstruction::RegisterImage(const image_t image_id) {
300 class Image& image = Image(image_id);
301 if (!image.IsRegistered()) {
302 image.SetRegistered(true);
303 reg_image_ids_.push_back(image_id);
304 }
305 }
306
DeRegisterImage(const image_t image_id)307 void Reconstruction::DeRegisterImage(const image_t image_id) {
308 class Image& image = Image(image_id);
309
310 for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
311 ++point2D_idx) {
312 if (image.Point2D(point2D_idx).HasPoint3D()) {
313 DeleteObservation(image_id, point2D_idx);
314 }
315 }
316
317 image.SetRegistered(false);
318
319 reg_image_ids_.erase(
320 std::remove(reg_image_ids_.begin(), reg_image_ids_.end(), image_id),
321 reg_image_ids_.end());
322 }
323
Normalize(const double extent,const double p0,const double p1,const bool use_images)324 void Reconstruction::Normalize(const double extent, const double p0,
325 const double p1, const bool use_images) {
326 CHECK_GT(extent, 0);
327 CHECK_GE(p0, 0);
328 CHECK_LE(p0, 1);
329 CHECK_GE(p1, 0);
330 CHECK_LE(p1, 1);
331 CHECK_LE(p0, p1);
332
333 if ((use_images && reg_image_ids_.size() < 2) ||
334 (!use_images && points3D_.size() < 2)) {
335 return;
336 }
337
338 EIGEN_STL_UMAP(class Image*, Eigen::Vector3d) proj_centers;
339
340 for (size_t i = 0; i < reg_image_ids_.size(); ++i) {
341 class Image& image = Image(reg_image_ids_[i]);
342 const Eigen::Vector3d proj_center = image.ProjectionCenter();
343 proj_centers[&image] = proj_center;
344 }
345
346 // Coordinates of image centers or point locations.
347 std::vector<float> coords_x;
348 std::vector<float> coords_y;
349 std::vector<float> coords_z;
350 if (use_images) {
351 coords_x.reserve(proj_centers.size());
352 coords_y.reserve(proj_centers.size());
353 coords_z.reserve(proj_centers.size());
354 for (const auto& proj_center : proj_centers) {
355 coords_x.push_back(static_cast<float>(proj_center.second(0)));
356 coords_y.push_back(static_cast<float>(proj_center.second(1)));
357 coords_z.push_back(static_cast<float>(proj_center.second(2)));
358 }
359 } else {
360 coords_x.reserve(points3D_.size());
361 coords_y.reserve(points3D_.size());
362 coords_z.reserve(points3D_.size());
363 for (const auto& point3D : points3D_) {
364 coords_x.push_back(static_cast<float>(point3D.second.X()));
365 coords_y.push_back(static_cast<float>(point3D.second.Y()));
366 coords_z.push_back(static_cast<float>(point3D.second.Z()));
367 }
368 }
369
370 // Determine robust bounding box and mean.
371
372 std::sort(coords_x.begin(), coords_x.end());
373 std::sort(coords_y.begin(), coords_y.end());
374 std::sort(coords_z.begin(), coords_z.end());
375
376 const size_t P0 = static_cast<size_t>(
377 (coords_x.size() > 3) ? p0 * (coords_x.size() - 1) : 0);
378 const size_t P1 = static_cast<size_t>(
379 (coords_x.size() > 3) ? p1 * (coords_x.size() - 1) : coords_x.size() - 1);
380
381 const Eigen::Vector3d bbox_min(coords_x[P0], coords_y[P0], coords_z[P0]);
382 const Eigen::Vector3d bbox_max(coords_x[P1], coords_y[P1], coords_z[P1]);
383
384 Eigen::Vector3d mean_coord(0, 0, 0);
385 for (size_t i = P0; i <= P1; ++i) {
386 mean_coord(0) += coords_x[i];
387 mean_coord(1) += coords_y[i];
388 mean_coord(2) += coords_z[i];
389 }
390 mean_coord /= P1 - P0 + 1;
391
392 // Calculate scale and translation, such that
393 // translation is applied before scaling.
394 const double old_extent = (bbox_max - bbox_min).norm();
395 double scale;
396 if (old_extent < std::numeric_limits<double>::epsilon()) {
397 scale = 1;
398 } else {
399 scale = extent / old_extent;
400 }
401
402 const Eigen::Vector3d translation = mean_coord;
403
404 // Transform images.
405 for (auto& image_proj_center : proj_centers) {
406 image_proj_center.second -= translation;
407 image_proj_center.second *= scale;
408 const Eigen::Quaterniond quat(
409 image_proj_center.first->Qvec(0), image_proj_center.first->Qvec(1),
410 image_proj_center.first->Qvec(2), image_proj_center.first->Qvec(3));
411 image_proj_center.first->SetTvec(quat * -image_proj_center.second);
412 }
413
414 // Transform points.
415 for (auto& point3D : points3D_) {
416 point3D.second.XYZ() -= translation;
417 point3D.second.XYZ() *= scale;
418 }
419 }
420
Transform(const SimilarityTransform3 & tform)421 void Reconstruction::Transform(const SimilarityTransform3& tform) {
422 for (auto& image : images_) {
423 tform.TransformPose(&image.second.Qvec(), &image.second.Tvec());
424 }
425 for (auto& point3D : points3D_) {
426 tform.TransformPoint(&point3D.second.XYZ());
427 }
428 }
429
Merge(const Reconstruction & reconstruction,const double max_reproj_error)430 bool Reconstruction::Merge(const Reconstruction& reconstruction,
431 const double max_reproj_error) {
432 const double kMinInlierObservations = 0.3;
433
434 Eigen::Matrix3x4d alignment;
435 if (!ComputeAlignmentBetweenReconstructions(reconstruction, *this,
436 kMinInlierObservations,
437 max_reproj_error, &alignment)) {
438 return false;
439 }
440
441 const SimilarityTransform3 tform(alignment);
442
443 // Find common and missing images in the two reconstructions.
444
445 std::unordered_set<image_t> common_image_ids;
446 common_image_ids.reserve(reconstruction.NumRegImages());
447 std::unordered_set<image_t> missing_image_ids;
448 missing_image_ids.reserve(reconstruction.NumRegImages());
449
450 for (const auto& image_id : reconstruction.RegImageIds()) {
451 if (ExistsImage(image_id)) {
452 common_image_ids.insert(image_id);
453 } else {
454 missing_image_ids.insert(image_id);
455 }
456 }
457
458 // Register the missing images in this reconstruction.
459
460 for (const auto image_id : missing_image_ids) {
461 auto reg_image = reconstruction.Image(image_id);
462 reg_image.SetRegistered(false);
463 AddImage(reg_image);
464 RegisterImage(image_id);
465 if (!ExistsCamera(reg_image.CameraId())) {
466 AddCamera(reconstruction.Camera(reg_image.CameraId()));
467 }
468 auto& image = Image(image_id);
469 tform.TransformPose(&image.Qvec(), &image.Tvec());
470 }
471
472 // Merge the two point clouds using the following two rules:
473 // - copy points to this reconstruction with non-conflicting tracks,
474 // i.e. points that do not have an already triangulated observation
475 // in this reconstruction.
476 // - merge tracks that are unambiguous, i.e. only merge points in the two
477 // reconstructions if they have a one-to-one mapping.
478 // Note that in both cases no cheirality or reprojection test is performed.
479
480 for (const auto& point3D : reconstruction.Points3D()) {
481 Track new_track;
482 Track old_track;
483 std::set<point3D_t> old_point3D_ids;
484 for (const auto& track_el : point3D.second.Track().Elements()) {
485 if (common_image_ids.count(track_el.image_id) > 0) {
486 const auto& point2D =
487 Image(track_el.image_id).Point2D(track_el.point2D_idx);
488 if (point2D.HasPoint3D()) {
489 old_track.AddElement(track_el);
490 old_point3D_ids.insert(point2D.Point3DId());
491 } else {
492 new_track.AddElement(track_el);
493 }
494 } else if (missing_image_ids.count(track_el.image_id) > 0) {
495 Image(track_el.image_id).ResetPoint3DForPoint2D(track_el.point2D_idx);
496 new_track.AddElement(track_el);
497 }
498 }
499
500 const bool create_new_point = new_track.Length() >= 2;
501 const bool merge_new_and_old_point =
502 (new_track.Length() + old_track.Length()) >= 2 &&
503 old_point3D_ids.size() == 1;
504 if (create_new_point || merge_new_and_old_point) {
505 Eigen::Vector3d xyz = point3D.second.XYZ();
506 tform.TransformPoint(&xyz);
507 const auto point3D_id =
508 AddPoint3D(xyz, new_track, point3D.second.Color());
509 if (old_point3D_ids.size() == 1) {
510 MergePoints3D(point3D_id, *old_point3D_ids.begin());
511 }
512 }
513 }
514
515 FilterPoints3DWithLargeReprojectionError(max_reproj_error, Point3DIds());
516
517 return true;
518 }
519
Align(const std::vector<std::string> & image_names,const std::vector<Eigen::Vector3d> & locations,const int min_common_images)520 bool Reconstruction::Align(const std::vector<std::string>& image_names,
521 const std::vector<Eigen::Vector3d>& locations,
522 const int min_common_images) {
523 CHECK_GE(min_common_images, 3);
524 CHECK_EQ(image_names.size(), locations.size());
525
526 // Find out which images are contained in the reconstruction and get the
527 // positions of their camera centers.
528 std::set<image_t> common_image_ids;
529 std::vector<Eigen::Vector3d> src;
530 std::vector<Eigen::Vector3d> dst;
531 for (size_t i = 0; i < image_names.size(); ++i) {
532 const class Image* image = FindImageWithName(image_names[i]);
533 if (image == nullptr) {
534 continue;
535 }
536
537 if (!IsImageRegistered(image->ImageId())) {
538 continue;
539 }
540
541 // Ignore duplicate images.
542 if (common_image_ids.count(image->ImageId()) > 0) {
543 continue;
544 }
545
546 common_image_ids.insert(image->ImageId());
547 src.push_back(image->ProjectionCenter());
548 dst.push_back(locations[i]);
549 }
550
551 // Only compute the alignment if there are enough correspondences.
552 if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
553 return false;
554 }
555
556 SimilarityTransform3 tform;
557 if (!tform.Estimate(src, dst)) {
558 return false;
559 }
560
561 Transform(tform);
562
563 return true;
564 }
565
AlignRobust(const std::vector<std::string> & image_names,const std::vector<Eigen::Vector3d> & locations,const int min_common_images,const RANSACOptions & ransac_options)566 bool Reconstruction::AlignRobust(const std::vector<std::string>& image_names,
567 const std::vector<Eigen::Vector3d>& locations,
568 const int min_common_images,
569 const RANSACOptions& ransac_options) {
570 CHECK_GE(min_common_images, 3);
571 CHECK_EQ(image_names.size(), locations.size());
572
573 // Find out which images are contained in the reconstruction and get the
574 // positions of their camera centers.
575 std::set<image_t> common_image_ids;
576 std::vector<Eigen::Vector3d> src;
577 std::vector<Eigen::Vector3d> dst;
578 for (size_t i = 0; i < image_names.size(); ++i) {
579 const class Image* image = FindImageWithName(image_names[i]);
580 if (image == nullptr) {
581 continue;
582 }
583
584 if (!IsImageRegistered(image->ImageId())) {
585 continue;
586 }
587
588 // Ignore duplicate images.
589 if (common_image_ids.count(image->ImageId()) > 0) {
590 continue;
591 }
592
593 common_image_ids.insert(image->ImageId());
594 src.push_back(image->ProjectionCenter());
595 dst.push_back(locations[i]);
596 }
597
598 // Only compute the alignment if there are enough correspondences.
599 if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
600 return false;
601 }
602
603 LORANSAC<SimilarityTransformEstimator<3>, SimilarityTransformEstimator<3>>
604 ransac(ransac_options);
605
606 const auto report = ransac.Estimate(src, dst);
607
608 if (report.support.num_inliers < static_cast<size_t>(min_common_images)) {
609 return false;
610 }
611
612 Transform(SimilarityTransform3(report.model));
613
614 return true;
615 }
616
FindImageWithName(const std::string & name) const617 const class Image* Reconstruction::FindImageWithName(
618 const std::string& name) const {
619 for (const auto& image : images_) {
620 if (image.second.Name() == name) {
621 return &image.second;
622 }
623 }
624 return nullptr;
625 }
626
FindCommonRegImageIds(const Reconstruction & reconstruction) const627 std::vector<image_t> Reconstruction::FindCommonRegImageIds(
628 const Reconstruction& reconstruction) const {
629 std::vector<image_t> common_reg_image_ids;
630 for (const auto image_id : reg_image_ids_) {
631 if (reconstruction.ExistsImage(image_id) &&
632 reconstruction.IsImageRegistered(image_id)) {
633 CHECK_EQ(Image(image_id).Name(), reconstruction.Image(image_id).Name());
634 common_reg_image_ids.push_back(image_id);
635 }
636 }
637 return common_reg_image_ids;
638 }
639
TranscribeImageIdsToDatabase(const Database & database)640 void Reconstruction::TranscribeImageIdsToDatabase(const Database& database) {
641 std::unordered_map<image_t, image_t> old_to_new_image_ids;
642 old_to_new_image_ids.reserve(NumImages());
643
644 EIGEN_STL_UMAP(image_t, class Image) new_images;
645 new_images.reserve(NumImages());
646
647 for (auto& image : images_) {
648 if (!database.ExistsImageWithName(image.second.Name())) {
649 LOG(FATAL) << "Image with name " << image.second.Name()
650 << " does not exist in database";
651 }
652
653 const auto database_image = database.ReadImageWithName(image.second.Name());
654 old_to_new_image_ids.emplace(image.second.ImageId(),
655 database_image.ImageId());
656 image.second.SetImageId(database_image.ImageId());
657 new_images.emplace(database_image.ImageId(), image.second);
658 }
659
660 images_ = std::move(new_images);
661
662 for (auto& image_id : reg_image_ids_) {
663 image_id = old_to_new_image_ids.at(image_id);
664 }
665
666 for (auto& point3D : points3D_) {
667 for (auto& track_el : point3D.second.Track().Elements()) {
668 track_el.image_id = old_to_new_image_ids.at(track_el.image_id);
669 }
670 }
671 }
672
FilterPoints3D(const double max_reproj_error,const double min_tri_angle,const std::unordered_set<point3D_t> & point3D_ids)673 size_t Reconstruction::FilterPoints3D(
674 const double max_reproj_error, const double min_tri_angle,
675 const std::unordered_set<point3D_t>& point3D_ids) {
676 size_t num_filtered = 0;
677 num_filtered +=
678 FilterPoints3DWithLargeReprojectionError(max_reproj_error, point3D_ids);
679 num_filtered +=
680 FilterPoints3DWithSmallTriangulationAngle(min_tri_angle, point3D_ids);
681 return num_filtered;
682 }
683
FilterPoints3DInImages(const double max_reproj_error,const double min_tri_angle,const std::unordered_set<image_t> & image_ids)684 size_t Reconstruction::FilterPoints3DInImages(
685 const double max_reproj_error, const double min_tri_angle,
686 const std::unordered_set<image_t>& image_ids) {
687 std::unordered_set<point3D_t> point3D_ids;
688 for (const image_t image_id : image_ids) {
689 const class Image& image = Image(image_id);
690 for (const Point2D& point2D : image.Points2D()) {
691 if (point2D.HasPoint3D()) {
692 point3D_ids.insert(point2D.Point3DId());
693 }
694 }
695 }
696 return FilterPoints3D(max_reproj_error, min_tri_angle, point3D_ids);
697 }
698
FilterAllPoints3D(const double max_reproj_error,const double min_tri_angle)699 size_t Reconstruction::FilterAllPoints3D(const double max_reproj_error,
700 const double min_tri_angle) {
701 // Important: First filter observations and points with large reprojection
702 // error, so that observations with large reprojection error do not make
703 // a point stable through a large triangulation angle.
704 const std::unordered_set<point3D_t>& point3D_ids = Point3DIds();
705 size_t num_filtered = 0;
706 num_filtered +=
707 FilterPoints3DWithLargeReprojectionError(max_reproj_error, point3D_ids);
708 num_filtered +=
709 FilterPoints3DWithSmallTriangulationAngle(min_tri_angle, point3D_ids);
710 return num_filtered;
711 }
712
FilterObservationsWithNegativeDepth()713 size_t Reconstruction::FilterObservationsWithNegativeDepth() {
714 size_t num_filtered = 0;
715 for (const auto image_id : reg_image_ids_) {
716 const class Image& image = Image(image_id);
717 const Eigen::Matrix3x4d proj_matrix = image.ProjectionMatrix();
718 for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
719 ++point2D_idx) {
720 const Point2D& point2D = image.Point2D(point2D_idx);
721 if (point2D.HasPoint3D()) {
722 const class Point3D& point3D = Point3D(point2D.Point3DId());
723 if (!HasPointPositiveDepth(proj_matrix, point3D.XYZ())) {
724 DeleteObservation(image_id, point2D_idx);
725 num_filtered += 1;
726 }
727 }
728 }
729 }
730 return num_filtered;
731 }
732
FilterImages(const double min_focal_length_ratio,const double max_focal_length_ratio,const double max_extra_param)733 std::vector<image_t> Reconstruction::FilterImages(
734 const double min_focal_length_ratio, const double max_focal_length_ratio,
735 const double max_extra_param) {
736 std::vector<image_t> filtered_image_ids;
737 for (const image_t image_id : RegImageIds()) {
738 const class Image& image = Image(image_id);
739 const class Camera& camera = Camera(image.CameraId());
740 if (image.NumPoints3D() == 0) {
741 filtered_image_ids.push_back(image_id);
742 } else if (camera.HasBogusParams(min_focal_length_ratio,
743 max_focal_length_ratio, max_extra_param)) {
744 filtered_image_ids.push_back(image_id);
745 }
746 }
747
748 // Only de-register after iterating over reg_image_ids_ to avoid
749 // simultaneous iteration and modification of the vector.
750 for (const image_t image_id : filtered_image_ids) {
751 DeRegisterImage(image_id);
752 }
753
754 return filtered_image_ids;
755 }
756
ComputeNumObservations() const757 size_t Reconstruction::ComputeNumObservations() const {
758 size_t num_obs = 0;
759 for (const image_t image_id : reg_image_ids_) {
760 num_obs += Image(image_id).NumPoints3D();
761 }
762 return num_obs;
763 }
764
ComputeMeanTrackLength() const765 double Reconstruction::ComputeMeanTrackLength() const {
766 if (points3D_.empty()) {
767 return 0.0;
768 } else {
769 return ComputeNumObservations() / static_cast<double>(points3D_.size());
770 }
771 }
772
ComputeMeanObservationsPerRegImage() const773 double Reconstruction::ComputeMeanObservationsPerRegImage() const {
774 if (reg_image_ids_.empty()) {
775 return 0.0;
776 } else {
777 return ComputeNumObservations() /
778 static_cast<double>(reg_image_ids_.size());
779 }
780 }
781
ComputeMeanReprojectionError() const782 double Reconstruction::ComputeMeanReprojectionError() const {
783 double error_sum = 0.0;
784 size_t num_valid_errors = 0;
785 for (const auto& point3D : points3D_) {
786 if (point3D.second.HasError()) {
787 error_sum += point3D.second.Error();
788 num_valid_errors += 1;
789 }
790 }
791
792 if (num_valid_errors == 0) {
793 return 0.0;
794 } else {
795 return error_sum / num_valid_errors;
796 }
797 }
798
Read(const std::string & path)799 void Reconstruction::Read(const std::string& path) {
800 if (ExistsFile(JoinPaths(path, "cameras.bin")) &&
801 ExistsFile(JoinPaths(path, "images.bin")) &&
802 ExistsFile(JoinPaths(path, "points3D.bin"))) {
803 ReadBinary(path);
804 } else if (ExistsFile(JoinPaths(path, "cameras.txt")) &&
805 ExistsFile(JoinPaths(path, "images.txt")) &&
806 ExistsFile(JoinPaths(path, "points3D.txt"))) {
807 ReadText(path);
808 } else {
809 LOG(FATAL) << "cameras, images, points3D files do not exist at " << path;
810 }
811 }
812
Write(const std::string & path) const813 void Reconstruction::Write(const std::string& path) const { WriteBinary(path); }
814
ReadText(const std::string & path)815 void Reconstruction::ReadText(const std::string& path) {
816 ReadCamerasText(JoinPaths(path, "cameras.txt"));
817 ReadImagesText(JoinPaths(path, "images.txt"));
818 ReadPoints3DText(JoinPaths(path, "points3D.txt"));
819 }
820
ReadBinary(const std::string & path)821 void Reconstruction::ReadBinary(const std::string& path) {
822 ReadCamerasBinary(JoinPaths(path, "cameras.bin"));
823 ReadImagesBinary(JoinPaths(path, "images.bin"));
824 ReadPoints3DBinary(JoinPaths(path, "points3D.bin"));
825 }
826
WriteText(const std::string & path) const827 void Reconstruction::WriteText(const std::string& path) const {
828 WriteCamerasText(JoinPaths(path, "cameras.txt"));
829 WriteImagesText(JoinPaths(path, "images.txt"));
830 WritePoints3DText(JoinPaths(path, "points3D.txt"));
831 }
832
WriteBinary(const std::string & path) const833 void Reconstruction::WriteBinary(const std::string& path) const {
834 WriteCamerasBinary(JoinPaths(path, "cameras.bin"));
835 WriteImagesBinary(JoinPaths(path, "images.bin"));
836 WritePoints3DBinary(JoinPaths(path, "points3D.bin"));
837 }
838
ConvertToPLY() const839 std::vector<PlyPoint> Reconstruction::ConvertToPLY() const {
840 std::vector<PlyPoint> ply_points;
841 ply_points.reserve(points3D_.size());
842
843 for (const auto& point3D : points3D_) {
844 PlyPoint ply_point;
845 ply_point.x = point3D.second.X();
846 ply_point.y = point3D.second.Y();
847 ply_point.z = point3D.second.Z();
848 ply_point.r = point3D.second.Color(0);
849 ply_point.g = point3D.second.Color(1);
850 ply_point.b = point3D.second.Color(2);
851 ply_points.push_back(ply_point);
852 }
853
854 return ply_points;
855 }
856
ImportPLY(const std::string & path)857 void Reconstruction::ImportPLY(const std::string& path) {
858 points3D_.clear();
859
860 const auto ply_points = ReadPly(path);
861
862 points3D_.reserve(ply_points.size());
863
864 for (const auto& ply_point : ply_points) {
865 AddPoint3D(Eigen::Vector3d(ply_point.x, ply_point.y, ply_point.z), Track(),
866 Eigen::Vector3ub(ply_point.r, ply_point.g, ply_point.b));
867 }
868 }
869
ExportNVM(const std::string & path) const870 bool Reconstruction::ExportNVM(const std::string& path) const {
871 std::ofstream file(path, std::ios::trunc);
872 CHECK(file.is_open()) << path;
873
874 // White space added for compatibility with Meshlab.
875 file << "NVM_V3 " << std::endl << " " << std::endl;
876 file << reg_image_ids_.size() << " " << std::endl;
877
878 std::unordered_map<image_t, size_t> image_id_to_idx_;
879 size_t image_idx = 0;
880
881 for (const auto image_id : reg_image_ids_) {
882 const class Image& image = Image(image_id);
883 const class Camera& camera = Camera(image.CameraId());
884
885 if (camera.ModelId() != SimpleRadialCameraModel::model_id) {
886 std::cout << "WARNING: NVM only supports `SIMPLE_RADIAL` camera model."
887 << std::endl;
888 return false;
889 }
890
891 const double f =
892 camera.Params(SimpleRadialCameraModel::focal_length_idxs[0]);
893 const double k =
894 -1 * camera.Params(SimpleRadialCameraModel::extra_params_idxs[0]);
895 const Eigen::Vector3d proj_center = image.ProjectionCenter();
896
897 file << image.Name() << " ";
898 file << f << " ";
899 file << image.Qvec(0) << " ";
900 file << image.Qvec(1) << " ";
901 file << image.Qvec(2) << " ";
902 file << image.Qvec(3) << " ";
903 file << proj_center(0) << " ";
904 file << proj_center(1) << " ";
905 file << proj_center(2) << " ";
906 file << k << " ";
907 file << 0 << std::endl;
908
909 image_id_to_idx_[image_id] = image_idx;
910 image_idx += 1;
911 }
912
913 file << std::endl << points3D_.size() << std::endl;
914
915 for (const auto& point3D : points3D_) {
916 file << point3D.second.XYZ()(0) << " ";
917 file << point3D.second.XYZ()(1) << " ";
918 file << point3D.second.XYZ()(2) << " ";
919 file << static_cast<int>(point3D.second.Color(0)) << " ";
920 file << static_cast<int>(point3D.second.Color(1)) << " ";
921 file << static_cast<int>(point3D.second.Color(2)) << " ";
922
923 std::ostringstream line;
924
925 std::unordered_set<image_t> image_ids;
926 for (const auto& track_el : point3D.second.Track().Elements()) {
927 // Make sure that each point only has a single observation per image,
928 // since VisualSfM does not support with multiple observations.
929 if (image_ids.count(track_el.image_id) == 0) {
930 const class Image& image = Image(track_el.image_id);
931 const Point2D& point2D = image.Point2D(track_el.point2D_idx);
932 line << image_id_to_idx_[track_el.image_id] << " ";
933 line << track_el.point2D_idx << " ";
934 line << point2D.X() << " ";
935 line << point2D.Y() << " ";
936 image_ids.insert(track_el.image_id);
937 }
938 }
939
940 std::string line_string = line.str();
941 line_string = line_string.substr(0, line_string.size() - 1);
942
943 file << image_ids.size() << " ";
944 file << line_string << std::endl;
945 }
946
947 return true;
948 }
949
ExportBundler(const std::string & path,const std::string & list_path) const950 bool Reconstruction::ExportBundler(const std::string& path,
951 const std::string& list_path) const {
952 std::ofstream file(path, std::ios::trunc);
953 CHECK(file.is_open()) << path;
954
955 std::ofstream list_file(list_path, std::ios::trunc);
956 CHECK(list_file.is_open()) << list_path;
957
958 file << "# Bundle file v0.3" << std::endl;
959
960 file << reg_image_ids_.size() << " " << points3D_.size() << std::endl;
961
962 std::unordered_map<image_t, size_t> image_id_to_idx_;
963 size_t image_idx = 0;
964
965 for (const image_t image_id : reg_image_ids_) {
966 const class Image& image = Image(image_id);
967 const class Camera& camera = Camera(image.CameraId());
968
969 double f;
970 double k1;
971 double k2;
972 if (camera.ModelId() == SimplePinholeCameraModel::model_id ||
973 camera.ModelId() == PinholeCameraModel::model_id) {
974 f = camera.MeanFocalLength();
975 k1 = 0.0;
976 k2 = 0.0;
977 } else if (camera.ModelId() == SimpleRadialCameraModel::model_id) {
978 f = camera.Params(SimpleRadialCameraModel::focal_length_idxs[0]);
979 k1 = camera.Params(SimpleRadialCameraModel::extra_params_idxs[0]);
980 k2 = 0.0;
981 } else if (camera.ModelId() == RadialCameraModel::model_id) {
982 f = camera.Params(RadialCameraModel::focal_length_idxs[0]);
983 k1 = camera.Params(RadialCameraModel::extra_params_idxs[0]);
984 k2 = camera.Params(RadialCameraModel::extra_params_idxs[1]);
985 } else {
986 std::cout << "WARNING: Bundler only supports `SIMPLE_RADIAL` and "
987 "`RADIAL` camera models."
988 << std::endl;
989 return false;
990 }
991
992 file << f << " " << k1 << " " << k2 << std::endl;
993
994 const Eigen::Matrix3d R = image.RotationMatrix();
995 file << R(0, 0) << " " << R(0, 1) << " " << R(0, 2) << std::endl;
996 file << -R(1, 0) << " " << -R(1, 1) << " " << -R(1, 2) << std::endl;
997 file << -R(2, 0) << " " << -R(2, 1) << " " << -R(2, 2) << std::endl;
998
999 file << image.Tvec(0) << " ";
1000 file << -image.Tvec(1) << " ";
1001 file << -image.Tvec(2) << std::endl;
1002
1003 list_file << image.Name() << std::endl;
1004
1005 image_id_to_idx_[image_id] = image_idx;
1006 image_idx += 1;
1007 }
1008
1009 for (const auto& point3D : points3D_) {
1010 file << point3D.second.XYZ()(0) << " ";
1011 file << point3D.second.XYZ()(1) << " ";
1012 file << point3D.second.XYZ()(2) << std::endl;
1013
1014 file << static_cast<int>(point3D.second.Color(0)) << " ";
1015 file << static_cast<int>(point3D.second.Color(1)) << " ";
1016 file << static_cast<int>(point3D.second.Color(2)) << std::endl;
1017
1018 std::ostringstream line;
1019
1020 line << point3D.second.Track().Length() << " ";
1021
1022 for (const auto& track_el : point3D.second.Track().Elements()) {
1023 const class Image& image = Image(track_el.image_id);
1024 const class Camera& camera = Camera(image.CameraId());
1025
1026 // Bundler output assumes image coordinate system origin
1027 // in the lower left corner of the image with the center of
1028 // the lower left pixel being (0, 0). Our coordinate system
1029 // starts in the upper left corner with the center of the
1030 // upper left pixel being (0.5, 0.5).
1031
1032 const Point2D& point2D = image.Point2D(track_el.point2D_idx);
1033
1034 line << image_id_to_idx_.at(track_el.image_id) << " ";
1035 line << track_el.point2D_idx << " ";
1036 line << point2D.X() - camera.PrincipalPointX() << " ";
1037 line << camera.PrincipalPointY() - point2D.Y() << " ";
1038 }
1039
1040 std::string line_string = line.str();
1041 line_string = line_string.substr(0, line_string.size() - 1);
1042
1043 file << line_string << std::endl;
1044 }
1045
1046 return true;
1047 }
1048
ExportPLY(const std::string & path) const1049 void Reconstruction::ExportPLY(const std::string& path) const {
1050 const auto ply_points = ConvertToPLY();
1051
1052 const bool kWriteNormal = false;
1053 const bool kWriteRGB = true;
1054 WriteBinaryPlyPoints(path, ply_points, kWriteNormal, kWriteRGB);
1055 }
1056
ExportVRML(const std::string & images_path,const std::string & points3D_path,const double image_scale,const Eigen::Vector3d & image_rgb) const1057 void Reconstruction::ExportVRML(const std::string& images_path,
1058 const std::string& points3D_path,
1059 const double image_scale,
1060 const Eigen::Vector3d& image_rgb) const {
1061 std::ofstream images_file(images_path, std::ios::trunc);
1062 CHECK(images_file.is_open()) << images_path;
1063
1064 const double six = image_scale * 0.15;
1065 const double siy = image_scale * 0.1;
1066
1067 std::vector<Eigen::Vector3d> points;
1068 points.emplace_back(-six, -siy, six * 1.0 * 2.0);
1069 points.emplace_back(+six, -siy, six * 1.0 * 2.0);
1070 points.emplace_back(+six, +siy, six * 1.0 * 2.0);
1071 points.emplace_back(-six, +siy, six * 1.0 * 2.0);
1072 points.emplace_back(0, 0, 0);
1073 points.emplace_back(-six / 3.0, -siy / 3.0, six * 1.0 * 2.0);
1074 points.emplace_back(+six / 3.0, -siy / 3.0, six * 1.0 * 2.0);
1075 points.emplace_back(+six / 3.0, +siy / 3.0, six * 1.0 * 2.0);
1076 points.emplace_back(-six / 3.0, +siy / 3.0, six * 1.0 * 2.0);
1077
1078 for (const auto& image : images_) {
1079 if (!image.second.IsRegistered()) {
1080 continue;
1081 }
1082
1083 images_file << "Shape{\n";
1084 images_file << " appearance Appearance {\n";
1085 images_file << " material DEF Default-ffRffGffB Material {\n";
1086 images_file << " ambientIntensity 0\n";
1087 images_file << " diffuseColor "
1088 << " " << image_rgb(0) << " " << image_rgb(1) << " "
1089 << image_rgb(2) << "\n";
1090 images_file << " emissiveColor 0.1 0.1 0.1 } }\n";
1091 images_file << " geometry IndexedFaceSet {\n";
1092 images_file << " solid FALSE \n";
1093 images_file << " colorPerVertex TRUE \n";
1094 images_file << " ccw TRUE \n";
1095
1096 images_file << " coord Coordinate {\n";
1097 images_file << " point [\n";
1098
1099 Eigen::Transform<double, 3, Eigen::Affine> transform;
1100 transform.matrix().topLeftCorner<3, 4>() =
1101 image.second.InverseProjectionMatrix();
1102
1103 // Move camera base model to camera pose.
1104 for (size_t i = 0; i < points.size(); i++) {
1105 const Eigen::Vector3d point = transform * points[i];
1106 images_file << point(0) << " " << point(1) << " " << point(2) << "\n";
1107 }
1108
1109 images_file << " ] }\n";
1110
1111 images_file << "color Color {color [\n";
1112 for (size_t p = 0; p < points.size(); p++) {
1113 images_file << " " << image_rgb(0) << " " << image_rgb(1) << " "
1114 << image_rgb(2) << "\n";
1115 }
1116
1117 images_file << "\n] }\n";
1118
1119 images_file << "coordIndex [\n";
1120 images_file << " 0, 1, 2, 3, -1\n";
1121 images_file << " 5, 6, 4, -1\n";
1122 images_file << " 6, 7, 4, -1\n";
1123 images_file << " 7, 8, 4, -1\n";
1124 images_file << " 8, 5, 4, -1\n";
1125 images_file << " \n] \n";
1126
1127 images_file << " texCoord TextureCoordinate { point [\n";
1128 images_file << " 1 1,\n";
1129 images_file << " 0 1,\n";
1130 images_file << " 0 0,\n";
1131 images_file << " 1 0,\n";
1132 images_file << " 0 0,\n";
1133 images_file << " 0 0,\n";
1134 images_file << " 0 0,\n";
1135 images_file << " 0 0,\n";
1136 images_file << " 0 0,\n";
1137
1138 images_file << " ] }\n";
1139 images_file << "} }\n";
1140 }
1141
1142 // Write 3D points
1143
1144 std::ofstream points3D_file(points3D_path, std::ios::trunc);
1145 CHECK(points3D_file.is_open()) << points3D_path;
1146
1147 points3D_file << "#VRML V2.0 utf8\n";
1148 points3D_file << "Background { skyColor [1.0 1.0 1.0] } \n";
1149 points3D_file << "Shape{ appearance Appearance {\n";
1150 points3D_file << " material Material {emissiveColor 1 1 1} }\n";
1151 points3D_file << " geometry PointSet {\n";
1152 points3D_file << " coord Coordinate {\n";
1153 points3D_file << " point [\n";
1154
1155 for (const auto& point3D : points3D_) {
1156 points3D_file << point3D.second.XYZ()(0) << ", ";
1157 points3D_file << point3D.second.XYZ()(1) << ", ";
1158 points3D_file << point3D.second.XYZ()(2) << std::endl;
1159 }
1160
1161 points3D_file << " ] }\n";
1162 points3D_file << " color Color { color [\n";
1163
1164 for (const auto& point3D : points3D_) {
1165 points3D_file << point3D.second.Color(0) / 255.0 << ", ";
1166 points3D_file << point3D.second.Color(1) / 255.0 << ", ";
1167 points3D_file << point3D.second.Color(2) / 255.0 << std::endl;
1168 }
1169
1170 points3D_file << " ] } } }\n";
1171 }
1172
ExtractColorsForImage(const image_t image_id,const std::string & path)1173 bool Reconstruction::ExtractColorsForImage(const image_t image_id,
1174 const std::string& path) {
1175 const class Image& image = Image(image_id);
1176
1177 Bitmap bitmap;
1178 if (!bitmap.Read(JoinPaths(path, image.Name()))) {
1179 return false;
1180 }
1181
1182 const Eigen::Vector3ub kBlackColor(0, 0, 0);
1183 for (const Point2D point2D : image.Points2D()) {
1184 if (point2D.HasPoint3D()) {
1185 class Point3D& point3D = Point3D(point2D.Point3DId());
1186 if (point3D.Color() == kBlackColor) {
1187 BitmapColor<float> color;
1188 // COLMAP assumes that the upper left pixel center is (0.5, 0.5).
1189 if (bitmap.InterpolateBilinear(point2D.X() - 0.5, point2D.Y() - 0.5,
1190 &color)) {
1191 const BitmapColor<uint8_t> color_ub = color.Cast<uint8_t>();
1192 point3D.SetColor(
1193 Eigen::Vector3ub(color_ub.r, color_ub.g, color_ub.b));
1194 }
1195 }
1196 }
1197 }
1198
1199 return true;
1200 }
1201
ExtractColorsForAllImages(const std::string & path)1202 void Reconstruction::ExtractColorsForAllImages(const std::string& path) {
1203 EIGEN_STL_UMAP(point3D_t, Eigen::Vector3d) color_sums;
1204 std::unordered_map<point3D_t, size_t> color_counts;
1205
1206 for (size_t i = 0; i < reg_image_ids_.size(); ++i) {
1207 const class Image& image = Image(reg_image_ids_[i]);
1208 const std::string image_path = JoinPaths(path, image.Name());
1209
1210 Bitmap bitmap;
1211 if (!bitmap.Read(image_path)) {
1212 std::cout << StringPrintf("Could not read image %s at path %s.",
1213 image.Name().c_str(), image_path.c_str())
1214 << std::endl;
1215 continue;
1216 }
1217
1218 for (const Point2D point2D : image.Points2D()) {
1219 if (point2D.HasPoint3D()) {
1220 BitmapColor<float> color;
1221 // COLMAP assumes that the upper left pixel center is (0.5, 0.5).
1222 if (bitmap.InterpolateBilinear(point2D.X() - 0.5, point2D.Y() - 0.5,
1223 &color)) {
1224 if (color_sums.count(point2D.Point3DId())) {
1225 Eigen::Vector3d& color_sum = color_sums[point2D.Point3DId()];
1226 color_sum(0) += color.r;
1227 color_sum(1) += color.g;
1228 color_sum(2) += color.b;
1229 color_counts[point2D.Point3DId()] += 1;
1230 } else {
1231 color_sums.emplace(point2D.Point3DId(),
1232 Eigen::Vector3d(color.r, color.g, color.b));
1233 color_counts.emplace(point2D.Point3DId(), 1);
1234 }
1235 }
1236 }
1237 }
1238 }
1239
1240 const Eigen::Vector3ub kBlackColor = Eigen::Vector3ub::Zero();
1241 for (auto& point3D : points3D_) {
1242 if (color_sums.count(point3D.first)) {
1243 Eigen::Vector3d color =
1244 color_sums[point3D.first] / color_counts[point3D.first];
1245 for (Eigen::Index i = 0; i < color.size(); ++i) {
1246 color[i] = std::round(color[i]);
1247 }
1248 point3D.second.SetColor(color.cast<uint8_t>());
1249 } else {
1250 point3D.second.SetColor(kBlackColor);
1251 }
1252 }
1253 }
1254
CreateImageDirs(const std::string & path) const1255 void Reconstruction::CreateImageDirs(const std::string& path) const {
1256 std::set<std::string> image_dirs;
1257 for (const auto& image : images_) {
1258 const std::vector<std::string> name_split =
1259 StringSplit(image.second.Name(), "/");
1260 if (name_split.size() > 1) {
1261 std::string dir = path;
1262 for (size_t i = 0; i < name_split.size() - 1; ++i) {
1263 dir = JoinPaths(dir, name_split[i]);
1264 image_dirs.insert(dir);
1265 }
1266 }
1267 }
1268 for (const auto& dir : image_dirs) {
1269 CreateDirIfNotExists(dir);
1270 }
1271 }
1272
FilterPoints3DWithSmallTriangulationAngle(const double min_tri_angle,const std::unordered_set<point3D_t> & point3D_ids)1273 size_t Reconstruction::FilterPoints3DWithSmallTriangulationAngle(
1274 const double min_tri_angle,
1275 const std::unordered_set<point3D_t>& point3D_ids) {
1276 // Number of filtered points.
1277 size_t num_filtered = 0;
1278
1279 // Minimum triangulation angle in radians.
1280 const double min_tri_angle_rad = DegToRad(min_tri_angle);
1281
1282 // Cache for image projection centers.
1283 EIGEN_STL_UMAP(image_t, Eigen::Vector3d) proj_centers;
1284
1285 for (const auto point3D_id : point3D_ids) {
1286 if (!ExistsPoint3D(point3D_id)) {
1287 continue;
1288 }
1289
1290 const class Point3D& point3D = Point3D(point3D_id);
1291
1292 // Calculate triangulation angle for all pairwise combinations of image
1293 // poses in the track. Only delete point if none of the combinations
1294 // has a sufficient triangulation angle.
1295 bool keep_point = false;
1296 for (size_t i1 = 0; i1 < point3D.Track().Length(); ++i1) {
1297 const image_t image_id1 = point3D.Track().Element(i1).image_id;
1298
1299 Eigen::Vector3d proj_center1;
1300 if (proj_centers.count(image_id1) == 0) {
1301 const class Image& image1 = Image(image_id1);
1302 proj_center1 = image1.ProjectionCenter();
1303 proj_centers.emplace(image_id1, proj_center1);
1304 } else {
1305 proj_center1 = proj_centers.at(image_id1);
1306 }
1307
1308 for (size_t i2 = 0; i2 < i1; ++i2) {
1309 const image_t image_id2 = point3D.Track().Element(i2).image_id;
1310 const Eigen::Vector3d proj_center2 = proj_centers.at(image_id2);
1311
1312 const double tri_angle = CalculateTriangulationAngle(
1313 proj_center1, proj_center2, point3D.XYZ());
1314
1315 if (tri_angle >= min_tri_angle_rad) {
1316 keep_point = true;
1317 break;
1318 }
1319 }
1320
1321 if (keep_point) {
1322 break;
1323 }
1324 }
1325
1326 if (!keep_point) {
1327 num_filtered += 1;
1328 DeletePoint3D(point3D_id);
1329 }
1330 }
1331
1332 return num_filtered;
1333 }
1334
FilterPoints3DWithLargeReprojectionError(const double max_reproj_error,const std::unordered_set<point3D_t> & point3D_ids)1335 size_t Reconstruction::FilterPoints3DWithLargeReprojectionError(
1336 const double max_reproj_error,
1337 const std::unordered_set<point3D_t>& point3D_ids) {
1338 const double max_squared_reproj_error = max_reproj_error * max_reproj_error;
1339
1340 // Number of filtered points.
1341 size_t num_filtered = 0;
1342
1343 for (const auto point3D_id : point3D_ids) {
1344 if (!ExistsPoint3D(point3D_id)) {
1345 continue;
1346 }
1347
1348 class Point3D& point3D = Point3D(point3D_id);
1349
1350 if (point3D.Track().Length() < 2) {
1351 DeletePoint3D(point3D_id);
1352 num_filtered += point3D.Track().Length();
1353 continue;
1354 }
1355
1356 double reproj_error_sum = 0.0;
1357
1358 std::vector<TrackElement> track_els_to_delete;
1359
1360 for (const auto& track_el : point3D.Track().Elements()) {
1361 const class Image& image = Image(track_el.image_id);
1362 const class Camera& camera = Camera(image.CameraId());
1363 const Point2D& point2D = image.Point2D(track_el.point2D_idx);
1364 const double squared_reproj_error = CalculateSquaredReprojectionError(
1365 point2D.XY(), point3D.XYZ(), image.Qvec(), image.Tvec(), camera);
1366 if (squared_reproj_error > max_squared_reproj_error) {
1367 track_els_to_delete.push_back(track_el);
1368 } else {
1369 reproj_error_sum += std::sqrt(squared_reproj_error);
1370 }
1371 }
1372
1373 if (track_els_to_delete.size() >= point3D.Track().Length() - 1) {
1374 num_filtered += point3D.Track().Length();
1375 DeletePoint3D(point3D_id);
1376 } else {
1377 num_filtered += track_els_to_delete.size();
1378 for (const auto& track_el : track_els_to_delete) {
1379 DeleteObservation(track_el.image_id, track_el.point2D_idx);
1380 }
1381 point3D.SetError(reproj_error_sum / point3D.Track().Length());
1382 }
1383 }
1384
1385 return num_filtered;
1386 }
1387
ReadCamerasText(const std::string & path)1388 void Reconstruction::ReadCamerasText(const std::string& path) {
1389 cameras_.clear();
1390
1391 std::ifstream file(path);
1392 CHECK(file.is_open()) << path;
1393
1394 std::string line;
1395 std::string item;
1396
1397 while (std::getline(file, line)) {
1398 StringTrim(&line);
1399
1400 if (line.empty() || line[0] == '#') {
1401 continue;
1402 }
1403
1404 std::stringstream line_stream(line);
1405
1406 class Camera camera;
1407
1408 // ID
1409 std::getline(line_stream, item, ' ');
1410 camera.SetCameraId(std::stoul(item));
1411
1412 // MODEL
1413 std::getline(line_stream, item, ' ');
1414 camera.SetModelIdFromName(item);
1415
1416 // WIDTH
1417 std::getline(line_stream, item, ' ');
1418 camera.SetWidth(std::stoll(item));
1419
1420 // HEIGHT
1421 std::getline(line_stream, item, ' ');
1422 camera.SetHeight(std::stoll(item));
1423
1424 // PARAMS
1425 camera.Params().clear();
1426 while (!line_stream.eof()) {
1427 std::getline(line_stream, item, ' ');
1428 camera.Params().push_back(std::stold(item));
1429 }
1430
1431 CHECK(camera.VerifyParams());
1432
1433 cameras_.emplace(camera.CameraId(), camera);
1434 }
1435 }
1436
ReadImagesText(const std::string & path)1437 void Reconstruction::ReadImagesText(const std::string& path) {
1438 images_.clear();
1439
1440 std::ifstream file(path);
1441 CHECK(file.is_open()) << path;
1442
1443 std::string line;
1444 std::string item;
1445
1446 while (std::getline(file, line)) {
1447 StringTrim(&line);
1448
1449 if (line.empty() || line[0] == '#') {
1450 continue;
1451 }
1452
1453 std::stringstream line_stream1(line);
1454
1455 // ID
1456 std::getline(line_stream1, item, ' ');
1457 const image_t image_id = std::stoul(item);
1458
1459 class Image image;
1460 image.SetImageId(image_id);
1461
1462 image.SetRegistered(true);
1463 reg_image_ids_.push_back(image_id);
1464
1465 // QVEC (qw, qx, qy, qz)
1466 std::getline(line_stream1, item, ' ');
1467 image.Qvec(0) = std::stold(item);
1468
1469 std::getline(line_stream1, item, ' ');
1470 image.Qvec(1) = std::stold(item);
1471
1472 std::getline(line_stream1, item, ' ');
1473 image.Qvec(2) = std::stold(item);
1474
1475 std::getline(line_stream1, item, ' ');
1476 image.Qvec(3) = std::stold(item);
1477
1478 image.NormalizeQvec();
1479
1480 // TVEC
1481 std::getline(line_stream1, item, ' ');
1482 image.Tvec(0) = std::stold(item);
1483
1484 std::getline(line_stream1, item, ' ');
1485 image.Tvec(1) = std::stold(item);
1486
1487 std::getline(line_stream1, item, ' ');
1488 image.Tvec(2) = std::stold(item);
1489
1490 // CAMERA_ID
1491 std::getline(line_stream1, item, ' ');
1492 image.SetCameraId(std::stoul(item));
1493
1494 // NAME
1495 std::getline(line_stream1, item, ' ');
1496 image.SetName(item);
1497
1498 // POINTS2D
1499 if (!std::getline(file, line)) {
1500 break;
1501 }
1502
1503 StringTrim(&line);
1504 std::stringstream line_stream2(line);
1505
1506 std::vector<Eigen::Vector2d> points2D;
1507 std::vector<point3D_t> point3D_ids;
1508
1509 if (!line.empty()) {
1510 while (!line_stream2.eof()) {
1511 Eigen::Vector2d point;
1512
1513 std::getline(line_stream2, item, ' ');
1514 point.x() = std::stold(item);
1515
1516 std::getline(line_stream2, item, ' ');
1517 point.y() = std::stold(item);
1518
1519 points2D.push_back(point);
1520
1521 std::getline(line_stream2, item, ' ');
1522 if (item == "-1") {
1523 point3D_ids.push_back(kInvalidPoint3DId);
1524 } else {
1525 point3D_ids.push_back(std::stoll(item));
1526 }
1527 }
1528 }
1529
1530 image.SetUp(Camera(image.CameraId()));
1531 image.SetPoints2D(points2D);
1532
1533 for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
1534 ++point2D_idx) {
1535 if (point3D_ids[point2D_idx] != kInvalidPoint3DId) {
1536 image.SetPoint3DForPoint2D(point2D_idx, point3D_ids[point2D_idx]);
1537 }
1538 }
1539
1540 images_.emplace(image.ImageId(), image);
1541 }
1542 }
1543
ReadPoints3DText(const std::string & path)1544 void Reconstruction::ReadPoints3DText(const std::string& path) {
1545 points3D_.clear();
1546
1547 std::ifstream file(path);
1548 CHECK(file.is_open()) << path;
1549
1550 std::string line;
1551 std::string item;
1552
1553 while (std::getline(file, line)) {
1554 StringTrim(&line);
1555
1556 if (line.empty() || line[0] == '#') {
1557 continue;
1558 }
1559
1560 std::stringstream line_stream(line);
1561
1562 // ID
1563 std::getline(line_stream, item, ' ');
1564 const point3D_t point3D_id = std::stoll(item);
1565
1566 // Make sure, that we can add new 3D points after reading 3D points
1567 // without overwriting existing 3D points.
1568 num_added_points3D_ = std::max(num_added_points3D_, point3D_id);
1569
1570 class Point3D point3D;
1571
1572 // XYZ
1573 std::getline(line_stream, item, ' ');
1574 point3D.XYZ(0) = std::stold(item);
1575
1576 std::getline(line_stream, item, ' ');
1577 point3D.XYZ(1) = std::stold(item);
1578
1579 std::getline(line_stream, item, ' ');
1580 point3D.XYZ(2) = std::stold(item);
1581
1582 // Color
1583 std::getline(line_stream, item, ' ');
1584 point3D.Color(0) = static_cast<uint8_t>(std::stoi(item));
1585
1586 std::getline(line_stream, item, ' ');
1587 point3D.Color(1) = static_cast<uint8_t>(std::stoi(item));
1588
1589 std::getline(line_stream, item, ' ');
1590 point3D.Color(2) = static_cast<uint8_t>(std::stoi(item));
1591
1592 // ERROR
1593 std::getline(line_stream, item, ' ');
1594 point3D.SetError(std::stold(item));
1595
1596 // TRACK
1597 while (!line_stream.eof()) {
1598 TrackElement track_el;
1599
1600 std::getline(line_stream, item, ' ');
1601 StringTrim(&item);
1602 if (item.empty()) {
1603 break;
1604 }
1605 track_el.image_id = std::stoul(item);
1606
1607 std::getline(line_stream, item, ' ');
1608 track_el.point2D_idx = std::stoul(item);
1609
1610 point3D.Track().AddElement(track_el);
1611 }
1612
1613 point3D.Track().Compress();
1614
1615 points3D_.emplace(point3D_id, point3D);
1616 }
1617 }
1618
ReadCamerasBinary(const std::string & path)1619 void Reconstruction::ReadCamerasBinary(const std::string& path) {
1620 std::ifstream file(path, std::ios::binary);
1621 CHECK(file.is_open()) << path;
1622
1623 const size_t num_cameras = ReadBinaryLittleEndian<uint64_t>(&file);
1624 for (size_t i = 0; i < num_cameras; ++i) {
1625 class Camera camera;
1626 camera.SetCameraId(ReadBinaryLittleEndian<camera_t>(&file));
1627 camera.SetModelId(ReadBinaryLittleEndian<int>(&file));
1628 camera.SetWidth(ReadBinaryLittleEndian<uint64_t>(&file));
1629 camera.SetHeight(ReadBinaryLittleEndian<uint64_t>(&file));
1630 ReadBinaryLittleEndian<double>(&file, &camera.Params());
1631 CHECK(camera.VerifyParams());
1632 cameras_.emplace(camera.CameraId(), camera);
1633 }
1634 }
1635
ReadImagesBinary(const std::string & path)1636 void Reconstruction::ReadImagesBinary(const std::string& path) {
1637 std::ifstream file(path, std::ios::binary);
1638 CHECK(file.is_open()) << path;
1639
1640 const size_t num_reg_images = ReadBinaryLittleEndian<uint64_t>(&file);
1641 for (size_t i = 0; i < num_reg_images; ++i) {
1642 class Image image;
1643
1644 image.SetImageId(ReadBinaryLittleEndian<image_t>(&file));
1645
1646 image.Qvec(0) = ReadBinaryLittleEndian<double>(&file);
1647 image.Qvec(1) = ReadBinaryLittleEndian<double>(&file);
1648 image.Qvec(2) = ReadBinaryLittleEndian<double>(&file);
1649 image.Qvec(3) = ReadBinaryLittleEndian<double>(&file);
1650 image.NormalizeQvec();
1651
1652 image.Tvec(0) = ReadBinaryLittleEndian<double>(&file);
1653 image.Tvec(1) = ReadBinaryLittleEndian<double>(&file);
1654 image.Tvec(2) = ReadBinaryLittleEndian<double>(&file);
1655
1656 image.SetCameraId(ReadBinaryLittleEndian<camera_t>(&file));
1657
1658 char name_char;
1659 do {
1660 file.read(&name_char, 1);
1661 if (name_char != '\0') {
1662 image.Name() += name_char;
1663 }
1664 } while (name_char != '\0');
1665
1666 const size_t num_points2D = ReadBinaryLittleEndian<uint64_t>(&file);
1667
1668 std::vector<Eigen::Vector2d> points2D;
1669 points2D.reserve(num_points2D);
1670 std::vector<point3D_t> point3D_ids;
1671 point3D_ids.reserve(num_points2D);
1672 for (size_t j = 0; j < num_points2D; ++j) {
1673 const double x = ReadBinaryLittleEndian<double>(&file);
1674 const double y = ReadBinaryLittleEndian<double>(&file);
1675 points2D.emplace_back(x, y);
1676 point3D_ids.push_back(ReadBinaryLittleEndian<point3D_t>(&file));
1677 }
1678
1679 image.SetUp(Camera(image.CameraId()));
1680 image.SetPoints2D(points2D);
1681
1682 for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
1683 ++point2D_idx) {
1684 if (point3D_ids[point2D_idx] != kInvalidPoint3DId) {
1685 image.SetPoint3DForPoint2D(point2D_idx, point3D_ids[point2D_idx]);
1686 }
1687 }
1688
1689 image.SetRegistered(true);
1690 reg_image_ids_.push_back(image.ImageId());
1691
1692 images_.emplace(image.ImageId(), image);
1693 }
1694 }
1695
ReadPoints3DBinary(const std::string & path)1696 void Reconstruction::ReadPoints3DBinary(const std::string& path) {
1697 std::ifstream file(path, std::ios::binary);
1698 CHECK(file.is_open()) << path;
1699
1700 const size_t num_points3D = ReadBinaryLittleEndian<uint64_t>(&file);
1701 for (size_t i = 0; i < num_points3D; ++i) {
1702 class Point3D point3D;
1703
1704 const point3D_t point3D_id = ReadBinaryLittleEndian<point3D_t>(&file);
1705 num_added_points3D_ = std::max(num_added_points3D_, point3D_id);
1706
1707 point3D.XYZ()(0) = ReadBinaryLittleEndian<double>(&file);
1708 point3D.XYZ()(1) = ReadBinaryLittleEndian<double>(&file);
1709 point3D.XYZ()(2) = ReadBinaryLittleEndian<double>(&file);
1710 point3D.Color(0) = ReadBinaryLittleEndian<uint8_t>(&file);
1711 point3D.Color(1) = ReadBinaryLittleEndian<uint8_t>(&file);
1712 point3D.Color(2) = ReadBinaryLittleEndian<uint8_t>(&file);
1713 point3D.SetError(ReadBinaryLittleEndian<double>(&file));
1714
1715 const size_t track_length = ReadBinaryLittleEndian<uint64_t>(&file);
1716 for (size_t j = 0; j < track_length; ++j) {
1717 const image_t image_id = ReadBinaryLittleEndian<image_t>(&file);
1718 const point2D_t point2D_idx = ReadBinaryLittleEndian<point2D_t>(&file);
1719 point3D.Track().AddElement(image_id, point2D_idx);
1720 }
1721 point3D.Track().Compress();
1722
1723 points3D_.emplace(point3D_id, point3D);
1724 }
1725 }
1726
WriteCamerasText(const std::string & path) const1727 void Reconstruction::WriteCamerasText(const std::string& path) const {
1728 std::ofstream file(path, std::ios::trunc);
1729 CHECK(file.is_open()) << path;
1730
1731 // Ensure that we don't loose any precision by storing in text.
1732 file.precision(17);
1733
1734 file << "# Camera list with one line of data per camera:" << std::endl;
1735 file << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]" << std::endl;
1736 file << "# Number of cameras: " << cameras_.size() << std::endl;
1737
1738 for (const auto& camera : cameras_) {
1739 std::ostringstream line;
1740
1741 line << camera.first << " ";
1742 line << camera.second.ModelName() << " ";
1743 line << camera.second.Width() << " ";
1744 line << camera.second.Height() << " ";
1745
1746 for (const double param : camera.second.Params()) {
1747 line << param << " ";
1748 }
1749
1750 std::string line_string = line.str();
1751 line_string = line_string.substr(0, line_string.size() - 1);
1752
1753 file << line_string << std::endl;
1754 }
1755 }
1756
WriteImagesText(const std::string & path) const1757 void Reconstruction::WriteImagesText(const std::string& path) const {
1758 std::ofstream file(path, std::ios::trunc);
1759 CHECK(file.is_open()) << path;
1760
1761 // Ensure that we don't loose any precision by storing in text.
1762 file.precision(17);
1763
1764 file << "# Image list with two lines of data per image:" << std::endl;
1765 file << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, "
1766 "NAME"
1767 << std::endl;
1768 file << "# POINTS2D[] as (X, Y, POINT3D_ID)" << std::endl;
1769 file << "# Number of images: " << reg_image_ids_.size()
1770 << ", mean observations per image: "
1771 << ComputeMeanObservationsPerRegImage() << std::endl;
1772
1773 for (const auto& image : images_) {
1774 if (!image.second.IsRegistered()) {
1775 continue;
1776 }
1777
1778 std::ostringstream line;
1779 std::string line_string;
1780
1781 line << image.first << " ";
1782
1783 // QVEC (qw, qx, qy, qz)
1784 const Eigen::Vector4d normalized_qvec =
1785 NormalizeQuaternion(image.second.Qvec());
1786 line << normalized_qvec(0) << " ";
1787 line << normalized_qvec(1) << " ";
1788 line << normalized_qvec(2) << " ";
1789 line << normalized_qvec(3) << " ";
1790
1791 // TVEC
1792 line << image.second.Tvec(0) << " ";
1793 line << image.second.Tvec(1) << " ";
1794 line << image.second.Tvec(2) << " ";
1795
1796 line << image.second.CameraId() << " ";
1797
1798 line << image.second.Name();
1799
1800 file << line.str() << std::endl;
1801
1802 line.str("");
1803 line.clear();
1804
1805 for (const Point2D& point2D : image.second.Points2D()) {
1806 line << point2D.X() << " ";
1807 line << point2D.Y() << " ";
1808 if (point2D.HasPoint3D()) {
1809 line << point2D.Point3DId() << " ";
1810 } else {
1811 line << -1 << " ";
1812 }
1813 }
1814 line_string = line.str();
1815 line_string = line_string.substr(0, line_string.size() - 1);
1816 file << line_string << std::endl;
1817 }
1818 }
1819
WritePoints3DText(const std::string & path) const1820 void Reconstruction::WritePoints3DText(const std::string& path) const {
1821 std::ofstream file(path, std::ios::trunc);
1822 CHECK(file.is_open()) << path;
1823
1824 // Ensure that we don't loose any precision by storing in text.
1825 file.precision(17);
1826
1827 file << "# 3D point list with one line of data per point:" << std::endl;
1828 file << "# POINT3D_ID, X, Y, Z, R, G, B, ERROR, "
1829 "TRACK[] as (IMAGE_ID, POINT2D_IDX)"
1830 << std::endl;
1831 file << "# Number of points: " << points3D_.size()
1832 << ", mean track length: " << ComputeMeanTrackLength() << std::endl;
1833
1834 for (const auto& point3D : points3D_) {
1835 file << point3D.first << " ";
1836 file << point3D.second.XYZ()(0) << " ";
1837 file << point3D.second.XYZ()(1) << " ";
1838 file << point3D.second.XYZ()(2) << " ";
1839 file << static_cast<int>(point3D.second.Color(0)) << " ";
1840 file << static_cast<int>(point3D.second.Color(1)) << " ";
1841 file << static_cast<int>(point3D.second.Color(2)) << " ";
1842 file << point3D.second.Error() << " ";
1843
1844 std::ostringstream line;
1845
1846 for (const auto& track_el : point3D.second.Track().Elements()) {
1847 line << track_el.image_id << " ";
1848 line << track_el.point2D_idx << " ";
1849 }
1850
1851 std::string line_string = line.str();
1852 line_string = line_string.substr(0, line_string.size() - 1);
1853
1854 file << line_string << std::endl;
1855 }
1856 }
1857
WriteCamerasBinary(const std::string & path) const1858 void Reconstruction::WriteCamerasBinary(const std::string& path) const {
1859 std::ofstream file(path, std::ios::trunc | std::ios::binary);
1860 CHECK(file.is_open()) << path;
1861
1862 WriteBinaryLittleEndian<uint64_t>(&file, cameras_.size());
1863
1864 for (const auto& camera : cameras_) {
1865 WriteBinaryLittleEndian<camera_t>(&file, camera.first);
1866 WriteBinaryLittleEndian<int>(&file, camera.second.ModelId());
1867 WriteBinaryLittleEndian<uint64_t>(&file, camera.second.Width());
1868 WriteBinaryLittleEndian<uint64_t>(&file, camera.second.Height());
1869 for (const double param : camera.second.Params()) {
1870 WriteBinaryLittleEndian<double>(&file, param);
1871 }
1872 }
1873 }
1874
WriteImagesBinary(const std::string & path) const1875 void Reconstruction::WriteImagesBinary(const std::string& path) const {
1876 std::ofstream file(path, std::ios::trunc | std::ios::binary);
1877 CHECK(file.is_open()) << path;
1878
1879 WriteBinaryLittleEndian<uint64_t>(&file, reg_image_ids_.size());
1880
1881 for (const auto& image : images_) {
1882 if (!image.second.IsRegistered()) {
1883 continue;
1884 }
1885
1886 WriteBinaryLittleEndian<image_t>(&file, image.first);
1887
1888 const Eigen::Vector4d normalized_qvec =
1889 NormalizeQuaternion(image.second.Qvec());
1890 WriteBinaryLittleEndian<double>(&file, normalized_qvec(0));
1891 WriteBinaryLittleEndian<double>(&file, normalized_qvec(1));
1892 WriteBinaryLittleEndian<double>(&file, normalized_qvec(2));
1893 WriteBinaryLittleEndian<double>(&file, normalized_qvec(3));
1894
1895 WriteBinaryLittleEndian<double>(&file, image.second.Tvec(0));
1896 WriteBinaryLittleEndian<double>(&file, image.second.Tvec(1));
1897 WriteBinaryLittleEndian<double>(&file, image.second.Tvec(2));
1898
1899 WriteBinaryLittleEndian<camera_t>(&file, image.second.CameraId());
1900
1901 const std::string name = image.second.Name() + '\0';
1902 file.write(name.c_str(), name.size());
1903
1904 WriteBinaryLittleEndian<uint64_t>(&file, image.second.NumPoints2D());
1905 for (const Point2D& point2D : image.second.Points2D()) {
1906 WriteBinaryLittleEndian<double>(&file, point2D.X());
1907 WriteBinaryLittleEndian<double>(&file, point2D.Y());
1908 WriteBinaryLittleEndian<point3D_t>(&file, point2D.Point3DId());
1909 }
1910 }
1911 }
1912
WritePoints3DBinary(const std::string & path) const1913 void Reconstruction::WritePoints3DBinary(const std::string& path) const {
1914 std::ofstream file(path, std::ios::trunc | std::ios::binary);
1915 CHECK(file.is_open()) << path;
1916
1917 WriteBinaryLittleEndian<uint64_t>(&file, points3D_.size());
1918
1919 for (const auto& point3D : points3D_) {
1920 WriteBinaryLittleEndian<point3D_t>(&file, point3D.first);
1921 WriteBinaryLittleEndian<double>(&file, point3D.second.XYZ()(0));
1922 WriteBinaryLittleEndian<double>(&file, point3D.second.XYZ()(1));
1923 WriteBinaryLittleEndian<double>(&file, point3D.second.XYZ()(2));
1924 WriteBinaryLittleEndian<uint8_t>(&file, point3D.second.Color(0));
1925 WriteBinaryLittleEndian<uint8_t>(&file, point3D.second.Color(1));
1926 WriteBinaryLittleEndian<uint8_t>(&file, point3D.second.Color(2));
1927 WriteBinaryLittleEndian<double>(&file, point3D.second.Error());
1928
1929 WriteBinaryLittleEndian<uint64_t>(&file, point3D.second.Track().Length());
1930 for (const auto& track_el : point3D.second.Track().Elements()) {
1931 WriteBinaryLittleEndian<image_t>(&file, track_el.image_id);
1932 WriteBinaryLittleEndian<point2D_t>(&file, track_el.point2D_idx);
1933 }
1934 }
1935 }
1936
SetObservationAsTriangulated(const image_t image_id,const point2D_t point2D_idx,const bool is_continued_point3D)1937 void Reconstruction::SetObservationAsTriangulated(
1938 const image_t image_id, const point2D_t point2D_idx,
1939 const bool is_continued_point3D) {
1940 if (correspondence_graph_ == nullptr) {
1941 return;
1942 }
1943
1944 const class Image& image = Image(image_id);
1945 const Point2D& point2D = image.Point2D(point2D_idx);
1946 const std::vector<CorrespondenceGraph::Correspondence>& corrs =
1947 correspondence_graph_->FindCorrespondences(image_id, point2D_idx);
1948
1949 CHECK(image.IsRegistered());
1950 CHECK(point2D.HasPoint3D());
1951
1952 for (const auto& corr : corrs) {
1953 class Image& corr_image = Image(corr.image_id);
1954 const Point2D& corr_point2D = corr_image.Point2D(corr.point2D_idx);
1955 corr_image.IncrementCorrespondenceHasPoint3D(corr.point2D_idx);
1956 // Update number of shared 3D points between image pairs and make sure to
1957 // only count the correspondences once (not twice forward and backward).
1958 if (point2D.Point3DId() == corr_point2D.Point3DId() &&
1959 (is_continued_point3D || image_id < corr.image_id)) {
1960 const image_pair_t pair_id =
1961 Database::ImagePairToPairId(image_id, corr.image_id);
1962 image_pair_stats_[pair_id].num_tri_corrs += 1;
1963 CHECK_LE(image_pair_stats_[pair_id].num_tri_corrs,
1964 image_pair_stats_[pair_id].num_total_corrs)
1965 << "The correspondence graph graph must not contain duplicate "
1966 "matches";
1967 }
1968 }
1969 }
1970
ResetTriObservations(const image_t image_id,const point2D_t point2D_idx,const bool is_deleted_point3D)1971 void Reconstruction::ResetTriObservations(const image_t image_id,
1972 const point2D_t point2D_idx,
1973 const bool is_deleted_point3D) {
1974 if (correspondence_graph_ == nullptr) {
1975 return;
1976 }
1977
1978 const class Image& image = Image(image_id);
1979 const Point2D& point2D = image.Point2D(point2D_idx);
1980 const std::vector<CorrespondenceGraph::Correspondence>& corrs =
1981 correspondence_graph_->FindCorrespondences(image_id, point2D_idx);
1982
1983 CHECK(image.IsRegistered());
1984 CHECK(point2D.HasPoint3D());
1985
1986 for (const auto& corr : corrs) {
1987 class Image& corr_image = Image(corr.image_id);
1988 const Point2D& corr_point2D = corr_image.Point2D(corr.point2D_idx);
1989 corr_image.DecrementCorrespondenceHasPoint3D(corr.point2D_idx);
1990 // Update number of shared 3D points between image pairs and make sure to
1991 // only count the correspondences once (not twice forward and backward).
1992 if (point2D.Point3DId() == corr_point2D.Point3DId() &&
1993 (!is_deleted_point3D || image_id < corr.image_id)) {
1994 const image_pair_t pair_id =
1995 Database::ImagePairToPairId(image_id, corr.image_id);
1996 image_pair_stats_[pair_id].num_tri_corrs -= 1;
1997 CHECK_GE(image_pair_stats_[pair_id].num_tri_corrs, 0)
1998 << "The scene graph graph must not contain duplicate matches";
1999 }
2000 }
2001 }
2002
2003 } // namespace colmap
2004