1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html.
4
5 #include "../precomp.hpp"
6 #include "../usac.hpp"
7
8 namespace cv { namespace usac {
9 class EpipolarGeometryDegeneracyImpl : public EpipolarGeometryDegeneracy {
10 private:
11 const Mat * points_mat;
12 const float * const points; // i-th row xi1 yi1 xi2 yi2
13 const int min_sample_size;
14 public:
EpipolarGeometryDegeneracyImpl(const Mat & points_,int sample_size_)15 explicit EpipolarGeometryDegeneracyImpl (const Mat &points_, int sample_size_) :
16 points_mat(&points_), points ((float*) points_.data), min_sample_size (sample_size_) {}
17 /*
18 * Do oriented constraint to verify if epipolar geometry is in front or behind the camera.
19 * Return: true if all points are in front of the camers w.r.t. tested epipolar geometry - satisfies constraint.
20 * false - otherwise.
21 * x'^T F x = 0
22 * e' × x' ~+ Fx <=> λe' × x' = Fx, λ > 0
23 * e × x ~+ x'^T F
24 */
isModelValid(const Mat & F_,const std::vector<int> & sample) const25 inline bool isModelValid(const Mat &F_, const std::vector<int> &sample) const override {
26 // F is of rank 2, taking cross product of two rows we obtain null vector of F
27 Vec3d ec_mat = F_.row(0).cross(F_.row(2));
28 auto * ec = ec_mat.val; // of size 3x1
29
30 // e is zero vector, recompute e
31 if (ec[0] <= 1.9984e-15 && ec[0] >= -1.9984e-15 &&
32 ec[1] <= 1.9984e-15 && ec[1] >= -1.9984e-15 &&
33 ec[2] <= 1.9984e-15 && ec[2] >= -1.9984e-15) {
34 ec_mat = F_.row(1).cross(F_.row(2));
35 ec = ec_mat.val;
36 }
37 const auto * const F = (double *) F_.data;
38
39 // without loss of generality, let the first point in sample be in front of the camera.
40 int pt = 4*sample[0];
41 // s1 = F11 * x2 + F21 * y2 + F31 * 1
42 // s2 = e'_2 * 1 - e'_3 * y1
43 // sign1 = s1 * s2
44 const double sign1 = (F[0]*points[pt+2]+F[3]*points[pt+3]+F[6])*(ec[1]-ec[2]*points[pt+1]);
45
46 for (int i = 1; i < min_sample_size; i++) {
47 pt = 4 * sample[i];
48 // if signum of the first point and tested point differs
49 // then two points are on different sides of the camera.
50 if (sign1*(F[0]*points[pt+2]+F[3]*points[pt+3]+F[6])*(ec[1]-ec[2]*points[pt+1])<0)
51 return false;
52 }
53 return true;
54 }
55
clone(int) const56 Ptr<Degeneracy> clone(int /*state*/) const override {
57 return makePtr<EpipolarGeometryDegeneracyImpl>(*points_mat, min_sample_size);
58 }
59 };
recoverRank(Mat & model,bool is_fundamental_mat)60 void EpipolarGeometryDegeneracy::recoverRank (Mat &model, bool is_fundamental_mat) {
61 /*
62 * Do singular value decomposition.
63 * Make last eigen value zero of diagonal matrix of singular values.
64 */
65 Matx33d U, Vt;
66 Vec3d w;
67 SVD::compute(model, w, U, Vt, SVD::MODIFY_A);
68 if (is_fundamental_mat)
69 model = Mat(U * Matx33d(w(0), 0, 0, 0, w(1), 0, 0, 0, 0) * Vt);
70 else {
71 const double mean_singular_val = (w[0] + w[1]) * 0.5;
72 model = Mat(U * Matx33d(mean_singular_val, 0, 0, 0, mean_singular_val, 0, 0, 0, 0) * Vt);
73 }
74 }
create(const Mat & points_,int sample_size_)75 Ptr<EpipolarGeometryDegeneracy> EpipolarGeometryDegeneracy::create (const Mat &points_,
76 int sample_size_) {
77 return makePtr<EpipolarGeometryDegeneracyImpl>(points_, sample_size_);
78 }
79
80 class HomographyDegeneracyImpl : public HomographyDegeneracy {
81 private:
82 const Mat * points_mat;
83 const float * const points;
84 public:
HomographyDegeneracyImpl(const Mat & points_)85 explicit HomographyDegeneracyImpl (const Mat &points_) :
86 points_mat(&points_), points ((float *)points_.data) {}
87
isSampleGood(const std::vector<int> & sample) const88 inline bool isSampleGood (const std::vector<int> &sample) const override {
89 const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2], smpl4 = 4*sample[3];
90 // planar correspondences must lie on the same side of any line from two points in sample
91 const float x1 = points[smpl1], y1 = points[smpl1+1], X1 = points[smpl1+2], Y1 = points[smpl1+3];
92 const float x2 = points[smpl2], y2 = points[smpl2+1], X2 = points[smpl2+2], Y2 = points[smpl2+3];
93 const float x3 = points[smpl3], y3 = points[smpl3+1], X3 = points[smpl3+2], Y3 = points[smpl3+3];
94 const float x4 = points[smpl4], y4 = points[smpl4+1], X4 = points[smpl4+2], Y4 = points[smpl4+3];
95 // line from points 1 and 2
96 const float ab_cross_x = y1 - y2, ab_cross_y = x2 - x1, ab_cross_z = x1 * y2 - y1 * x2;
97 const float AB_cross_x = Y1 - Y2, AB_cross_y = X2 - X1, AB_cross_z = X1 * Y2 - Y1 * X2;
98
99 // check if points 3 and 4 are on the same side of line ab on both images
100 if ((ab_cross_x * x3 + ab_cross_y * y3 + ab_cross_z) *
101 (AB_cross_x * X3 + AB_cross_y * Y3 + AB_cross_z) < 0)
102 return false;
103 if ((ab_cross_x * x4 + ab_cross_y * y4 + ab_cross_z) *
104 (AB_cross_x * X4 + AB_cross_y * Y4 + AB_cross_z) < 0)
105 return false;
106
107 // line from points 3 and 4
108 const float cd_cross_x = y3 - y4, cd_cross_y = x4 - x3, cd_cross_z = x3 * y4 - y3 * x4;
109 const float CD_cross_x = Y3 - Y4, CD_cross_y = X4 - X3, CD_cross_z = X3 * Y4 - Y3 * X4;
110
111 // check if points 1 and 2 are on the same side of line cd on both images
112 if ((cd_cross_x * x1 + cd_cross_y * y1 + cd_cross_z) *
113 (CD_cross_x * X1 + CD_cross_y * Y1 + CD_cross_z) < 0)
114 return false;
115 if ((cd_cross_x * x2 + cd_cross_y * y2 + cd_cross_z) *
116 (CD_cross_x * X2 + CD_cross_y * Y2 + CD_cross_z) < 0)
117 return false;
118
119 // Checks if points are not collinear
120 // If area of triangle constructed with 3 points is less then threshold then points are collinear:
121 // |x1 y1 1| |x1 y1 1|
122 // (1/2) det |x2 y2 1| = (1/2) det |x2-x1 y2-y1 0| = (1/2) det |x2-x1 y2-y1| < threshold
123 // |x3 y3 1| |x3-x1 y3-y1 0| |x3-x1 y3-y1|
124 // for points on the first image
125 if (fabsf((x2-x1) * (y3-y1) - (y2-y1) * (x3-x1)) * 0.5 < FLT_EPSILON) return false; //1,2,3
126 if (fabsf((x2-x1) * (y4-y1) - (y2-y1) * (x4-x1)) * 0.5 < FLT_EPSILON) return false; //1,2,4
127 if (fabsf((x3-x1) * (y4-y1) - (y3-y1) * (x4-x1)) * 0.5 < FLT_EPSILON) return false; //1,3,4
128 if (fabsf((x3-x2) * (y4-y2) - (y3-y2) * (x4-x2)) * 0.5 < FLT_EPSILON) return false; //2,3,4
129 // for points on the second image
130 if (fabsf((X2-X1) * (Y3-Y1) - (Y2-Y1) * (X3-X1)) * 0.5 < FLT_EPSILON) return false; //1,2,3
131 if (fabsf((X2-X1) * (Y4-Y1) - (Y2-Y1) * (X4-X1)) * 0.5 < FLT_EPSILON) return false; //1,2,4
132 if (fabsf((X3-X1) * (Y4-Y1) - (Y3-Y1) * (X4-X1)) * 0.5 < FLT_EPSILON) return false; //1,3,4
133 if (fabsf((X3-X2) * (Y4-Y2) - (Y3-Y2) * (X4-X2)) * 0.5 < FLT_EPSILON) return false; //2,3,4
134
135 return true;
136 }
clone(int) const137 Ptr<Degeneracy> clone(int /*state*/) const override {
138 return makePtr<HomographyDegeneracyImpl>(*points_mat);
139 }
140 };
create(const Mat & points_)141 Ptr<HomographyDegeneracy> HomographyDegeneracy::create (const Mat &points_) {
142 return makePtr<HomographyDegeneracyImpl>(points_);
143 }
144
145 ///////////////////////////////// Fundamental Matrix Degeneracy ///////////////////////////////////
146 class FundamentalDegeneracyImpl : public FundamentalDegeneracy {
147 private:
148 RNG rng;
149 const Ptr<Quality> quality;
150 const float * const points;
151 const Mat * points_mat;
152 const Ptr<ReprojectionErrorForward> h_reproj_error;
153 Ptr<HomographyNonMinimalSolver> h_non_min_solver;
154 const EpipolarGeometryDegeneracyImpl ep_deg;
155 // threshold to find inliers for homography model
156 const double homography_threshold, log_conf = log(0.05);
157 // points (1-7) to verify in sample
158 std::vector<std::vector<int>> h_sample {{0,1,2},{3,4,5},{0,1,6},{3,4,6},{2,5,6}};
159 std::vector<int> h_inliers;
160 std::vector<double> weights;
161 std::vector<Mat> h_models;
162 const int points_size, sample_size;
163 public:
164
FundamentalDegeneracyImpl(int state,const Ptr<Quality> & quality_,const Mat & points_,int sample_size_,double homography_threshold_)165 FundamentalDegeneracyImpl (int state, const Ptr<Quality> &quality_, const Mat &points_,
166 int sample_size_, double homography_threshold_) :
167 rng (state), quality(quality_), points((float *) points_.data), points_mat(&points_),
168 h_reproj_error(ReprojectionErrorForward::create(points_)),
169 ep_deg (points_, sample_size_), homography_threshold (homography_threshold_),
170 points_size (quality_->getPointsSize()), sample_size (sample_size_) {
171 if (sample_size_ == 8) {
172 // add more homography samples to test for 8-points F
173 h_sample.emplace_back(std::vector<int>{0, 1, 7});
174 h_sample.emplace_back(std::vector<int>{0, 2, 7});
175 h_sample.emplace_back(std::vector<int>{3, 5, 7});
176 h_sample.emplace_back(std::vector<int>{3, 6, 7});
177 h_sample.emplace_back(std::vector<int>{2, 4, 7});
178 }
179 h_inliers = std::vector<int>(points_size);
180 h_non_min_solver = HomographyNonMinimalSolver::create(points_);
181 }
isModelValid(const Mat & F,const std::vector<int> & sample) const182 inline bool isModelValid(const Mat &F, const std::vector<int> &sample) const override {
183 return ep_deg.isModelValid(F, sample);
184 }
recoverIfDegenerate(const std::vector<int> & sample,const Mat & F_best,Mat & non_degenerate_model,Score & non_degenerate_model_score)185 bool recoverIfDegenerate (const std::vector<int> &sample, const Mat &F_best,
186 Mat &non_degenerate_model, Score &non_degenerate_model_score) override {
187 non_degenerate_model_score = Score(); // set worst case
188
189 // According to Two-view Geometry Estimation Unaffected by a Dominant Plane
190 // (http://cmp.felk.cvut.cz/~matas/papers/chum-degen-cvpr05.pdf)
191 // only 5 homographies enough to test
192 // triplets {1,2,3}, {4,5,6}, {1,2,7}, {4,5,7} and {3,6,7}
193
194 // H = A - e' (M^-1 b)^T
195 // A = [e']_x F
196 // b_i = (x′i × (A xi))^T (x′i × e′)‖x′i×e′‖^−2,
197 // M is a 3×3 matrix with rows x^T_i
198 // epipole e' is left nullspace of F s.t. e′^T F=0,
199
200 // find e', null space of F^T
201 Vec3d e_prime = F_best.col(0).cross(F_best.col(2));
202 if (fabs(e_prime(0)) < 1e-10 && fabs(e_prime(1)) < 1e-10 &&
203 fabs(e_prime(2)) < 1e-10) // if e' is zero
204 e_prime = F_best.col(1).cross(F_best.col(2));
205
206 const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
207
208 Vec3d xi_prime(0,0,1), xi(0,0,1), b;
209 Matx33d M(0,0,1,0,0,1,0,0,1); // last column of M is 1
210
211 bool is_model_degenerate = false;
212 for (const auto &h_i : h_sample) { // only 5 samples
213 for (int pt_i = 0; pt_i < 3; pt_i++) {
214 // find b and M
215 const int smpl = 4*sample[h_i[pt_i]];
216 xi[0] = points[smpl];
217 xi[1] = points[smpl+1];
218 xi_prime[0] = points[smpl+2];
219 xi_prime[1] = points[smpl+3];
220
221 // (x′i × e')
222 const Vec3d xprime_X_eprime = xi_prime.cross(e_prime);
223
224 // (x′i × (A xi))
225 const Vec3d xprime_X_Ax = xi_prime.cross(A * xi);
226
227 // x′i × (A xi))^T (x′i × e′) / ‖x′i×e′‖^2,
228 b[pt_i] = xprime_X_Ax.dot(xprime_X_eprime) /
229 std::pow(norm(xprime_X_eprime), 2);
230
231 // M from x^T
232 M(pt_i, 0) = xi[0];
233 M(pt_i, 1) = xi[1];
234 }
235
236 // compute H
237 Matx33d H = A - e_prime * (M.inv() * b).t();
238
239 int inliers_out_plane = 0;
240 h_reproj_error->setModelParameters(Mat(H));
241
242 // find inliers from sample, points related to H, x' ~ Hx
243 for (int s = 0; s < sample_size; s++)
244 if (h_reproj_error->getError(sample[s]) > homography_threshold)
245 if (++inliers_out_plane > 2)
246 break;
247
248 // if there are at least 5 points lying on plane then F is degenerate
249 if (inliers_out_plane <= 2) {
250 is_model_degenerate = true;
251
252 // update homography by polishing on all inliers
253 int h_inls_cnt = 0;
254 const auto &h_errors = h_reproj_error->getErrors(Mat(H));
255 for (int pt = 0; pt < points_size; pt++)
256 if (h_errors[pt] < homography_threshold)
257 h_inliers[h_inls_cnt++] = pt;
258 if (h_non_min_solver->estimate(h_inliers, h_inls_cnt, h_models, weights) != 0)
259 H = Matx33d(h_models[0]);
260
261 Mat newF;
262 const Score newF_score = planeAndParallaxRANSAC(H, newF, h_errors);
263 if (newF_score.isBetter(non_degenerate_model_score)) {
264 // store non degenerate model
265 non_degenerate_model_score = newF_score;
266 newF.copyTo(non_degenerate_model);
267 }
268 }
269 }
270 return is_model_degenerate;
271 }
clone(int state) const272 Ptr<Degeneracy> clone(int state) const override {
273 return makePtr<FundamentalDegeneracyImpl>(state, quality->clone(), *points_mat,
274 sample_size, homography_threshold);
275 }
276 private:
277 // RANSAC with plane-and-parallax to find new Fundamental matrix
planeAndParallaxRANSAC(const Matx33d & H,Mat & best_F,const std::vector<float> & h_errors)278 Score planeAndParallaxRANSAC (const Matx33d &H, Mat &best_F, const std::vector<float> &h_errors) {
279 int max_iters = 100; // with 95% confidence assume at least 17% of inliers
280 Score best_score;
281 for (int iters = 0; iters < max_iters; iters++) {
282 // draw two random points
283 int h_outlier1 = rng.uniform(0, points_size);
284 int h_outlier2 = rng.uniform(0, points_size);
285 while (h_outlier1 == h_outlier2)
286 h_outlier2 = rng.uniform(0, points_size);
287
288 // find outliers of homography H
289 if (h_errors[h_outlier1] > homography_threshold &&
290 h_errors[h_outlier2] > homography_threshold) {
291
292 // do plane and parallax with outliers of H
293 // F = [(p1' x Hp1) x (p2' x Hp2)]_x H
294 const Matx33d F = Math::getSkewSymmetric(
295 (Vec3d(points[4*h_outlier1+2], points[4*h_outlier1+3], 1).cross // p1'
296 (H * Vec3d(points[4*h_outlier1 ], points[4*h_outlier1+1], 1))).cross // Hp1
297 (Vec3d(points[4*h_outlier2+2], points[4*h_outlier2+3], 1).cross // p2'
298 (H * Vec3d(points[4*h_outlier2 ], points[4*h_outlier2+1], 1))) // Hp2
299 ) * H;
300
301 const Score score = quality->getScore(Mat(F));
302 if (score.isBetter(best_score)) {
303 best_score = score;
304 best_F = Mat(F);
305 const double predicted_iters = log_conf / log(1 - std::pow
306 (static_cast<double>(score.inlier_number) / points_size, 2));
307
308 if (! std::isinf(predicted_iters) && predicted_iters < max_iters)
309 max_iters = static_cast<int>(predicted_iters);
310 }
311 }
312 }
313 return best_score;
314 }
315 };
create(int state,const Ptr<Quality> & quality_,const Mat & points_,int sample_size_,double homography_threshold_)316 Ptr<FundamentalDegeneracy> FundamentalDegeneracy::create (int state, const Ptr<Quality> &quality_,
317 const Mat &points_, int sample_size_, double homography_threshold_) {
318 return makePtr<FundamentalDegeneracyImpl>(state, quality_, points_, sample_size_,
319 homography_threshold_);
320 }
321
322 class EssentialDegeneracyImpl : public EssentialDegeneracy {
323 private:
324 const Mat * points_mat;
325 const int sample_size;
326 const EpipolarGeometryDegeneracyImpl ep_deg;
327 public:
EssentialDegeneracyImpl(const Mat & points,int sample_size_)328 explicit EssentialDegeneracyImpl (const Mat &points, int sample_size_) :
329 points_mat(&points), sample_size(sample_size_), ep_deg (points, sample_size_) {}
isModelValid(const Mat & E,const std::vector<int> & sample) const330 inline bool isModelValid(const Mat &E, const std::vector<int> &sample) const override {
331 return ep_deg.isModelValid(E, sample);
332 }
clone(int) const333 Ptr<Degeneracy> clone(int /*state*/) const override {
334 return makePtr<EssentialDegeneracyImpl>(*points_mat, sample_size);
335 }
336 };
create(const Mat & points_,int sample_size_)337 Ptr<EssentialDegeneracy> EssentialDegeneracy::create (const Mat &points_, int sample_size_) {
338 return makePtr<EssentialDegeneracyImpl>(points_, sample_size_);
339 }
340 }}
341