23InitialPair::compute_pair (
Result* result)
25 if (this->viewports ==
nullptr || this->tracks ==
nullptr)
26 throw std::invalid_argument(
"Null viewports or tracks");
28 std::cout <<
"Searching for initial pair..." << std::endl;
33 std::vector<CandidatePair> candidates;
34 this->compute_candidate_pairs(&candidates);
37 std::sort(candidates.rbegin(), candidates.rend());
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)
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))
59 this->debug_output(candidate);
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)
68 this->debug_output(candidate, num_inliers);
74 bool const found_pose = this->compute_pose(candidate, &pose1, &pose2);
77 this->debug_output(candidate, num_inliers);
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)
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)
99 positions[0] =
math::Vec2f(candidate.matches[j].p1);
100 positions[1] =
math::Vec2f(candidate.matches[j].p2);
102 if (triangulator.
triangulate(poses, positions, &pos3d, &stats))
103 successful_triangulations += 1;
105 if (successful_triangulations * 2 < candidate.matches.size())
109 if (i < found_pair_id)
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)
130 if (pair_scores[i] <= best_score)
133 best_score = pair_scores[i];
138 if (best_score > 0.0f)
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],
148InitialPair::compute_pair (
int view_1_id,
int view_2_id,
Result* result)
150 if (view_1_id > view_2_id)
154 std::vector<CandidatePair> candidates;
155 this->compute_candidate_pairs(&candidates);
158 CandidatePair* candidate =
nullptr;
159 for (std::size_t i = 0; candidate ==
nullptr && i < candidates.size(); ++i)
161 if (view_1_id == candidates[i].view_1_id
162 && view_2_id == candidates[i].view_2_id)
163 candidate = &candidates[i];
165 if (candidate ==
nullptr)
166 throw std::runtime_error(
"No matches for initial pair");
171 bool const found_pose = this->compute_pose(*candidate,
174 throw std::runtime_error(
"Cannot compute pose for initial pair");
178InitialPair::compute_candidate_pairs (CandidatePairs* candidates)
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)
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)
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;
205 int const lookup_id = v1id * num_viewports + v2id;
206 int pair_id = candidate_lookup[lookup_id];
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;
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);
230InitialPair::compute_homography_inliers (CandidatePair
const& candidate)
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();
240InitialPair::compute_pose (CandidatePair
const& candidate,
241 CameraPose* pose1, CameraPose* pose2)
247 if (matches.size() > 1000ul) {
249 std::shuffle(matches.begin(), matches.end(), g);
250 matches.resize(1000ul);
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);
267 std::vector<CameraPose> poses;
271 bool found_pose =
false;
272 for (std::size_t i = 0; i < poses.size(); ++i)
274 poses[i].K = pose2->K;
286InitialPair::angle_for_pose (CandidatePair
const& candidate,
287 CameraPose
const& pose1, CameraPose
const& pose2)
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)
298 Correspondence2D2D
const& match = candidate.matches[i];
300 p1 = T1.
mult(p1).normalized();
302 p2 = T2.
mult(p2).normalized();
303 cos_angles.push_back(p1.dot(p2));
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);
315InitialPair::score_for_pair (CandidatePair
const& candidate,
316 std::size_t num_inliers,
double angle)
318 float const matches =
static_cast<float>(candidate.matches.size());
319 float const inliers =
static_cast<float>(num_inliers) / matches;
323 float f1 = 2.0 / (1.0 + std::exp((20.0 - matches) * 6.0 / 200.0)) - 1.0;
325 float f2 = 2.0 / (1.0 + std::exp((1.0 - angle_d) * 6.0 / 8.0)) - 1.0;
327 float f3 = 2.0 / (1.0 + std::exp((inliers - 0.7) * 6.0 / 0.4)) - 1.0;
336InitialPair::debug_output (CandidatePair
const& candidate,
337 std::size_t num_inliers,
double angle)
339 if (!this->opts.verbose_output)
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";
350 float percentage =
static_cast<float>(num_inliers) / num_matches;
351 message <<
", " << std::setw(4) << num_inliers
352 <<
" H-inliers (" << (int)(100.0f * percentage) <<
"%)";
357 message <<
", " << std::setw(5)
363 std::cout << message.str() << std::endl;
Matrix< T, N, U > mult(Matrix< T, M, U > const &rhs) const
Matrix with matrix multiplication.
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
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
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'.
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
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
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.
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
The camera pose is the 3x4 matrix P = K [R | t].
The resulting initial pair with view IDs and relative camera pose.
Representation of a feature track.
FeatureReferenceList features