29 this->viewports = viewports;
30 this->tracks = tracks;
31 this->survey_points = survey_points;
33 if (this->viewports->empty())
34 throw std::invalid_argument(
"No viewports given");
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");
45 for (std::size_t i = 0; i < tracks->size(); ++i)
47 Track& track = tracks->at(i);
55Incremental::find_next_views (std::vector<int>* next_views)
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));
62 for (std::size_t i = 0; i < this->tracks->size(); ++i)
64 Track const& track = this->tracks->at(i);
68 for (std::size_t j = 0; j < track.
features.size(); ++j)
70 int const view_id = track.
features[j].view_id;
71 if (this->viewports->at(view_id).pose.is_valid())
73 valid_tracks[view_id].first += 1;
78 std::sort(valid_tracks.rbegin(), valid_tracks.rend());
82 for (std::size_t i = 0; i < valid_tracks.size(); ++i)
84 if (valid_tracks[i].
first > 6)
85 next_views->push_back(valid_tracks[i].
second);
92Incremental::reconstruct_next_view (
int view_id)
94 Viewport const& viewport = this->viewports->at(view_id);
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)
103 int const track_id = viewport.
track_ids[i];
104 if (track_id < 0 || !this->tracks->at(track_id).is_valid())
107 math::Vec3f const& pos3d = this->tracks->at(track_id).pos;
113 track_ids.push_back(track_id);
114 feature_ids.push_back(i);
117 if (this->opts.verbose_output)
119 std::cout <<
"Collected " << corr.size()
120 <<
" 2D-3D correspondences." << std::endl;
132 ransac.
estimate(corr, temp_camera.
K, &ransac_result);
136 if (3 * ransac_result.
inliers.size() < corr.size())
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;
145 else if (this->opts.verbose_output)
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;
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)
162 if (track_ids[i] < 0)
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]);
174 CameraPose& pose = this->viewports->at(view_id).pose;
179 if (this->opts.verbose_output)
181 std::cout <<
"Reconstructed camera "
182 << view_id <<
" with focal length "
187 if (this->survey_points !=
nullptr && !registered)
188 this->try_registration();
194Incremental::try_restore_tracks_for_views (
void)
196 for (std::size_t i = 0; i < this->viewports->size(); ++i)
198 Viewport& viewport = this->viewports->at(i);
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()
211 math::Vec3f const& pos3d = this->tracks->at(track_id).pos;
218 math::Vec4d const pos3dh(pos3d[0], pos3d[1], pos3d[2], 1.0);
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)
224 viewport.
track_ids[feature_id] = track_id;
225 this->tracks->at(track_id).features.emplace_back(i, feature_id);
234Incremental::try_registration () {
235 std::vector<math::Vec3d> p0;
236 std::vector<math::Vec3d> p1;
238 for (std::size_t i = 0; i < this->survey_points->size(); ++i)
240 SurveyPoint const& survey_point = this->survey_points->at(i);
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)
247 int const view_id = obs.
view_id;
248 if (!this->viewports->at(view_id).pose.is_valid())
251 pos.push_back(obs.
pos);
252 poses.push_back(&this->viewports->at(view_id).pose);
259 p1.push_back(survey_point.
pos);
273 for (std::size_t i = 0; i < this->viewports->size(); ++i)
275 Viewport& view = this->viewports->at(i);
285 for (std::size_t i = 0; i < this->tracks->size(); ++i)
287 Track& track = this->tracks->at(i);
291 track.
pos = R * s * track.
pos + t;
294 this->registered =
true;
300Incremental::triangulate_new_tracks (
int min_num_views)
303 triangulate_opts.
error_threshold = this->opts.new_track_error_threshold;
310 std::size_t initial_tracks_size = this->tracks->size();
311 for (std::size_t i = 0; i < this->tracks->size(); ++i)
314 Track const& track = this->tracks->at(i);
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)
328 int const view_id = track.
features[j].view_id;
329 if (!this->viewports->at(view_id).pose.is_valid())
331 Viewport const& viewport = this->viewports->at(view_id);
332 int const feature_id = track.
features[j].feature_id;
338 poses.push_back(&this->viewports->at(view_id).pose);
339 view_ids.push_back(view_id);
340 feature_ids.push_back(feature_id);
344 if ((
int)poses.size() < min_num_views)
348 std::vector<std::size_t> outlier;
350 if (!triangulator.
triangulate(poses, pos, &track_pos, &stats, &outlier))
352 this->tracks->at(i).pos = track_pos;
355 if (outlier.size() == 0)
359 Track & inlier_track = this->tracks->at(i);
363 for (std::size_t i = 0; i < outlier.size(); ++i)
365 int const view_id = view_ids[outlier[i]];
366 int const feature_id = feature_ids[outlier[i]];
370 outlier_track.
features.emplace_back(view_id, feature_id);
372 this->viewports->at(view_id).track_ids[feature_id] =
373 this->tracks->size();
375 this->tracks->push_back(outlier_track);
378 if (this->opts.verbose_output)
381 std::cout <<
" Splitted " << this->tracks->size()
382 - initial_tracks_size <<
" new tracks." << std::endl;
389Incremental::bundle_adjustment_full (
void)
391 this->bundle_adjustment_intern(-1);
397Incremental::bundle_adjustment_single_cam (
int view_id)
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);
408Incremental::bundle_adjustment_points_only (
void)
410 this->bundle_adjustment_intern(-2);
416Incremental::bundle_adjustment_intern (
int single_camera_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;
428 throw std::invalid_argument(
"Invalid BA mode selection");
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)
435 if (single_camera_ba >= 0 &&
int(i) != single_camera_ba)
438 Viewport const& view = this->viewports->at(i);
449 ba_cameras_mapping[i] = ba_cameras.size();
450 ba_cameras.push_back(cam);
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)
459 Track
const& track = this->tracks->at(i);
460 if (!track.is_valid())
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);
470 for (std::size_t j = 0; j < track.features.size(); ++j)
472 int const view_id = track.features[j].view_id;
473 if (!this->viewports->at(view_id).pose.is_valid())
475 if (single_camera_ba >= 0 && view_id != single_camera_ba)
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];
484 point.
camera_id = ba_cameras_mapping[view_id];
485 point.
point_id = ba_tracks_mapping[i];
486 ba_points_2d.push_back(point);
490 for (std::size_t i = 0; registered && i < this->survey_points->size(); ++i)
492 SurveyPoint
const& survey_point = this->survey_points->at(i);
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);
501 for (std::size_t j = 0; j < survey_point.observations.size(); ++j)
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())
507 if (single_camera_ba >= 0 && view_id != single_camera_ba)
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);
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);
527 std::size_t ba_cam_counter = 0;
528 for (std::size_t i = 0; i < this->viewports->size(); ++i)
530 if (ba_cameras_mapping[i] == -1)
533 Viewport& view = this->viewports->at(i);
534 CameraPose& pose = view.pose;
535 ba::Camera
const& cam = ba_cameras[ba_cam_counter];
537 if (this->opts.verbose_output && !this->opts.ba_fixed_intrinsics)
539 std::cout <<
"Camera " << std::setw(3) << i
540 <<
", focal length: "
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);
558 if (single_camera_ba >= 0)
562 std::size_t ba_track_counter = 0;
563 for (std::size_t i = 0; i < this->tracks->size(); ++i)
565 Track& track = this->tracks->at(i);
566 if (!track.is_valid())
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;
578Incremental::invalidate_large_error_tracks (
void)
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)
585 if (!this->tracks->at(i).is_valid())
588 num_valid_tracks += 1;
589 math::Vec3f const& pos3d = this->tracks->at(i).pos;
592 double total_error = 0.0f;
594 for (std::size_t j = 0; j < ref.size(); ++j)
597 int view_id = ref[j].view_id;
598 int feature_id = ref[j].feature_id;
600 Viewport const& viewport = this->viewports->at(view_id);
614 total_error += (pos2d - x2d).square_norm();
617 total_error /=
static_cast<double>(num_valid);
618 all_errors.push_back(std::pair<double, int>(total_error, i));
621 if (num_valid_tracks < 2)
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;
632 int num_deleted_tracks = 0;
633 for (std::size_t i = nth_position; i < all_errors.size(); ++i)
635 if (all_errors[i].
first > square_threshold)
637 this->tracks->at(all_errors[i].
second).invalidate();
638 num_deleted_tracks += 1;
642 if (this->opts.verbose_output)
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 ("
649 <<
"%) above a threshold of "
650 << std::sqrt(square_threshold) <<
"." << std::endl;
657Incremental::normalize_scene (
void)
659 this->registered =
false;
662 math::Vec3d aabb_min(std::numeric_limits<double>::max());
663 math::Vec3d aabb_max(-std::numeric_limits<double>::max());
665 int num_valid_cameras = 0;
666 for (std::size_t i = 0; i < this->viewports->size(); ++i)
668 CameraPose const& pose = this->viewports->at(i).pose;
673 for (
int j = 0; j < 3; ++j)
675 aabb_min[j] = std::min(center[j], aabb_min[j]);
676 aabb_max[j] = std::max(center[j], aabb_max[j]);
678 camera_mean += center;
679 num_valid_cameras += 1;
683 double scale = 10.0 / (aabb_max - aabb_min).maximum();
684 math::Vec3d trans = -(camera_mean /
static_cast<double>(num_valid_cameras));
687 for (std::size_t i = 0; i < this->tracks->size(); ++i)
689 if (!this->tracks->at(i).is_valid())
692 this->tracks->at(i).pos = (this->tracks->at(i).pos + trans) * scale;
696 for (std::size_t i = 0; i < this->viewports->size(); ++i)
698 CameraPose& pose = this->viewports->at(i).pose;
701 pose.
t = pose.
t * scale - pose.
R * trans * scale;
708Incremental::print_registration_error (
void)
const
712 for (std::size_t i = 0; i < this->survey_points->size(); ++i)
714 SurveyPoint const& survey_point = this->survey_points->at(i);
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)
721 int const view_id = obs.
view_id;
722 if (!this->viewports->at(view_id).pose.is_valid())
725 pos.push_back(obs.
pos);
726 poses.push_back(&this->viewports->at(view_id).pose);
733 sum += (survey_point.
pos - recon).square_norm();
739 double mse = sum / num_points;
740 std::cout <<
"Reconstructed " << num_points
741 <<
" survey points with a MSE of " << mse << std::endl;
745 std::cout <<
"Failed to reconstruct all survey points." << std::endl;
752Incremental::create_bundle (
void)
const
754 if (this->opts.verbose_output && this->registered)
755 this->print_registration_error();
762 bundle_cams.resize(this->viewports->size());
763 for (std::size_t i = 0; i < this->viewports->size(); ++i)
766 Viewport const& viewport = this->viewports->at(i);
775 cam.
ppoint[0] = pose.
K[2] + 0.5f;
776 cam.
ppoint[1] = pose.
K[5] + 0.5f;
785 bundle_feats.reserve(this->tracks->size());
786 for (std::size_t i = 0; i < this->tracks->size(); ++i)
788 Track const& track = this->tracks->at(i);
800 for (std::size_t j = 0; j < track.
features.size(); ++j)
809 = this->viewports->at(f2d.
view_id).features;
812 std::copy(f2d_pos.
begin(), f2d_pos.
end(), f2d.
pos);
Matrix class for arbitrary dimensions and types.
Vector< T, N > col(int index) const
Returns a column of the matrix as vector.
Matrix< T, N, M-1 > delete_col(int index) const
Returns a new matrix with the specified column deleted.
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Vector class for arbitrary dimensions and types.
T square_norm(void) const
Computes the squared norm of the vector (much cheaper).
std::vector< Feature3D > Features
std::vector< CameraInfo > Cameras
std::shared_ptr< Bundle > Ptr
The FeatureSet holds per-feature information for a single view, and allows to transparently compute a...
std::vector< math::Vec2f > positions
Per-feature image position.
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...
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.
std::size_t get_elapsed(void) const
Returns the milli seconds since last reset.
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)
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).
#define SFM_BUNDLER_NAMESPACE_END
#define SFM_BUNDLER_NAMESPACE_BEGIN
#define SFM_NAMESPACE_END
#define SFM_NAMESPACE_BEGIN
Representation of a 2D feature.
Representation of a 3D feature with position and color.
std::vector< Feature2D > refs
References to views that see the feature.
float pos[3]
3D Position of the feature (track).
float color[3]
RGB color of the feature in [0,1]^3.
Per-view camera information with various helper functions.
float trans[3]
Camera translation vector.
float rot[9]
Camera rotation which transforms from world to cam.
float ppoint[2]
Principal point in x- and y-direction.
float dist[2]
Image distortion parameters.
The camera pose is the 3x4 matrix P = K [R | t].
math::Matrix< double, 3, 3 > R
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].
void set_k_matrix(double flen, double px, double py)
Initializes the K matrix from focal length and principal point.
double get_focal_length(void) const
Returns the focal length as average of x and y focal length.
math::Matrix< double, 3, 3 > K
math::Vector< double, 3 > t
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).
double error_threshold
Threshold on reprojection error for outlier detection.
int min_num_views
Minimal number of views with small error (inliers).
Camera representation for bundle adjustment.
Observation of a 3D point for a camera.
3D point representation for bundle adjustment.
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.