21RansacPoseP3P::RansacPoseP3P (
Options const& options)
33 <<
" iterations, threshold " << this->opts.
threshold
34 <<
"..." << std::endl;
39 std::atomic<int> num_iterations;
43 std::vector<int> inliers;
44 inliers.reserve(corresp.size());
50 iteration = num_iterations++;
54 this->compute_p3p(corresp, inv_k_matrix, &poses);
57 for (std::size_t j = 0; j < poses.size(); ++j)
59 this->find_inliers(corresp, k_matrix, poses[j], &inliers);
61 if (inliers.size() > result->
inliers.size())
63 result->
pose = poses[j];
65 inliers.reserve(corresp.size());
69 std::cout <<
"RANSAC-3: Iteration " << iteration
70 <<
", inliers " << result->
inliers.size() <<
" ("
71 << (100.0 * result->
inliers.size() / corresp.size())
83 PutativePoses* poses)
const
85 if (corresp.size() < 3)
86 throw std::invalid_argument(
"At least 3 correspondences required");
90 while (result.size() < 3)
93 std::set<int>::const_iterator iter = result.begin();
108 Pose
const& pose, std::vector<int>* inliers)
const
112 for (std::size_t i = 0; i < corresp.size(); ++i)
114 Correspondence2D3D
const& c = corresp[i];
115 math::Vec4d p3d(c.p3d[0], c.p3d[1], c.p3d[2], 1.0);
117 double square_error =
MATH_POW2(p2d[0] / p2d[2] - c.p2d[0])
119 if (square_error < square_threshold)
120 inliers->push_back(i);
Matrix class for arbitrary dimensions and types.
Matrix< T, N, U > mult(Matrix< T, M, U > const &rhs) const
Matrix with matrix multiplication.
void estimate(Correspondences2D3D const &corresp, math::Matrix< double, 3, 3 > const &k_matrix, Result *result) const
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.
std::vector< Correspondence2D3D > Correspondences2D3D
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
int rand_int(void)
Returns a random number in [0, 2^31].
#define SFM_NAMESPACE_END
#define SFM_NAMESPACE_BEGIN
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.