MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
bundler_incremental.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#include <limits>
11#include <iostream>
12#include <utility>
13
14#include "util/timer.h"
15#include "math/transform.h"
16#include "math/matrix_svd.h"
17#include "math/matrix_tools.h"
18#include "sfm/triangulate.h"
21
24
25void
26Incremental::initialize (ViewportList* viewports, TrackList* tracks,
27 SurveyPointList* survey_points)
28{
29 this->viewports = viewports;
30 this->tracks = tracks;
31 this->survey_points = survey_points;
32
33 if (this->viewports->empty())
34 throw std::invalid_argument("No viewports given");
35
36 /* Check if at least two cameras are initialized. */
37 std::size_t num_valid_cameras = 0;
38 for (std::size_t i = 0; i < this->viewports->size(); ++i)
39 if (this->viewports->at(i).pose.is_valid())
40 num_valid_cameras += 1;
41 if (num_valid_cameras < 2)
42 throw std::invalid_argument("Two or more valid cameras required");
43
44 /* Set track positions to invalid state. */
45 for (std::size_t i = 0; i < tracks->size(); ++i)
46 {
47 Track& track = tracks->at(i);
48 track.invalidate();
49 }
50}
51
52/* ---------------------------------------------------------------- */
53
54void
55Incremental::find_next_views (std::vector<int>* next_views)
56{
57 /* Create mapping from valid tracks to view ID. */
58 std::vector<std::pair<int, int> > valid_tracks(this->viewports->size());
59 for (std::size_t i = 0; i < valid_tracks.size(); ++i)
60 valid_tracks[i] = std::make_pair(0, static_cast<int>(i));
61
62 for (std::size_t i = 0; i < this->tracks->size(); ++i)
63 {
64 Track const& track = this->tracks->at(i);
65 if (!track.is_valid())
66 continue;
67
68 for (std::size_t j = 0; j < track.features.size(); ++j)
69 {
70 int const view_id = track.features[j].view_id;
71 if (this->viewports->at(view_id).pose.is_valid())
72 continue;
73 valid_tracks[view_id].first += 1;
74 }
75 }
76
77 /* Sort descending by number of valid tracks. */
78 std::sort(valid_tracks.rbegin(), valid_tracks.rend());
79
80 /* Return unreconstructed views with most valid tracks. */
81 next_views->clear();
82 for (std::size_t i = 0; i < valid_tracks.size(); ++i)
83 {
84 if (valid_tracks[i].first > 6)
85 next_views->push_back(valid_tracks[i].second);
86 }
87}
88
89/* ---------------------------------------------------------------- */
90
91bool
92Incremental::reconstruct_next_view (int view_id)
93{
94 Viewport const& viewport = this->viewports->at(view_id);
95 FeatureSet const& features = viewport.features;
96
97 /* Collect all 2D-3D correspondences. */
99 std::vector<int> track_ids;
100 std::vector<int> feature_ids;
101 for (std::size_t i = 0; i < viewport.track_ids.size(); ++i)
102 {
103 int const track_id = viewport.track_ids[i];
104 if (track_id < 0 || !this->tracks->at(track_id).is_valid())
105 continue;
106 math::Vec2f const& pos2d = features.positions[i];
107 math::Vec3f const& pos3d = this->tracks->at(track_id).pos;
108
109 corr.push_back(Correspondence2D3D());
110 Correspondence2D3D& c = corr.back();
111 std::copy(pos3d.begin(), pos3d.end(), c.p3d);
112 std::copy(pos2d.begin(), pos2d.end(), c.p2d);
113 track_ids.push_back(track_id);
114 feature_ids.push_back(i);
115 }
116
117 if (this->opts.verbose_output)
118 {
119 std::cout << "Collected " << corr.size()
120 << " 2D-3D correspondences." << std::endl;
121 }
122
123 /* Initialize a temporary camera. */
124 CameraPose temp_camera;
125 temp_camera.set_k_matrix(viewport.focal_length, 0.0, 0.0);
126
127 /* Compute pose from 2D-3D correspondences using P3P. */
128 util::WallTimer timer;
129 RansacPoseP3P::Result ransac_result;
130 {
131 RansacPoseP3P ransac(this->opts.pose_p3p_opts);
132 ransac.estimate(corr, temp_camera.K, &ransac_result);
133 }
134
135 /* Cancel if inliers are below a 33% threshold. */
136 if (3 * ransac_result.inliers.size() < corr.size())
137 {
138 if (this->opts.verbose_output)
139 std::cout << "Only " << ransac_result.inliers.size()
140 << " 2D-3D correspondences inliers ("
141 << (100 * ransac_result.inliers.size() / corr.size())
142 << "%). Skipping view." << std::endl;
143 return false;
144 }
145 else if (this->opts.verbose_output)
146 {
147 std::cout << "Selected " << ransac_result.inliers.size()
148 << " 2D-3D correspondences inliers ("
149 << (100 * ransac_result.inliers.size() / corr.size())
150 << "%), took " << timer.get_elapsed() << "ms." << std::endl;
151 }
152
153 /*
154 * Remove outliers from tracks and tracks from viewport.
155 * Once single cam BA has been performed and parameters for this
156 * camera are optimized, tracks are evaluated again and restored.
157 */
158 for (std::size_t i = 0; i < ransac_result.inliers.size(); ++i)
159 track_ids[ransac_result.inliers[i]] = -1;
160 for (std::size_t i = 0; i < track_ids.size(); ++i)
161 {
162 if (track_ids[i] < 0)
163 continue;
164 this->tracks->at(track_ids[i]).remove_view(view_id);
165 this->viewports->at(view_id).track_ids[feature_ids[i]] = -1;
166 this->viewports->at(view_id).backup_tracks.emplace(
167 feature_ids[i], track_ids[i]);
168 }
169 track_ids.clear();
170 feature_ids.clear();
171
172 /* Commit camera using known K and computed R and t. */
173 {
174 CameraPose& pose = this->viewports->at(view_id).pose;
175 pose = temp_camera;
176 pose.R = ransac_result.pose.delete_col(3);
177 pose.t = ransac_result.pose.col(3);
178
179 if (this->opts.verbose_output)
180 {
181 std::cout << "Reconstructed camera "
182 << view_id << " with focal length "
183 << pose.get_focal_length() << std::endl;
184 }
185 }
186
187 if (this->survey_points != nullptr && !registered)
188 this->try_registration();
189
190 return true;
191}
192
193void
194Incremental::try_restore_tracks_for_views (void)
195{
196 for (std::size_t i = 0; i < this->viewports->size(); ++i)
197 {
198 Viewport& viewport = this->viewports->at(i);
199 if (!viewport.pose.is_valid())
200 continue;
201 FeatureSet const& features = viewport.features;
203 viewport.pose.fill_p_matrix(&P);
204 for (auto const& feature_track : viewport.backup_tracks)
205 {
206 int const feature_id = feature_track.first;
207 int const track_id = feature_track.second;
208 if (track_id < 0 || !this->tracks->at(track_id).is_valid()
209 || viewport.track_ids[feature_id] >= 0)
210 continue;
211 math::Vec3f const& pos3d = this->tracks->at(track_id).pos;
212 math::Vec2f const& pos2d = undistort_feature(
213 features.positions[feature_id],
214 static_cast<double>(viewport.radial_distortion[0]),
215 static_cast<double>(viewport.radial_distortion[1]),
216 viewport.focal_length);
217
218 math::Vec4d const pos3dh(pos3d[0], pos3d[1], pos3d[2], 1.0);
219 math::Vec3d proj = P * pos3dh;
220 math::Vec2f cam_point(proj[0] / proj[2], proj[1] / proj[2]);
221 if ((cam_point - pos2d).norm()
222 < this->opts.new_track_error_threshold)
223 {
224 viewport.track_ids[feature_id] = track_id;
225 this->tracks->at(track_id).features.emplace_back(i, feature_id);
226 }
227 }
228 }
229}
230
231/* ---------------------------------------------------------------- */
232
233void
234Incremental::try_registration () {
235 std::vector<math::Vec3d> p0;
236 std::vector<math::Vec3d> p1;
237
238 for (std::size_t i = 0; i < this->survey_points->size(); ++i)
239 {
240 SurveyPoint const& survey_point = this->survey_points->at(i);
241
242 std::vector<math::Vec2f> pos;
243 std::vector<CameraPose const*> poses;
244 for (std::size_t j = 0; j < survey_point.observations.size(); ++j)
245 {
246 SurveyObservation const& obs = survey_point.observations[j];
247 int const view_id = obs.view_id;
248 if (!this->viewports->at(view_id).pose.is_valid())
249 continue;
250
251 pos.push_back(obs.pos);
252 poses.push_back(&this->viewports->at(view_id).pose);
253 }
254
255 if (pos.size() < 2)
256 continue;
257
258 p0.push_back(triangulate_track(pos, poses));
259 p1.push_back(survey_point.pos);
260 }
261
262 if (p0.size() < 3)
263 return;
264
265 /* Determine transformation. */
267 double s;
268 math::Vec3d t;
269 if (!math::determine_transform(p0, p1, &R, &s, &t))
270 return;
271
272 /* Transform every camera. */
273 for (std::size_t i = 0; i < this->viewports->size(); ++i)
274 {
275 Viewport& view = this->viewports->at(i);
276 CameraPose& pose = view.pose;
277 if (!pose.is_valid())
278 continue;
279
280 pose.t = -pose.R * R.transposed() * t + pose.t * s;
281 pose.R = pose.R * R.transposed();
282 }
283
284 /* Transform every point. */
285 for (std::size_t i = 0; i < this->tracks->size(); ++i)
286 {
287 Track& track = this->tracks->at(i);
288 if (!track.is_valid())
289 continue;
290
291 track.pos = R * s * track.pos + t;
292 }
293
294 this->registered = true;
295}
296
297/* ---------------------------------------------------------------- */
298
299void
300Incremental::triangulate_new_tracks (int min_num_views)
301{
302 Triangulate::Options triangulate_opts;
303 triangulate_opts.error_threshold = this->opts.new_track_error_threshold;
304 triangulate_opts.angle_threshold = this->opts.min_triangulation_angle;
305 triangulate_opts.min_num_views = min_num_views;
306
308 Triangulate triangulator(triangulate_opts);
309
310 std::size_t initial_tracks_size = this->tracks->size();
311 for (std::size_t i = 0; i < this->tracks->size(); ++i)
312 {
313 /* Skip tracks that have already been triangulated. */
314 Track const& track = this->tracks->at(i);
315 if (track.is_valid())
316 continue;
317
318 /*
319 * Triangulate the track using all cameras. There can be more than two
320 * cameras if the track was rejected in previous triangulation attempts.
321 */
322 std::vector<math::Vec2f> pos;
323 std::vector<CameraPose const*> poses;
324 std::vector<std::size_t> view_ids;
325 std::vector<std::size_t> feature_ids;
326 for (std::size_t j = 0; j < track.features.size(); ++j)
327 {
328 int const view_id = track.features[j].view_id;
329 if (!this->viewports->at(view_id).pose.is_valid())
330 continue;
331 Viewport const& viewport = this->viewports->at(view_id);
332 int const feature_id = track.features[j].feature_id;
333 pos.push_back(undistort_feature(
334 viewport.features.positions[feature_id],
335 static_cast<double>(viewport.radial_distortion[0]),
336 static_cast<double>(viewport.radial_distortion[1]),
337 viewport.focal_length));
338 poses.push_back(&this->viewports->at(view_id).pose);
339 view_ids.push_back(view_id);
340 feature_ids.push_back(feature_id);
341 }
342
343 /* Skip tracks with too few valid cameras. */
344 if ((int)poses.size() < min_num_views)
345 continue;
346
347 /* Accept track if triangulation was successful. */
348 std::vector<std::size_t> outlier;
349 math::Vec3d track_pos;
350 if (!triangulator.triangulate(poses, pos, &track_pos, &stats, &outlier))
351 continue;
352 this->tracks->at(i).pos = track_pos;
353
354 /* Check if track contains outliers */
355 if (outlier.size() == 0)
356 continue;
357
358 /* Split outliers from track and generate new track */
359 Track & inlier_track = this->tracks->at(i);
360 Track outlier_track;
361 outlier_track.invalidate();
362 outlier_track.color = inlier_track.color;
363 for (std::size_t i = 0; i < outlier.size(); ++i)
364 {
365 int const view_id = view_ids[outlier[i]];
366 int const feature_id = feature_ids[outlier[i]];
367 /* Remove outlier from inlier track */
368 inlier_track.remove_view(view_id);
369 /* Add features to new track */
370 outlier_track.features.emplace_back(view_id, feature_id);
371 /* Change TrackID in viewports */
372 this->viewports->at(view_id).track_ids[feature_id] =
373 this->tracks->size();
374 }
375 this->tracks->push_back(outlier_track);
376 }
377
378 if (this->opts.verbose_output)
379 {
380 triangulator.print_statistics(stats, std::cout);
381 std::cout << " Splitted " << this->tracks->size()
382 - initial_tracks_size << " new tracks." << std::endl;
383 }
384}
385
386/* ---------------------------------------------------------------- */
387
388void
389Incremental::bundle_adjustment_full (void)
390{
391 this->bundle_adjustment_intern(-1);
392}
393
394/* ---------------------------------------------------------------- */
395
396void
397Incremental::bundle_adjustment_single_cam (int view_id)
398{
399 if (view_id < 0 || std::size_t(view_id) >= this->viewports->size()
400 || !this->viewports->at(view_id).pose.is_valid())
401 throw std::invalid_argument("Invalid view ID");
402 this->bundle_adjustment_intern(view_id);
403}
404
405/* ---------------------------------------------------------------- */
406
407void
408Incremental::bundle_adjustment_points_only (void)
409{
410 this->bundle_adjustment_intern(-2);
411}
412
413/* ---------------------------------------------------------------- */
414
415void
416Incremental::bundle_adjustment_intern (int single_camera_ba)
417{
419 ba_opts.fixed_intrinsics = this->opts.ba_fixed_intrinsics;
420 ba_opts.verbose_output = this->opts.verbose_ba;
421 if (single_camera_ba >= 0)
422 ba_opts.bundle_mode = ba::BundleAdjustment::BA_CAMERAS;
423 else if (single_camera_ba == -2)
424 ba_opts.bundle_mode = ba::BundleAdjustment::BA_POINTS;
425 else if (single_camera_ba == -1)
426 ba_opts.bundle_mode = ba::BundleAdjustment::BA_CAMERAS_AND_POINTS;
427 else
428 throw std::invalid_argument("Invalid BA mode selection");
429
430 /* Convert camera to BA data structures. */
431 std::vector<ba::Camera> ba_cameras;
432 std::vector<int> ba_cameras_mapping(this->viewports->size(), -1);
433 for (std::size_t i = 0; i < this->viewports->size(); ++i)
434 {
435 if (single_camera_ba >= 0 && int(i) != single_camera_ba)
436 continue;
437
438 Viewport const& view = this->viewports->at(i);
439 CameraPose const& pose = view.pose;
440 if (!pose.is_valid())
441 continue;
442
443 ba::Camera cam;
444 cam.focal_length = pose.get_focal_length();
445 std::copy(pose.t.begin(), pose.t.end(), cam.translation);
446 std::copy(pose.R.begin(), pose.R.end(), cam.rotation);
447 std::copy(view.radial_distortion,
448 view.radial_distortion + 2, cam.distortion);
449 ba_cameras_mapping[i] = ba_cameras.size();
450 ba_cameras.push_back(cam);
451 }
452
453 /* Convert tracks and observations to BA data structures. */
454 std::vector<ba::Observation> ba_points_2d;
455 std::vector<ba::Point3D> ba_points_3d;
456 std::vector<int> ba_tracks_mapping(this->tracks->size(), -1);
457 for (std::size_t i = 0; i < this->tracks->size(); ++i)
458 {
459 Track const& track = this->tracks->at(i);
460 if (!track.is_valid())
461 continue;
462
463 /* Add corresponding 3D point to BA. */
464 ba::Point3D point;
465 std::copy(track.pos.begin(), track.pos.end(), point.pos);
466 ba_tracks_mapping[i] = ba_points_3d.size();
467 ba_points_3d.push_back(point);
468
469 /* Add all observations to BA. */
470 for (std::size_t j = 0; j < track.features.size(); ++j)
471 {
472 int const view_id = track.features[j].view_id;
473 if (!this->viewports->at(view_id).pose.is_valid())
474 continue;
475 if (single_camera_ba >= 0 && view_id != single_camera_ba)
476 continue;
477
478 int const feature_id = track.features[j].feature_id;
479 Viewport const& view = this->viewports->at(view_id);
480 math::Vec2f const& f2d = view.features.positions[feature_id];
481
482 ba::Observation point;
483 std::copy(f2d.begin(), f2d.end(), point.pos);
484 point.camera_id = ba_cameras_mapping[view_id];
485 point.point_id = ba_tracks_mapping[i];
486 ba_points_2d.push_back(point);
487 }
488 }
489
490 for (std::size_t i = 0; registered && i < this->survey_points->size(); ++i)
491 {
492 SurveyPoint const& survey_point = this->survey_points->at(i);
493
494 /* Add corresponding 3D point to BA. */
495 ba::Point3D point;
496 std::copy(survey_point.pos.begin(), survey_point.pos.end(), point.pos);
497 point.is_constant = true;
498 ba_points_3d.push_back(point);
499
500 /* Add all observations to BA. */
501 for (std::size_t j = 0; j < survey_point.observations.size(); ++j)
502 {
503 SurveyObservation const& obs = survey_point.observations[j];
504 int const view_id = obs.view_id;
505 if (!this->viewports->at(view_id).pose.is_valid())
506 continue;
507 if (single_camera_ba >= 0 && view_id != single_camera_ba)
508 continue;
509
510 ba::Observation point;
511 std::copy(obs.pos.begin(), obs.pos.end(), point.pos);
512 point.camera_id = ba_cameras_mapping[view_id];
513 point.point_id = ba_points_3d.size() - 1;
514 ba_points_2d.push_back(point);
515 }
516 }
517
518 /* Run bundle adjustment. */
519 ba::BundleAdjustment ba(ba_opts);
520 ba.set_cameras(&ba_cameras);
521 ba.set_points(&ba_points_3d);
522 ba.set_observations(&ba_points_2d);
523 ba.optimize();
524 ba.print_status();
525
526 /* Transfer cameras back to SfM data structures. */
527 std::size_t ba_cam_counter = 0;
528 for (std::size_t i = 0; i < this->viewports->size(); ++i)
529 {
530 if (ba_cameras_mapping[i] == -1)
531 continue;
532
533 Viewport& view = this->viewports->at(i);
534 CameraPose& pose = view.pose;
535 ba::Camera const& cam = ba_cameras[ba_cam_counter];
536
537 if (this->opts.verbose_output && !this->opts.ba_fixed_intrinsics)
538 {
539 std::cout << "Camera " << std::setw(3) << i
540 << ", focal length: "
541 << util::string::get_fixed(pose.get_focal_length(), 5)
542 << " -> "
543 << util::string::get_fixed(cam.focal_length, 5)
544 << ", distortion: "
545 << util::string::get_fixed(cam.distortion[0], 5) << " "
546 << util::string::get_fixed(cam.distortion[1], 5)
547 << std::endl;
548 }
549
550 std::copy(cam.translation, cam.translation + 3, pose.t.begin());
551 std::copy(cam.rotation, cam.rotation + 9, pose.R.begin());
552 std::copy(cam.distortion, cam.distortion + 2, view.radial_distortion);
553 pose.set_k_matrix(cam.focal_length, 0.0, 0.0);
554 ba_cam_counter += 1;
555 }
556
557 /* Exit if single camera BA is used. */
558 if (single_camera_ba >= 0)
559 return;
560
561 /* Transfer tracks back to SfM data structures. */
562 std::size_t ba_track_counter = 0;
563 for (std::size_t i = 0; i < this->tracks->size(); ++i)
564 {
565 Track& track = this->tracks->at(i);
566 if (!track.is_valid())
567 continue;
568
569 ba::Point3D const& point = ba_points_3d[ba_track_counter];
570 std::copy(point.pos, point.pos + 3, track.pos.begin());
571 ba_track_counter += 1;
572 }
573}
574
575/* ---------------------------------------------------------------- */
576
577void
578Incremental::invalidate_large_error_tracks (void)
579{
580 /* Iterate over all tracks and sum reprojection error. */
581 std::vector<std::pair<double, std::size_t> > all_errors;
582 std::size_t num_valid_tracks = 0;
583 for (std::size_t i = 0; i < this->tracks->size(); ++i)
584 {
585 if (!this->tracks->at(i).is_valid())
586 continue;
587
588 num_valid_tracks += 1;
589 math::Vec3f const& pos3d = this->tracks->at(i).pos;
590 FeatureReferenceList const& ref = this->tracks->at(i).features;
591
592 double total_error = 0.0f;
593 int num_valid = 0;
594 for (std::size_t j = 0; j < ref.size(); ++j)
595 {
596 /* Get pose and 2D position of feature. */
597 int view_id = ref[j].view_id;
598 int feature_id = ref[j].feature_id;
599
600 Viewport const& viewport = this->viewports->at(view_id);
601 CameraPose const& pose = viewport.pose;
602 if (!pose.is_valid())
603 continue;
604
605 math::Vec2f const& pos2d = viewport.features.positions[feature_id];
606
607 /* Project 3D feature and compute reprojection error. */
608 math::Vec3d x = pose.R * pos3d + pose.t;
609 math::Vec2d x2d(x[0] / x[2], x[1] / x[2]);
610 double r2 = x2d.square_norm();
611 x2d *= (1.0 + r2 * (viewport.radial_distortion[0]
612 + viewport.radial_distortion[1] * r2))
613 * pose.get_focal_length();
614 total_error += (pos2d - x2d).square_norm();
615 num_valid += 1;
616 }
617 total_error /= static_cast<double>(num_valid);
618 all_errors.push_back(std::pair<double, int>(total_error, i));
619 }
620
621 if (num_valid_tracks < 2)
622 return;
623
624 /* Find the 1/2 percentile. */
625 std::size_t const nth_position = all_errors.size() / 2;
626 std::nth_element(all_errors.begin(),
627 all_errors.begin() + nth_position, all_errors.end());
628 double const square_threshold = all_errors[nth_position].first
629 * this->opts.track_error_threshold_factor;
630
631 /* Delete all tracks with errors above the threshold. */
632 int num_deleted_tracks = 0;
633 for (std::size_t i = nth_position; i < all_errors.size(); ++i)
634 {
635 if (all_errors[i].first > square_threshold)
636 {
637 this->tracks->at(all_errors[i].second).invalidate();
638 num_deleted_tracks += 1;
639 }
640 }
641
642 if (this->opts.verbose_output)
643 {
644 float percent = 100.0f * static_cast<float>(num_deleted_tracks)
645 / static_cast<float>(num_valid_tracks);
646 std::cout << "Deleted " << num_deleted_tracks
647 << " of " << num_valid_tracks << " tracks ("
648 << util::string::get_fixed(percent, 2)
649 << "%) above a threshold of "
650 << std::sqrt(square_threshold) << "." << std::endl;
651 }
652}
653
654/* ---------------------------------------------------------------- */
655
656void
657Incremental::normalize_scene (void)
658{
659 this->registered = false;
660
661 /* Compute AABB for all camera centers. */
662 math::Vec3d aabb_min(std::numeric_limits<double>::max());
663 math::Vec3d aabb_max(-std::numeric_limits<double>::max());
664 math::Vec3d camera_mean(0.0);
665 int num_valid_cameras = 0;
666 for (std::size_t i = 0; i < this->viewports->size(); ++i)
667 {
668 CameraPose const& pose = this->viewports->at(i).pose;
669 if (!pose.is_valid())
670 continue;
671
672 math::Vec3d center = -(pose.R.transposed() * pose.t);
673 for (int j = 0; j < 3; ++j)
674 {
675 aabb_min[j] = std::min(center[j], aabb_min[j]);
676 aabb_max[j] = std::max(center[j], aabb_max[j]);
677 }
678 camera_mean += center;
679 num_valid_cameras += 1;
680 }
681
682 /* Compute scale and translation. */
683 double scale = 10.0 / (aabb_max - aabb_min).maximum();
684 math::Vec3d trans = -(camera_mean / static_cast<double>(num_valid_cameras));
685
686 /* Transform every point. */
687 for (std::size_t i = 0; i < this->tracks->size(); ++i)
688 {
689 if (!this->tracks->at(i).is_valid())
690 continue;
691
692 this->tracks->at(i).pos = (this->tracks->at(i).pos + trans) * scale;
693 }
694
695 /* Transform every camera. */
696 for (std::size_t i = 0; i < this->viewports->size(); ++i)
697 {
698 CameraPose& pose = this->viewports->at(i).pose;
699 if (!pose.is_valid())
700 continue;
701 pose.t = pose.t * scale - pose.R * trans * scale;
702 }
703}
704
705/* ---------------------------------------------------------------- */
706
707void
708Incremental::print_registration_error (void) const
709{
710 double sum = 0;
711 int num_points = 0;
712 for (std::size_t i = 0; i < this->survey_points->size(); ++i)
713 {
714 SurveyPoint const& survey_point = this->survey_points->at(i);
715
716 std::vector<math::Vec2f> pos;
717 std::vector<CameraPose const*> poses;
718 for (std::size_t j = 0; j < survey_point.observations.size(); ++j)
719 {
720 SurveyObservation const& obs = survey_point.observations[j];
721 int const view_id = obs.view_id;
722 if (!this->viewports->at(view_id).pose.is_valid())
723 continue;
724
725 pos.push_back(obs.pos);
726 poses.push_back(&this->viewports->at(view_id).pose);
727 }
728
729 if (pos.size() < 2)
730 continue;
731
732 math::Vec3d recon = triangulate_track(pos, poses);
733 sum += (survey_point.pos - recon).square_norm();
734 num_points += 1;
735 }
736
737 if (num_points > 0)
738 {
739 double mse = sum / num_points;
740 std::cout << "Reconstructed " << num_points
741 << " survey points with a MSE of " << mse << std::endl;
742 }
743 else
744 {
745 std::cout << "Failed to reconstruct all survey points." << std::endl;
746 }
747}
748
749/* ---------------------------------------------------------------- */
750
752Incremental::create_bundle (void) const
753{
754 if (this->opts.verbose_output && this->registered)
755 this->print_registration_error();
756
757 /* Create bundle data structure. */
759 {
760 /* Populate the cameras in the bundle. */
761 mve::Bundle::Cameras& bundle_cams = bundle->get_cameras();
762 bundle_cams.resize(this->viewports->size());
763 for (std::size_t i = 0; i < this->viewports->size(); ++i)
764 {
765 mve::CameraInfo& cam = bundle_cams[i];
766 Viewport const& viewport = this->viewports->at(i);
767 CameraPose const& pose = viewport.pose;
768 if (!pose.is_valid())
769 {
770 cam.flen = 0.0f;
771 continue;
772 }
773
774 cam.flen = static_cast<float>(pose.get_focal_length());
775 cam.ppoint[0] = pose.K[2] + 0.5f;
776 cam.ppoint[1] = pose.K[5] + 0.5f;
777 std::copy(pose.R.begin(), pose.R.end(), cam.rot);
778 std::copy(pose.t.begin(), pose.t.end(), cam.trans);
779 cam.dist[0] = viewport.radial_distortion[0];
780 cam.dist[1] = viewport.radial_distortion[1];
781 }
782
783 /* Populate the features in the Bundle. */
784 mve::Bundle::Features& bundle_feats = bundle->get_features();
785 bundle_feats.reserve(this->tracks->size());
786 for (std::size_t i = 0; i < this->tracks->size(); ++i)
787 {
788 Track const& track = this->tracks->at(i);
789 if (!track.is_valid())
790 continue;
791
792 /* Copy position and color of the track. */
793 bundle_feats.push_back(mve::Bundle::Feature3D());
794 mve::Bundle::Feature3D& f3d = bundle_feats.back();
795 std::copy(track.pos.begin(), track.pos.end(), f3d.pos);
796 f3d.color[0] = track.color[0] / 255.0f;
797 f3d.color[1] = track.color[1] / 255.0f;
798 f3d.color[2] = track.color[2] / 255.0f;
799 f3d.refs.reserve(track.features.size());
800 for (std::size_t j = 0; j < track.features.size(); ++j)
801 {
802 /* For each reference copy view ID, feature ID and 2D pos. */
803 f3d.refs.push_back(mve::Bundle::Feature2D());
804 mve::Bundle::Feature2D& f2d = f3d.refs.back();
805 f2d.view_id = track.features[j].view_id;
806 f2d.feature_id = track.features[j].feature_id;
807
808 FeatureSet const& features
809 = this->viewports->at(f2d.view_id).features;
810 math::Vec2f const& f2d_pos
811 = features.positions[f2d.feature_id];
812 std::copy(f2d_pos.begin(), f2d_pos.end(), f2d.pos);
813 }
814 }
815 }
816
817 return bundle;
818}
819
Matrix class for arbitrary dimensions and types.
Definition matrix.h:54
T * end(void)
Definition matrix.h:520
Vector< T, N > col(int index) const
Returns a column of the matrix as vector.
Definition matrix.h:330
Matrix< T, N, M-1 > delete_col(int index) const
Returns a new matrix with the specified column deleted.
Definition matrix.h:408
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Definition matrix.h:448
T * begin(void)
Definition matrix.h:506
Vector class for arbitrary dimensions and types.
Definition vector.h:87
T square_norm(void) const
Computes the squared norm of the vector (much cheaper).
Definition vector.h:441
T * end(void)
Definition vector.h:597
T * begin(void)
Definition vector.h:583
std::vector< Feature3D > Features
Definition bundle.h:62
std::vector< CameraInfo > Cameras
Definition bundle.h:61
static Ptr create(void)
Definition bundle.h:102
std::shared_ptr< Bundle > Ptr
Definition bundle.h:59
The FeatureSet holds per-feature information for a single view, and allows to transparently compute a...
Definition feature_set.h:28
std::vector< math::Vec2f > positions
Per-feature image position.
Definition feature_set.h:66
RANSAC pose estimation from 2D-3D correspondences and known camera calibration using the perspective ...
void estimate(Correspondences2D3D const &corresp, math::Matrix< double, 3, 3 > const &k_matrix, Result *result) const
Triangulation routine that triangulates a track from camera poses and 2D image positions while keepin...
Definition triangulate.h:59
bool triangulate(std::vector< CameraPose const * > const &poses, std::vector< math::Vec2f > const &positions, math::Vec3d *track_pos, Statistics *stats=nullptr, std::vector< std::size_t > *outliers=nullptr) const
void print_statistics(Statistics const &stats, std::ostream &out) const
Cross-platform high-resolution real-time timer.
Definition timer.h:30
std::size_t get_elapsed(void) const
Returns the milli seconds since last reset.
Definition timer.h:94
std::size_t second
Definition mesh_info.cc:47
std::size_t first
Definition mesh_info.cc:46
bool determine_transform(std::vector< math::Vector< T, N > > const &p0, std::vector< math::Vector< T, N > > const &p1, math::Matrix< T, N, N > *rot, T *scale, math::Vector< T, N > *trans)
Definition transform.h:36
std::vector< Viewport > ViewportList
The list of all viewports considered for bundling.
math::Vec2f undistort_feature(math::Vec2f const &f, double const k1, double const k2, float const focal_length)
std::vector< Track > TrackList
The list of all tracks.
std::vector< FeatureReference > FeatureReferenceList
The list of all feature references inside a track.
std::vector< SurveyPoint > SurveyPointList
The list of all survey poins.
math::Vector< double, 3 > triangulate_track(std::vector< math::Vec2f > const &pos, std::vector< CameraPose const * > const &poses)
Given any number of 2D image positions and the corresponding camera poses, this function triangulates...
std::vector< Correspondence2D3D > Correspondences2D3D
std::string get_fixed(T const &value, int digits)
Returns string with 'digits' of fixed precision (fills with zeros).
Definition strings.h:124
#define SFM_BUNDLER_NAMESPACE_END
Definition defines.h:17
#define SFM_BUNDLER_NAMESPACE_BEGIN
Definition defines.h:16
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13
Representation of a 2D feature.
Definition bundle.h:34
Representation of a 3D feature with position and color.
Definition bundle.h:47
std::vector< Feature2D > refs
References to views that see the feature.
Definition bundle.h:53
float pos[3]
3D Position of the feature (track).
Definition bundle.h:49
float color[3]
RGB color of the feature in [0,1]^3.
Definition bundle.h:51
Per-view camera information with various helper functions.
Definition camera.h:24
float trans[3]
Camera translation vector.
Definition camera.h:167
float rot[9]
Camera rotation which transforms from world to cam.
Definition camera.h:169
float ppoint[2]
Principal point in x- and y-direction.
Definition camera.h:158
float flen
Focal length.
Definition camera.h:156
float dist[2]
Image distortion parameters.
Definition camera.h:162
The camera pose is the 3x4 matrix P = K [R | t].
Definition camera_pose.h:40
math::Matrix< double, 3, 3 > R
Definition camera_pose.h:59
bool is_valid(void) const
Returns true if K matrix is valid (non-zero focal length).
void fill_p_matrix(math::Matrix< double, 3, 4 > *result) const
Returns the P matrix as the product K [R | t].
Definition camera_pose.h:81
void set_k_matrix(double flen, double px, double py)
Initializes the K matrix from focal length and principal point.
Definition camera_pose.h:89
double get_focal_length(void) const
Returns the focal length as average of x and y focal length.
Definition camera_pose.h:98
math::Matrix< double, 3, 3 > K
Definition camera_pose.h:58
math::Vector< double, 3 > t
Definition camera_pose.h:60
A 3D point and an image coordinate which correspond to each other in terms of the image observing thi...
math::Matrix< double, 3, 4 > pose
The pose [R|t] which led to the inliers.
std::vector< int > inliers
The correspondence indices which led to the result.
double angle_threshold
Threshold on the triangulation angle (in radians).
Definition triangulate.h:68
double error_threshold
Threshold on reprojection error for outlier detection.
Definition triangulate.h:66
int min_num_views
Minimal number of views with small error (inliers).
Definition triangulate.h:70
Camera representation for bundle adjustment.
Definition ba_types.h:13
double focal_length
Definition ba_types.h:16
double distortion[2]
Definition ba_types.h:17
double rotation[9]
Definition ba_types.h:19
double translation[3]
Definition ba_types.h:18
Observation of a 3D point for a camera.
Definition ba_types.h:32
3D point representation for bundle adjustment.
Definition ba_types.h:25
double pos[3]
Definition ba_types.h:26
Representation of a survey point.
SurveyObservationList observations
Representation of a feature track.
bool is_valid(void) const
FeatureReferenceList features
void remove_view(int view_id)
Per-viewport information.
float radial_distortion[2]
Radial distortion parameter.
std::unordered_map< int, int > backup_tracks
Backup map from features to tracks that were removed due to errors.
CameraPose pose
Camera pose for the viewport.
std::vector< int > track_ids
Per-feature track ID, -1 if not part of a track.
float focal_length
Initial focal length estimate for the image.
FeatureSet features
Per-feature information.