MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
ransac_pose_p3p.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 <set>
11#include <atomic>
12#include <iostream>
13
14#include "util/system.h"
15#include "math/matrix_tools.h"
16#include "sfm/ransac_pose_p3p.h"
17#include "sfm/pose_p3p.h"
18
20
21RansacPoseP3P::RansacPoseP3P (Options const& options)
22 : opts(options)
23{
24}
25
26void
28 math::Matrix<double, 3, 3> const& k_matrix, Result* result) const
29{
30 if (this->opts.verbose_output)
31 {
32 std::cout << "RANSAC-3: Running for " << this->opts.max_iterations
33 << " iterations, threshold " << this->opts.threshold
34 << "..." << std::endl;
35 }
36
37 /* Pre-compute inverse K matrix to compute directions from corresp. */
38 math::Matrix<double, 3, 3> inv_k_matrix = math::matrix_inverse(k_matrix);
39 std::atomic<int> num_iterations;
40
41#pragma omp parallel
42 {
43 std::vector<int> inliers;
44 inliers.reserve(corresp.size());
45#pragma omp for
46 for (int i = 0; i < this->opts.max_iterations; ++i)
47 {
48 int iteration = i;
49 if (this->opts.verbose_output)
50 iteration = num_iterations++;
51
52 /* Compute up to four poses [R|t] using P3P algorithm. */
53 PutativePoses poses;
54 this->compute_p3p(corresp, inv_k_matrix, &poses);
55
56 /* Check all putative solutions and count inliers. */
57 for (std::size_t j = 0; j < poses.size(); ++j)
58 {
59 this->find_inliers(corresp, k_matrix, poses[j], &inliers);
60#pragma omp critical
61 if (inliers.size() > result->inliers.size())
62 {
63 result->pose = poses[j];
64 std::swap(result->inliers, inliers);
65 inliers.reserve(corresp.size());
66
67 if (this->opts.verbose_output)
68 {
69 std::cout << "RANSAC-3: Iteration " << iteration
70 << ", inliers " << result->inliers.size() << " ("
71 << (100.0 * result->inliers.size() / corresp.size())
72 << "%)" << std::endl;
73 }
74 }
75 }
76 }
77 }
78}
79
80void
81RansacPoseP3P::compute_p3p (Correspondences2D3D const& corresp,
82 math::Matrix<double, 3, 3> const& inv_k_matrix,
83 PutativePoses* poses) const
84{
85 if (corresp.size() < 3)
86 throw std::invalid_argument("At least 3 correspondences required");
87
88 /* Draw 3 unique random numbers. */
89 std::set<int> result;
90 while (result.size() < 3)
91 result.insert(util::system::rand_int() % corresp.size());
92
93 std::set<int>::const_iterator iter = result.begin();
94 Correspondence2D3D const& c1(corresp[*iter++]);
95 Correspondence2D3D const& c2(corresp[*iter++]);
96 Correspondence2D3D const& c3(corresp[*iter]);
98 math::Vec3d(c1.p3d), math::Vec3d(c2.p3d), math::Vec3d(c3.p3d),
99 inv_k_matrix.mult(math::Vec3d(c1.p2d[0], c1.p2d[1], 1.0)),
100 inv_k_matrix.mult(math::Vec3d(c2.p2d[0], c2.p2d[1], 1.0)),
101 inv_k_matrix.mult(math::Vec3d(c3.p2d[0], c3.p2d[1], 1.0)),
102 poses);
103}
104
105void
106RansacPoseP3P::find_inliers (Correspondences2D3D const& corresp,
107 math::Matrix<double, 3, 3> const& k_matrix,
108 Pose const& pose, std::vector<int>* inliers) const
109{
110 inliers->resize(0);
111 double const square_threshold = MATH_POW2(this->opts.threshold);
112 for (std::size_t i = 0; i < corresp.size(); ++i)
113 {
114 Correspondence2D3D const& c = corresp[i];
115 math::Vec4d p3d(c.p3d[0], c.p3d[1], c.p3d[2], 1.0);
116 math::Vec3d p2d = k_matrix * (pose * p3d);
117 double square_error = MATH_POW2(p2d[0] / p2d[2] - c.p2d[0])
118 + MATH_POW2(p2d[1] / p2d[2] - c.p2d[1]);
119 if (square_error < square_threshold)
120 inliers->push_back(i);
121 }
122}
123
Matrix class for arbitrary dimensions and types.
Definition matrix.h:54
Matrix< T, N, U > mult(Matrix< T, M, U > const &rhs) const
Matrix with matrix multiplication.
Definition matrix.h:462
void estimate(Correspondences2D3D const &corresp, math::Matrix< double, 3, 3 > const &k_matrix, Result *result) const
#define MATH_POW2(x)
Definition defines.h:68
Matrix< T, N, N > matrix_inverse(Matrix< T, N, N > const &mat)
Calculates the inverse of the given matrix.
void pose_p3p_kneip(math::Vec3d p1, math::Vec3d p2, math::Vec3d p3, math::Vec3d f1, math::Vec3d f2, math::Vec3d f3, std::vector< math::Matrix< double, 3, 4 > > *solutions)
Implementation of the perspective three point (P3P) algorithm.
Definition pose_p3p.cc:74
std::vector< Correspondence2D3D > Correspondences2D3D
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
Definition image.h:478
int rand_int(void)
Returns a random number in [0, 2^31].
Definition system.h:126
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13
A 3D point and an image coordinate which correspond to each other in terms of the image observing thi...
bool verbose_output
Produce status messages on the console.
double threshold
Threshold used to determine inliers.
int max_iterations
The number of RANSAC iterations.
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.