MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
bundler_init_pair.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann
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 <iostream>
11#include <vector>
12#include <sstream>
13#include <random>
14
16#include "sfm/triangulate.h"
18
21
22void
23InitialPair::compute_pair (Result* result)
24{
25 if (this->viewports == nullptr || this->tracks == nullptr)
26 throw std::invalid_argument("Null viewports or tracks");
27
28 std::cout << "Searching for initial pair..." << std::endl;
29 result->view_1_id = -1;
30 result->view_2_id = -1;
31
32 /* Convert tracks to pairwise information. */
33 std::vector<CandidatePair> candidates;
34 this->compute_candidate_pairs(&candidates);
35
36 /* Sort the candidate pairs by number of matches. */
37 std::sort(candidates.rbegin(), candidates.rend());
38
39 /*
40 * Search for a good initial pair and return the first pair that
41 * satisfies all thresholds (min matches, max homography inliers,
42 * min triangulation angle). If no pair satisfies all thresholds, the
43 * pair with the best score is returned.
44 */
45 bool found_pair = false;
46 std::size_t found_pair_id = std::numeric_limits<std::size_t>::max();
47 std::vector<float> pair_scores(candidates.size(), 0.0f);
48#pragma omp parallel for schedule(dynamic)
49 for (std::size_t i = 0; i < candidates.size(); ++i)
50 {
51 if (found_pair)
52 continue;
53
54 /* Reject pairs with 8 or fewer matches. */
55 CandidatePair const& candidate = candidates[i];
56 std::size_t num_matches = candidate.matches.size();
57 if (num_matches < static_cast<std::size_t>(this->opts.min_num_matches))
58 {
59 this->debug_output(candidate);
60 continue;
61 }
62
63 /* Reject pairs with too high percentage of homograhy inliers. */
64 std::size_t num_inliers = this->compute_homography_inliers(candidate);
65 float percentage = static_cast<float>(num_inliers) / num_matches;
66 if (percentage > this->opts.max_homography_inliers)
67 {
68 this->debug_output(candidate, num_inliers);
69 continue;
70 }
71
72 /* Compute initial pair pose. */
73 CameraPose pose1, pose2;
74 bool const found_pose = this->compute_pose(candidate, &pose1, &pose2);
75 if (!found_pose)
76 {
77 this->debug_output(candidate, num_inliers);
78 continue;
79 }
80
81 /* Rejects pairs with bad triangulation angle. */
82 double const angle = this->angle_for_pose(candidate, pose1, pose2);
83 pair_scores[i] = this->score_for_pair(candidate, num_inliers, angle);
84 this->debug_output(candidate, num_inliers, angle);
85 if (angle < this->opts.min_triangulation_angle)
86 continue;
87
88 /* Run triangulation to ensure correct pair */
89 Triangulate::Options triangulate_opts;
90 Triangulate triangulator(triangulate_opts);
91 std::vector<CameraPose const*> poses;
92 poses.push_back(&pose1);
93 poses.push_back(&pose2);
94 std::size_t successful_triangulations = 0;
95 std::vector<math::Vec2f> positions(2);
97 for (std::size_t j = 0; j < candidate.matches.size(); ++j)
98 {
99 positions[0] = math::Vec2f(candidate.matches[j].p1);
100 positions[1] = math::Vec2f(candidate.matches[j].p2);
101 math::Vec3d pos3d;
102 if (triangulator.triangulate(poses, positions, &pos3d, &stats))
103 successful_triangulations += 1;
104 }
105 if (successful_triangulations * 2 < candidate.matches.size())
106 continue;
107
108#pragma omp critical
109 if (i < found_pair_id)
110 {
111 result->view_1_id = candidate.view_1_id;
112 result->view_2_id = candidate.view_2_id;
113 result->view_1_pose = pose1;
114 result->view_2_pose = pose2;
115 found_pair_id = i;
116 found_pair = true;
117 }
118 }
119
120 /* Return if a pair satisfying all thresholds has been found. */
121 if (found_pair)
122 return;
123
124 /* Return pair with best score (larger than 0.0). */
125 std::cout << "Searching for pair with best score..." << std::endl;
126 float best_score = 0.0f;
127 std::size_t best_pair_id = 0;
128 for (std::size_t i = 0; i < pair_scores.size(); ++i)
129 {
130 if (pair_scores[i] <= best_score)
131 continue;
132
133 best_score = pair_scores[i];
134 best_pair_id = i;
135 }
136
137 /* Recompute pose for pair with best score. */
138 if (best_score > 0.0f)
139 {
140 result->view_1_id = candidates[best_pair_id].view_1_id;
141 result->view_2_id = candidates[best_pair_id].view_2_id;
142 this->compute_pose(candidates[best_pair_id],
143 &result->view_1_pose, &result->view_2_pose);
144 }
145}
146
147void
148InitialPair::compute_pair (int view_1_id, int view_2_id, Result* result)
149{
150 if (view_1_id > view_2_id)
151 std::swap(view_1_id, view_2_id);
152
153 /* Convert tracks to pairwise information. */
154 std::vector<CandidatePair> candidates;
155 this->compute_candidate_pairs(&candidates);
156
157 /* Find candidate pair. */
158 CandidatePair* candidate = nullptr;
159 for (std::size_t i = 0; candidate == nullptr && i < candidates.size(); ++i)
160 {
161 if (view_1_id == candidates[i].view_1_id
162 && view_2_id == candidates[i].view_2_id)
163 candidate = &candidates[i];
164 }
165 if (candidate == nullptr)
166 throw std::runtime_error("No matches for initial pair");
167
168 /* Compute initial pair pose. */
169 result->view_1_id = view_1_id;
170 result->view_2_id = view_2_id;
171 bool const found_pose = this->compute_pose(*candidate,
172 &result->view_1_pose, &result->view_2_pose);
173 if (!found_pose)
174 throw std::runtime_error("Cannot compute pose for initial pair");
175}
176
177void
178InitialPair::compute_candidate_pairs (CandidatePairs* candidates)
179{
180 /*
181 * Convert the tracks to pairwise information. This is similar to using
182 * the pairwise matching result directly, however, the tracks have been
183 * further filtered during track generation.
184 */
185 int const num_viewports = static_cast<int>(this->viewports->size());
186 std::vector<int> candidate_lookup(MATH_POW2(num_viewports), -1);
187 candidates->reserve(1000);
188 for (std::size_t i = 0; i < this->tracks->size(); ++i)
189 {
190 Track const& track = this->tracks->at(i);
191 for (std::size_t j = 1; j < track.features.size(); ++j)
192 for (std::size_t k = 0; k < j; ++k)
193 {
194 int v1id = track.features[j].view_id;
195 int v2id = track.features[k].view_id;
196 int f1id = track.features[j].feature_id;
197 int f2id = track.features[k].feature_id;
198 if (v1id > v2id)
199 {
200 std::swap(v1id, v2id);
201 std::swap(f1id, f2id);
202 }
203
204 /* Lookup pair. */
205 int const lookup_id = v1id * num_viewports + v2id;
206 int pair_id = candidate_lookup[lookup_id];
207 if (pair_id == -1)
208 {
209 pair_id = static_cast<int>(candidates->size());
210 candidate_lookup[lookup_id] = pair_id;
211 candidates->push_back(CandidatePair());
212 candidates->back().view_1_id = v1id;
213 candidates->back().view_2_id = v2id;
214 }
215
216 /* Fill candidate with additional info. */
217 Viewport const& view1 = this->viewports->at(v1id);
218 Viewport const& view2 = this->viewports->at(v2id);
219 math::Vec2f const v1pos = view1.features.positions[f1id];
220 math::Vec2f const v2pos = view2.features.positions[f2id];
221 Correspondence2D2D match;
222 std::copy(v1pos.begin(), v1pos.end(), match.p1);
223 std::copy(v2pos.begin(), v2pos.end(), match.p2);
224 candidates->at(pair_id).matches.push_back(match);
225 }
226 }
227}
228
229std::size_t
230InitialPair::compute_homography_inliers (CandidatePair const& candidate)
231{
232 /* Execute homography RANSAC. */
233 RansacHomography::Result ransac_result;
234 RansacHomography homography_ransac(this->opts.homography_opts);
235 homography_ransac.estimate(candidate.matches, &ransac_result);
236 return ransac_result.inliers.size();
237}
238
239bool
240InitialPair::compute_pose (CandidatePair const& candidate,
241 CameraPose* pose1, CameraPose* pose2)
242{
243 /* Compute fundamental matrix from pair correspondences. */
244 FundamentalMatrix fundamental;
245 {
246 Correspondences2D2D matches = candidate.matches;
247 if (matches.size() > 1000ul) {
248 std::mt19937 g;
249 std::shuffle(matches.begin(), matches.end(), g);
250 matches.resize(1000ul);
251 }
252 fundamental_least_squares(matches, &fundamental);
254 }
255
256 /* Populate K-matrices. */
257 Viewport const& view_1 = this->viewports->at(candidate.view_1_id);
258 Viewport const& view_2 = this->viewports->at(candidate.view_2_id);
259 pose1->set_k_matrix(view_1.focal_length, 0.0, 0.0);
260 pose1->init_canonical_form();
261 pose2->set_k_matrix(view_2.focal_length, 0.0, 0.0);
262
263 /* Compute essential matrix from fundamental matrix (HZ (9.12)). */
264 EssentialMatrix E = pose2->K.transposed() * fundamental * pose1->K;
265
266 /* Compute pose from essential. */
267 std::vector<CameraPose> poses;
268 pose_from_essential(E, &poses);
269
270 /* Find the correct pose using point test (HZ Fig. 9.12). */
271 bool found_pose = false;
272 for (std::size_t i = 0; i < poses.size(); ++i)
273 {
274 poses[i].K = pose2->K;
275 if (is_consistent_pose(candidate.matches[0], *pose1, poses[i]))
276 {
277 *pose2 = poses[i];
278 found_pose = true;
279 break;
280 }
281 }
282 return found_pose;
283}
284
285double
286InitialPair::angle_for_pose (CandidatePair const& candidate,
287 CameraPose const& pose1, CameraPose const& pose2)
288{
289 /* Compute transformation from image coordinates to viewing direction. */
290 math::Matrix3d T1 = pose1.R.transposed() * math::matrix_inverse(pose1.K);
291 math::Matrix3d T2 = pose2.R.transposed() * math::matrix_inverse(pose2.K);
292
293 /* Compute triangulation angle for each correspondence. */
294 std::vector<double> cos_angles;
295 cos_angles.reserve(candidate.matches.size());
296 for (std::size_t i = 0; i < candidate.matches.size(); ++i)
297 {
298 Correspondence2D2D const& match = candidate.matches[i];
299 math::Vec3d p1(match.p1[0], match.p1[1], 1.0);
300 p1 = T1.mult(p1).normalized();
301 math::Vec3d p2(match.p2[0], match.p2[1], 1.0);
302 p2 = T2.mult(p2).normalized();
303 cos_angles.push_back(p1.dot(p2));
304 }
305
306 /* Return 50% median. */
307 std::size_t median_index = cos_angles.size() / 2;
308 std::nth_element(cos_angles.begin(),
309 cos_angles.begin() + median_index, cos_angles.end());
310 double const cos_angle = math::clamp(cos_angles[median_index], -1.0, 1.0);
311 return std::acos(cos_angle);
312}
313
314float
315InitialPair::score_for_pair (CandidatePair const& candidate,
316 std::size_t num_inliers, double angle)
317{
318 float const matches = static_cast<float>(candidate.matches.size());
319 float const inliers = static_cast<float>(num_inliers) / matches;
320 float const angle_d = MATH_RAD2DEG(angle);
321
322 /* Score for matches (min: 20, good: 200). */
323 float f1 = 2.0 / (1.0 + std::exp((20.0 - matches) * 6.0 / 200.0)) - 1.0;
324 /* Score for angle (min 1 degree, good 8 degree). */
325 float f2 = 2.0 / (1.0 + std::exp((1.0 - angle_d) * 6.0 / 8.0)) - 1.0;
326 /* Score for H-Inliers (max 70%, good 40%). */
327 float f3 = 2.0 / (1.0 + std::exp((inliers - 0.7) * 6.0 / 0.4)) - 1.0;
328
329 f1 = math::clamp(f1, 0.0f, 1.0f);
330 f2 = math::clamp(f2, 0.0f, 1.0f);
331 f3 = math::clamp(f3, 0.0f, 1.0f);
332 return f1 * f2 * f3;
333}
334
335void
336InitialPair::debug_output (CandidatePair const& candidate,
337 std::size_t num_inliers, double angle)
338{
339 if (!this->opts.verbose_output)
340 return;
341
342 std::stringstream message;
343 std::size_t num_matches = candidate.matches.size();
344 message << " Pair " << std::setw(3) << candidate.view_1_id
345 << "," << std::setw(3) << candidate.view_2_id
346 << ": " << std::setw(4) << num_matches << " matches";
347
348 if (num_inliers > 0)
349 {
350 float percentage = static_cast<float>(num_inliers) / num_matches;
351 message << ", " << std::setw(4) << num_inliers
352 << " H-inliers (" << (int)(100.0f * percentage) << "%)";
353 }
354
355 if (angle > 0.0)
356 {
357 message << ", " << std::setw(5)
359 << " pair angle";
360 }
361
362#pragma omp critical
363 std::cout << message.str() << std::endl;
364}
365
368
Matrix< T, N, U > mult(Matrix< T, M, U > const &rhs) const
Matrix with matrix multiplication.
Definition matrix.h:462
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Definition matrix.h:448
T * end(void)
Definition vector.h:597
T * begin(void)
Definition vector.h:583
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
#define MATH_RAD2DEG(x)
Definition defines.h:77
#define MATH_POW2(x)
Definition defines.h:68
Vector< float, 2 > Vec2f
Definition vector.h:30
T const & clamp(T const &v, T const &min=T(0), T const &max=T(1))
Returns value 'v' clamped to the interval specified by 'min' and 'max'.
Definition functions.h:204
Matrix< T, N, N > matrix_inverse(Matrix< T, N, N > const &mat)
Calculates the inverse of the given matrix.
void enforce_fundamental_constraints(FundamentalMatrix *matrix)
Constraints the given matrix to have TWO NON-ZERO eigenvalues.
bool fundamental_least_squares(Correspondences2D2D const &points, FundamentalMatrix *result)
Algorithm to compute the fundamental or essential matrix from image correspondences.
math::Matrix< double, 3, 3 > FundamentalMatrix
Definition fundamental.h:60
std::vector< Correspondence2D2D > Correspondences2D2D
void pose_from_essential(EssentialMatrix const &matrix, std::vector< CameraPose > *result)
Retrieves the camera matrices from the essential matrix.
math::Matrix< double, 3, 3 > EssentialMatrix
Definition fundamental.h:61
bool is_consistent_pose(Correspondence2D2D const &match, CameraPose const &pose1, CameraPose const &pose2)
Given a two-view pose configuration and a correspondence, this function returns true if the triangula...
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
Definition image.h:478
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
The camera pose is the 3x4 matrix P = K [R | t].
Definition camera_pose.h:40
The resulting initial pair with view IDs and relative camera pose.
Representation of a feature track.
FeatureReferenceList features