File size: 36,021 Bytes
dffe123 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 | // This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "../precomp.hpp"
#include "../usac.hpp"
namespace cv { namespace usac {
class EpipolarGeometryDegeneracyImpl : public EpipolarGeometryDegeneracy {
private:
Mat points_mat;
const int min_sample_size;
public:
explicit EpipolarGeometryDegeneracyImpl (const Mat &points_, int sample_size_) :
points_mat(points_), min_sample_size (sample_size_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
/*
* Do oriented constraint to verify if epipolar geometry is in front or behind the camera.
* Return: true if all points are in front of the camers w.r.t. tested epipolar geometry - satisfies constraint.
* false - otherwise.
* x'^T F x = 0
* e' × x' ~+ Fx <=> λe' × x' = Fx, λ > 0
* e × x ~+ x'^T F
*/
inline bool isModelValid(const Mat &F_, const std::vector<int> &sample) const override {
const Vec3d ep = Utils::getRightEpipole(F_);
const auto * const e = ep.val; // of size 3x1
const auto * const F = (double *) F_.data;
const float * points = points_mat.ptr<float>();
// without loss of generality, let the first point in sample be in front of the camera.
int pt = 4*sample[0];
// check only two first elements of vectors (e × x) and (x'^T F)
// s1 = (x'^T F)[0] = x2 * F11 + y2 * F21 + 1 * F31
// s2 = (e × x)[0] = e'_2 * 1 - e'_3 * y1
// sign1 = s1 * s2
const double sign1 = (F[0]*points[pt+2]+F[3]*points[pt+3]+F[6])*(e[1]-e[2]*points[pt+1]);
for (int i = 1; i < min_sample_size; i++) {
pt = 4 * sample[i];
// if signum of the first point and tested point differs
// then two points are on different sides of the camera.
if (sign1*(F[0]*points[pt+2]+F[3]*points[pt+3]+F[6])*(e[1]-e[2]*points[pt+1])<0)
return false;
}
return true;
}
};
void EpipolarGeometryDegeneracy::recoverRank (Mat &model, bool is_fundamental_mat) {
/*
* Do singular value decomposition.
* Make last eigen value zero of diagonal matrix of singular values.
*/
Matx33d U, Vt;
Vec3d w;
SVD::compute(model, w, U, Vt, SVD::MODIFY_A);
if (is_fundamental_mat)
model = Mat(U * Matx33d(w(0), 0, 0, 0, w(1), 0, 0, 0, 0) * Vt);
else {
const double mean_singular_val = (w[0] + w[1]) * 0.5;
model = Mat(U * Matx33d(mean_singular_val, 0, 0, 0, mean_singular_val, 0, 0, 0, 0) * Vt);
}
}
Ptr<EpipolarGeometryDegeneracy> EpipolarGeometryDegeneracy::create (const Mat &points_,
int sample_size_) {
return makePtr<EpipolarGeometryDegeneracyImpl>(points_, sample_size_);
}
class HomographyDegeneracyImpl : public HomographyDegeneracy {
private:
Mat points_mat;
const float TOLERANCE = 2 * FLT_EPSILON; // 2 from area of triangle
public:
explicit HomographyDegeneracyImpl (const Mat &points_) :
points_mat(points_)
{
CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
}
inline bool isSampleGood (const std::vector<int> &sample) const override {
const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2], smpl4 = 4*sample[3];
// planar correspondences must lie on the same side of any line from two points in sample
const float * points = points_mat.ptr<float>();
const float x1 = points[smpl1], y1 = points[smpl1+1], X1 = points[smpl1+2], Y1 = points[smpl1+3];
const float x2 = points[smpl2], y2 = points[smpl2+1], X2 = points[smpl2+2], Y2 = points[smpl2+3];
const float x3 = points[smpl3], y3 = points[smpl3+1], X3 = points[smpl3+2], Y3 = points[smpl3+3];
const float x4 = points[smpl4], y4 = points[smpl4+1], X4 = points[smpl4+2], Y4 = points[smpl4+3];
// line from points 1 and 2
const float ab_cross_x = y1 - y2, ab_cross_y = x2 - x1, ab_cross_z = x1 * y2 - y1 * x2;
const float AB_cross_x = Y1 - Y2, AB_cross_y = X2 - X1, AB_cross_z = X1 * Y2 - Y1 * X2;
// check if points 3 and 4 are on the same side of line ab on both images
if ((ab_cross_x * x3 + ab_cross_y * y3 + ab_cross_z) *
(AB_cross_x * X3 + AB_cross_y * Y3 + AB_cross_z) < 0)
return false;
if ((ab_cross_x * x4 + ab_cross_y * y4 + ab_cross_z) *
(AB_cross_x * X4 + AB_cross_y * Y4 + AB_cross_z) < 0)
return false;
// line from points 3 and 4
const float cd_cross_x = y3 - y4, cd_cross_y = x4 - x3, cd_cross_z = x3 * y4 - y3 * x4;
const float CD_cross_x = Y3 - Y4, CD_cross_y = X4 - X3, CD_cross_z = X3 * Y4 - Y3 * X4;
// check if points 1 and 2 are on the same side of line cd on both images
if ((cd_cross_x * x1 + cd_cross_y * y1 + cd_cross_z) *
(CD_cross_x * X1 + CD_cross_y * Y1 + CD_cross_z) < 0)
return false;
if ((cd_cross_x * x2 + cd_cross_y * y2 + cd_cross_z) *
(CD_cross_x * X2 + CD_cross_y * Y2 + CD_cross_z) < 0)
return false;
// Checks if points are not collinear
// If area of triangle constructed with 3 points is less than threshold then points are collinear:
// |x1 y1 1| |x1 y1 1|
// (1/2) det |x2 y2 1| = (1/2) det |x2-x1 y2-y1 0| = det |x2-x1 y2-y1| < 2 * threshold
// |x3 y3 1| |x3-x1 y3-y1 0| |x3-x1 y3-y1|
// for points on the first image
if (fabsf((x2-x1) * (y3-y1) - (y2-y1) * (x3-x1)) < TOLERANCE) return false; //1,2,3
if (fabsf((x2-x1) * (y4-y1) - (y2-y1) * (x4-x1)) < TOLERANCE) return false; //1,2,4
if (fabsf((x3-x1) * (y4-y1) - (y3-y1) * (x4-x1)) < TOLERANCE) return false; //1,3,4
if (fabsf((x3-x2) * (y4-y2) - (y3-y2) * (x4-x2)) < TOLERANCE) return false; //2,3,4
// for points on the second image
if (fabsf((X2-X1) * (Y3-Y1) - (Y2-Y1) * (X3-X1)) < TOLERANCE) return false; //1,2,3
if (fabsf((X2-X1) * (Y4-Y1) - (Y2-Y1) * (X4-X1)) < TOLERANCE) return false; //1,2,4
if (fabsf((X3-X1) * (Y4-Y1) - (Y3-Y1) * (X4-X1)) < TOLERANCE) return false; //1,3,4
if (fabsf((X3-X2) * (Y4-Y2) - (Y3-Y2) * (X4-X2)) < TOLERANCE) return false; //2,3,4
return true;
}
};
Ptr<HomographyDegeneracy> HomographyDegeneracy::create (const Mat &points_) {
return makePtr<HomographyDegeneracyImpl>(points_);
}
class FundamentalDegeneracyViaEImpl : public FundamentalDegeneracyViaE {
private:
bool is_F_objective;
std::vector<std::vector<int>> instances = {{0,1,2,3,4}, {2,3,4,5,6}, {0,1,4,5,6}};
std::vector<int> e_sample;
const Ptr<Quality> quality;
Ptr<EpipolarGeometryDegeneracy> e_degen, f_degen;
Ptr<EssentialMinimalSolver5pts> e_solver;
std::vector<Mat> e_models;
const int E_SAMPLE_SIZE = 5;
Matx33d K2_inv_t, K1_inv;
public:
FundamentalDegeneracyViaEImpl (const Ptr<Quality> &quality_, const Mat &pts, const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective)
: quality(quality_) {
is_F_objective = is_f_objective;
e_solver = EssentialMinimalSolver5pts::create(calib_pts, false/*svd*/, true/*nister*/);
f_degen = is_F_objective ? EpipolarGeometryDegeneracy::create(pts, 7) : EpipolarGeometryDegeneracy::create(calib_pts, 7);
e_degen = EpipolarGeometryDegeneracy::create(calib_pts, E_SAMPLE_SIZE);
e_sample = std::vector<int>(E_SAMPLE_SIZE);
if (is_f_objective) {
K2_inv_t = K2.inv().t();
K1_inv = K1.inv();
}
}
bool isModelValid (const Mat &F, const std::vector<int> &sample) const override {
return f_degen->isModelValid(F, sample);
}
bool recoverIfDegenerate (const std::vector<int> &sample_7pt, const Mat &/*best*/, const Score &best_score,
Mat &out_model, Score &out_score) override {
out_score = Score();
for (const auto &instance : instances) {
for (int i = 0; i < E_SAMPLE_SIZE; i++)
e_sample[i] = sample_7pt[instance[i]];
const int num_models = e_solver->estimate(e_sample, e_models);
for (int i = 0; i < num_models; i++) {
if (e_degen->isModelValid(e_models[i], e_sample)) {
const Mat model = is_F_objective ? Mat(K2_inv_t * Matx33d(e_models[i]) * K1_inv) : e_models[i];
const auto sc = quality->getScore(model);
if (sc.isBetter(out_score)) {
out_score = sc;
model.copyTo(out_model);
}
}
}
if (out_score.isBetter(best_score)) break;
}
return true;
}
};
Ptr<FundamentalDegeneracyViaE> FundamentalDegeneracyViaE::create (const Ptr<Quality> &quality, const Mat &pts,
const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective) {
return makePtr<FundamentalDegeneracyViaEImpl>(quality, pts, calib_pts, K1, K2, is_f_objective);
}
///////////////////////////////// Fundamental Matrix Degeneracy ///////////////////////////////////
class FundamentalDegeneracyImpl : public FundamentalDegeneracy {
private:
RNG rng;
const Ptr<Quality> quality;
const Ptr<Error> f_error;
Ptr<Quality> h_repr_quality;
Mat points_mat;
const Ptr<ReprojectionErrorForward> h_reproj_error;
Ptr<EpipolarNonMinimalSolver> f_non_min_solver;
Ptr<HomographyNonMinimalSolver> h_non_min_solver;
Ptr<UniformRandomGenerator> random_gen_H;
const EpipolarGeometryDegeneracyImpl ep_deg;
// threshold to find inliers for homography model
const double homography_threshold, log_conf = log(0.05), H_SAMPLE_THR_SQR = 49/*7^2*/, MAX_H_THR = 225/*15^2*/;
double f_threshold_sqr, best_focal = -1;
// points (1-7) to verify in sample
std::vector<std::vector<int>> h_sample {{0,1,2},{3,4,5},{0,1,6},{3,4,6},{2,5,6}};
std::vector<std::vector<int>> h_sample_ver {{3,4,5,6},{0,1,2,6},{2,3,4,5},{0,1,2,5},{0,1,3,4}};
std::vector<int> non_planar_supports, h_inliers, h_outliers, h_outliers_eval, f_inliers;
std::vector<double> weights;
std::vector<Mat> h_models;
const int points_size, max_iters_plane_and_parallax, MAX_H_SUBSET = 50, MAX_ITERS_H = 6;
int num_h_outliers, num_models_used_so_far = 0, estimated_min_non_planar_support,
num_h_outliers_eval, TENT_MIN_NON_PLANAR_SUPP;
const int MAX_MODELS_TO_TEST = 21, H_INLS_DEGEN_SAMPLE = 5; // 5 by DEGENSAC, Chum et al.
Matx33d K, K2, K_inv, K2_inv, K2_inv_t, true_K2_inv, true_K2_inv_t, true_K1_inv, true_K1, true_K2_t;
Score best_focal_score;
bool true_K_given, is_principal_pt_set = false;
public:
FundamentalDegeneracyImpl (int state, const Ptr<Quality> &quality_, const Mat &points_,
int sample_size_, int plane_and_parallax_iters, double homography_threshold_,
double f_inlier_thr_sqr, const Mat true_K1_, const Mat true_K2_) :
rng (state), quality(quality_), f_error(quality_->getErrorFnc()), points_mat(points_),
h_reproj_error(ReprojectionErrorForward::create(points_)),
ep_deg (points_, sample_size_), homography_threshold (homography_threshold_),
points_size (quality_->getPointsSize()),
max_iters_plane_and_parallax(plane_and_parallax_iters) {
if (sample_size_ == 8) {
// add more homography samples to test for 8-points F
h_sample.emplace_back(std::vector<int>{0, 1, 7}); h_sample_ver.emplace_back(std::vector<int>{2,3,4,5,6});
h_sample.emplace_back(std::vector<int>{0, 2, 7}); h_sample_ver.emplace_back(std::vector<int>{1,3,4,5,6});
h_sample.emplace_back(std::vector<int>{3, 5, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,2,4,6});
h_sample.emplace_back(std::vector<int>{3, 6, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,2,4,5});
h_sample.emplace_back(std::vector<int>{2, 4, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,3,5,6});
}
non_planar_supports = std::vector<int>(MAX_MODELS_TO_TEST);
h_inliers = std::vector<int>(points_size);
h_outliers = std::vector<int>(points_size);
h_outliers_eval = std::vector<int>(points_size);
f_inliers = std::vector<int>(points_size);
h_non_min_solver = HomographyNonMinimalSolver::create(points_);
f_non_min_solver = EpipolarNonMinimalSolver::create(points_, true /*F*/);
num_h_outliers_eval = num_h_outliers = points_size;
f_threshold_sqr = f_inlier_thr_sqr;
h_repr_quality = MsacQuality::create(points_.rows, homography_threshold_, h_reproj_error);
true_K_given = ! true_K1_.empty() && ! true_K2_.empty();
if (true_K_given) {
true_K1 = Matx33d((double *)true_K1_.data);
true_K2_inv = Matx33d(Mat(true_K2_.inv()));
true_K2_t = Matx33d(true_K2_).t();
true_K1_inv = true_K1.inv();
true_K2_inv_t = true_K2_inv.t();
}
random_gen_H = UniformRandomGenerator::create(rng.uniform(0, INT_MAX), points_size, MAX_H_SUBSET);
estimated_min_non_planar_support = TENT_MIN_NON_PLANAR_SUPP = std::min(5, (int)(0.05*points_size));
}
bool estimateHfrom3Points (const Mat &F_best, const std::vector<int> &sample, Mat &H_best) {
Score H_best_score;
// find e', null vector of F^T
const Vec3d e_prime = Utils::getLeftEpipole(F_best);
const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
bool is_degenerate = false;
int idx = -1;
for (const auto &h_i : h_sample) { // only 5 samples
idx++;
Matx33d H;
if (!getH(A, e_prime, 4 * sample[h_i[0]], 4 * sample[h_i[1]], 4 * sample[h_i[2]], H))
continue;
h_reproj_error->setModelParameters(Mat(H));
const auto &ver_pts = h_sample_ver[idx];
int inliers_in_plane = 3; // 3 points are always inliers
for (int s : ver_pts)
if (h_reproj_error->getError(sample[s]) < homography_threshold) {
if (++inliers_in_plane >= H_INLS_DEGEN_SAMPLE)
break;
}
if (inliers_in_plane >= H_INLS_DEGEN_SAMPLE) {
is_degenerate = true;
const auto h_score = h_repr_quality->getScore(Mat(H));
if (h_score.isBetter(H_best_score)) {
H_best_score = h_score;
H_best = Mat(H);
}
}
}
if (!is_degenerate)
return false;
int h_inls_cnt = optimizeH(H_best, H_best_score);
for (int iter = 0; iter < 2; iter++) {
if (h_non_min_solver->estimate(h_inliers, h_inls_cnt, h_models, weights) == 0)
break;
const auto h_score = h_repr_quality->getScore(h_models[0]);
if (h_score.isBetter(H_best_score)) {
H_best_score = h_score;
h_models[0].copyTo(H_best);
h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
} else break;
}
getOutliersH(H_best);
return true;
}
bool optimizeF (const Mat &F, const Score &score, Mat &F_new, Score &new_score) {
std::vector<Mat> Fs;
if (f_non_min_solver->estimate(f_inliers, quality->getInliers(F, f_inliers), Fs, weights)) {
const auto F_polished_score = quality->getScore(f_error->getErrors(Fs[0]));
if (F_polished_score.isBetter(score)) {
Fs[0].copyTo(F_new); new_score = F_polished_score;
return true;
}
}
return false;
}
int optimizeH (Mat &H_best, Score &H_best_score) {
int h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
random_gen_H->setSubsetSize(h_inls_cnt <= MAX_H_SUBSET ? (int)(0.8*h_inls_cnt) : MAX_H_SUBSET);
if (random_gen_H->getSubsetSize() >= 4/*min H sample size*/) {
for (int iter = 0; iter < MAX_ITERS_H; iter++) {
if (h_non_min_solver->estimate(random_gen_H->generateUniqueRandomSubset(h_inliers, h_inls_cnt), random_gen_H->getSubsetSize(), h_models, weights) == 0)
continue;
const auto h_score = h_repr_quality->getScore(h_models[0]);
if (h_score.isBetter(H_best_score)) {
h_models[0].copyTo(H_best);
// if more inliers than previous best
if (h_score.inlier_number > H_best_score.inlier_number || h_score.inlier_number >= MAX_H_SUBSET) {
h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
random_gen_H->setSubsetSize(h_inls_cnt <= MAX_H_SUBSET ? (int)(0.8*h_inls_cnt) : MAX_H_SUBSET);
}
H_best_score = h_score;
}
}
}
return h_inls_cnt;
}
bool recoverIfDegenerate (const std::vector<int> &sample, const Mat &F_best, const Score &F_best_score,
Mat &non_degenerate_model, Score &non_degenerate_model_score) override {
const auto swapF = [&] (const Mat &_F, const Score &_score) {
const auto non_min_solver = EpipolarNonMinimalSolver::create(points_mat, true);
if (! optimizeF(_F, _score, non_degenerate_model, non_degenerate_model_score)) {
_F.copyTo(non_degenerate_model);
non_degenerate_model_score = _score;
}
};
Mat F_from_H, F_from_E, H_best; Score F_from_H_score, F_from_E_score;
if (! estimateHfrom3Points(F_best, sample, H_best)) {
return false; // non degenerate
}
if (true_K_given) {
if (getFfromTrueK(H_best, F_from_H, F_from_H_score)) {
if (F_from_H_score.isBetter(F_from_E_score))
swapF(F_from_H, F_from_H_score);
else swapF(F_from_E, F_from_E_score);
return true;
} else {
non_degenerate_model_score = Score();
return true; // no translation
}
}
const int non_planar_support_degen_F = getNonPlanarSupport(F_best);
Score F_pl_par_score, F_calib_score; Mat F_pl_par, F_calib;
if (calibDegensac(H_best, F_calib, F_calib_score, non_planar_support_degen_F, F_best_score)) {
if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, max_iters_plane_and_parallax, true,
F_best_score, non_planar_support_degen_F, F_pl_par, F_pl_par_score) && F_pl_par_score.isBetter(F_calib_score)
&& getNonPlanarSupport(F_pl_par) > getNonPlanarSupport(F_calib)) {
swapF(F_pl_par, F_pl_par_score);
return true;
}
swapF(F_calib, F_calib_score);
return true;
} else {
if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, max_iters_plane_and_parallax, true,
F_best_score, non_planar_support_degen_F, F_pl_par, F_pl_par_score)) {
swapF(F_pl_par, F_pl_par_score);
return true;
}
}
if (! isFDegenerate(non_planar_support_degen_F)) {
return false;
}
non_degenerate_model_score = Score();
return true;
}
// RANSAC with plane-and-parallax to find new Fundamental matrix
bool getFfromTrueK (const Matx33d &H, Mat &F_from_K, Score &F_from_K_score) {
std::vector<Matx33d> R; std::vector<Vec3d> t;
if (decomposeHomographyMat(true_K2_inv * H * true_K1, Matx33d::eye(), R, t, noArray()) == 1) {
// std::cout << "Warning: translation is zero!\n";
return false; // is degenerate
}
// sign of translation does not make difference
const Mat F1 = Mat(true_K2_inv_t * Math::getSkewSymmetric(t[0]) * R[0] * true_K1_inv);
const Mat F2 = Mat(true_K2_inv_t * Math::getSkewSymmetric(t[1]) * R[1] * true_K1_inv);
const auto score_f1 = quality->getScore(f_error->getErrors(F1)), score_f2 = quality->getScore(f_error->getErrors(F2));
if (score_f1.isBetter(score_f2)) {
F_from_K = F1; F_from_K_score = score_f1;
} else {
F_from_K = F2; F_from_K_score = score_f2;
}
return true;
}
bool planeAndParallaxRANSAC (const Matx33d &H, std::vector<int> &non_planar_pts, int num_non_planar_pts,
int max_iters_pl_par, bool use_preemptive, const Score &score_degen_F, int non_planar_support_degen_F,
Mat &F_new, Score &F_new_score) {
if (num_non_planar_pts < 2)
return false;
num_models_used_so_far = 0; // reset estimation of lambda for plane-and-parallax
int max_iters = max_iters_pl_par, non_planar_support = 0, iters = 0;
const float * points = points_mat.ptr<float>();
std::vector<Matx33d> F_good;
const double CLOSE_THR = 1.0;
for (; iters < max_iters; iters++) {
// draw two random points
int h_outlier1 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
int h_outlier2 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
while (h_outlier1 == h_outlier2)
h_outlier2 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
const auto x1 = points[h_outlier1], y1 = points[h_outlier1+1], X1 = points[h_outlier1+2], Y1 = points[h_outlier1+3];
const auto x2 = points[h_outlier2], y2 = points[h_outlier2+1], X2 = points[h_outlier2+2], Y2 = points[h_outlier2+3];
if ((fabsf(X1 - X2) < CLOSE_THR && fabsf(Y1 - Y2) < CLOSE_THR) ||
(fabsf(x1 - x2) < CLOSE_THR && fabsf(y1 - y2) < CLOSE_THR))
continue;
// do plane and parallax with outliers of H
// F = [(p1' x Hp1) x (p2' x Hp2)]_x H = [e']_x H
Vec3d ep = (Vec3d(X1, Y1, 1).cross(H * Vec3d(x1, y1, 1))) // l1 = p1' x Hp1
.cross((Vec3d(X2, Y2, 1).cross(H * Vec3d(x2, y2, 1))));// l2 = p2' x Hp2
const Matx33d F = Math::getSkewSymmetric(ep) * H;
const auto * const f = F.val;
// check orientation constraint of epipolar lines
const bool valid = (f[0]*x1+f[1]*y1+f[2])*(ep[1]-ep[2]*Y1) * (f[0]*x2+f[1]*y2+f[2])*(ep[1]-ep[2]*Y2) > 0;
if (!valid) continue;
const int num_f_inliers_of_h_outliers = getNonPlanarSupport(Mat(F), num_models_used_so_far >= MAX_MODELS_TO_TEST, non_planar_support);
if (non_planar_support < num_f_inliers_of_h_outliers) {
non_planar_support = num_f_inliers_of_h_outliers;
const double predicted_iters = log_conf / log(1 - pow(static_cast<double>
(getNonPlanarSupport(Mat(F), non_planar_pts, num_non_planar_pts)) / num_non_planar_pts, 2));
if (use_preemptive && ! std::isinf(predicted_iters) && predicted_iters < max_iters)
max_iters = static_cast<int>(predicted_iters);
F_good = { F };
} else if (non_planar_support == num_f_inliers_of_h_outliers) {
F_good.emplace_back(F);
}
}
F_new_score = Score();
for (const auto &F_ : F_good) {
const auto sc = quality->getScore(f_error->getErrors(Mat(F_)));
if (sc.isBetter(F_new_score)) {
F_new_score = sc;
F_new = Mat(F_);
}
}
if (F_new_score.isBetter(score_degen_F) && non_planar_support > non_planar_support_degen_F)
return true;
if (isFDegenerate(non_planar_support))
return false;
return true;
}
bool calibDegensac (const Matx33d &H, Mat &F_new, Score &F_new_score, int non_planar_support_degen_F, const Score &F_degen_score) {
const float * points = points_mat.ptr<float>();
if (! is_principal_pt_set) {
// estimate principal points from coordinates
float px1 = 0, py1 = 0, px2 = 0, py2 = 0;
for (int i = 0; i < points_size; i++) {
const int idx = 4*i;
if (px1 < points[idx ]) px1 = points[idx ];
if (py1 < points[idx+1]) py1 = points[idx+1];
if (px2 < points[idx+2]) px2 = points[idx+2];
if (py2 < points[idx+3]) py2 = points[idx+3];
}
setPrincipalPoint((int)(px1/2)+1, (int)(py1/2)+1, (int)(px2/2)+1, (int)(py2/2)+1);
}
std::vector<Mat> R; std::vector<Mat> t; std::vector<Mat> F_good;
std::vector<double> best_f;
int non_planar_support_out = 0;
for (double f = 300; f <= 3000; f += 150.) {
K(0,0) = K(1,1) = K2(0,0) = K2(1,1) = f;
const double one_over_f = 1/f;
K_inv(0,0) = K_inv(1,1) = K2_inv(0,0) = K2_inv(1,1) = K2_inv_t(0,0) = K2_inv_t(1,1) = one_over_f;
K_inv(0,2) = -K(0,2)*one_over_f; K_inv(1,2) = -K(1,2)*one_over_f;
K2_inv_t(2,0) = K2_inv(0,2) = -K2(0,2)*one_over_f; K2_inv_t(2,1) = K2_inv(1,2) = -K2(1,2)*one_over_f;
if (decomposeHomographyMat(K2_inv * H * K, Matx33d::eye(), R, t, noArray()) == 1) continue;
Mat F1 = Mat(K2_inv_t * Math::getSkewSymmetric(Vec3d(t[0])) * Matx33d(R[0]) * K_inv);
Mat F2 = Mat(K2_inv_t * Math::getSkewSymmetric(Vec3d(t[2])) * Matx33d(R[2]) * K_inv);
int non_planar_f1 = getNonPlanarSupport(F1, true, non_planar_support_out),
non_planar_f2 = getNonPlanarSupport(F2, true, non_planar_support_out);
if (non_planar_f1 < non_planar_f2) {
non_planar_f1 = non_planar_f2; F1 = F2;
}
if (non_planar_support_out < non_planar_f1) {
non_planar_support_out = non_planar_f1;
F_good = {F1};
best_f = { f };
} else if (non_planar_support_out == non_planar_f1) {
F_good.emplace_back(F1);
best_f.emplace_back(f);
}
}
F_new_score = Score();
for (int i = 0; i < (int) F_good.size(); i++) {
const auto sc = quality->getScore(f_error->getErrors(F_good[i]));
if (sc.isBetter(F_new_score)) {
F_new_score = sc;
F_good[i].copyTo(F_new);
if (sc.isBetter(best_focal_score)) {
best_focal = best_f[i]; // save best focal length
best_focal_score = sc;
}
}
}
if (F_degen_score.isBetter(F_new_score) && non_planar_support_out <= non_planar_support_degen_F)
return false;
/*
// logarithmic search -> faster but less accurate
double f_min = 300, f_max = 3500;
while (f_max - f_min > 100) {
const double f_half = (f_max + f_min) * 0.5f, left_half = (f_min + f_half) * 0.5f, right_half = (f_half + f_max) * 0.5f;
const double inl_in_left = eval_f(left_half), inl_in_right = eval_f(right_half);
if (inl_in_left > inl_in_right)
f_max = f_half;
else f_min = f_half;
}
*/
return true;
}
void getOutliersH (const Mat &H_best) {
// get H outliers
num_h_outliers_eval = num_h_outliers = 0;
const auto &h_errors = h_reproj_error->getErrors(H_best);
for (int pt = 0; pt < points_size; pt++)
if (h_errors[pt] > H_SAMPLE_THR_SQR) {
h_outliers[num_h_outliers++] = pt;
if (h_errors[pt] > MAX_H_THR)
h_outliers_eval[num_h_outliers_eval++] = pt;
}
}
bool verifyFundamental (const Mat &F_best, const Score &F_score, const std::vector<bool> &inliers_mask, Mat &F_new, Score &new_score) override {
const int f_sample_size = 3, max_H_iters = 5; // 3.52 = log(0.01) / log(1 - std::pow(0.9, 3));
int num_f_inliers = 0;
std::vector<int> inliers(points_size), f_sample(f_sample_size);
for (int i = 0; i < points_size; i++) if (inliers_mask[i]) inliers[num_f_inliers++] = i;
const auto sampler = UniformSampler::create(0, f_sample_size, num_f_inliers);
// find e', null space of F^T
const Vec3d e_prime = Utils::getLeftEpipole(F_best);
const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
Score H_best_score; Mat H_best;
for (int iter = 0; iter < max_H_iters; iter++) {
sampler->generateSample(f_sample);
Matx33d H;
if (!getH(A, e_prime, 4*inliers[f_sample[0]], 4*inliers[f_sample[1]], 4*inliers[f_sample[2]], H))
continue;
const auto h_score = h_repr_quality->getScore(Mat(H));
if (h_score.isBetter(H_best_score)) {
H_best_score = h_score; H_best = Mat(H);
}
}
if (H_best.empty()) return false; // non-degenerate
optimizeH(H_best, H_best_score);
getOutliersH(H_best);
const int non_planar_support_best_F = getNonPlanarSupport(F_best);
const bool is_F_degen = isFDegenerate(non_planar_support_best_F);
Mat F_from_K; Score F_from_K_score;
bool success = false;
// generate non-degenerate F even though so-far-the-best one may not be degenerate
if (true_K_given) {
// use GT calibration
if (getFfromTrueK(H_best, F_from_K, F_from_K_score)) {
new_score = F_from_K_score;
F_from_K.copyTo(F_new);
success = true;
}
} else {
// use calibrated DEGENSAC
if (calibDegensac(H_best, F_from_K, F_from_K_score, non_planar_support_best_F, F_score)) {
new_score = F_from_K_score;
F_from_K.copyTo(F_new);
success = true;
}
}
if (!is_F_degen) {
return false;
} else if (success) // F is degenerate
return true; // but successfully recovered
// recover degenerate F using plane-and-parallax
Score plane_parallax_score; Mat F_plane_parallax;
if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, 20, true,
F_score, non_planar_support_best_F, F_plane_parallax, plane_parallax_score)) {
new_score = plane_parallax_score;
F_plane_parallax.copyTo(F_new);
return true;
}
// plane-and-parallax failed. A previous non-degenerate so-far-the-best model will be used instead
new_score = Score();
return true;
}
void setPrincipalPoint (double px_, double py_) override {
setPrincipalPoint(px_, py_, 0, 0);
}
void setPrincipalPoint (double px_, double py_, double px2_, double py2_) override {
if (px_ > DBL_EPSILON && py_ > DBL_EPSILON) {
is_principal_pt_set = true;
K = {1, 0, px_, 0, 1, py_, 0, 0, 1};
if (px2_ > DBL_EPSILON && py2_ > DBL_EPSILON) K2 = {1, 0, px2_, 0, 1, py2_, 0, 0, 1};
else K2 = K;
K_inv = K2_inv = K2_inv_t = Matx33d::eye();
}
}
private:
bool getH (const Matx33d &A, const Vec3d &e_prime, int smpl1, int smpl2, int smpl3, Matx33d &H) {
const float * points = points_mat.ptr<float>();
Vec3d p1(points[smpl1 ], points[smpl1+1], 1), p2(points[smpl2 ], points[smpl2+1], 1), p3(points[smpl3 ], points[smpl3+1], 1);
Vec3d P1(points[smpl1+2], points[smpl1+3], 1), P2(points[smpl2+2], points[smpl2+3], 1), P3(points[smpl3+2], points[smpl3+3], 1);
const Matx33d M (p1[0], p1[1], 1, p2[0], p2[1], 1, p3[0], p3[1], 1);
if (p1.cross(p2).dot(p3) * P1.cross(P2).dot(P3) < 0) return false;
// (x′i × e')
const Vec3d P1e = P1.cross(e_prime), P2e = P2.cross(e_prime), P3e = P3.cross(e_prime);
// x′i × (A xi))^T (x′i × e′) / ‖x′i×e′‖^2,
const Vec3d b (P1.cross(A * p1).dot(P1e) / (P1e[0]*P1e[0]+P1e[1]*P1e[1]+P1e[2]*P1e[2]),
P2.cross(A * p2).dot(P2e) / (P2e[0]*P2e[0]+P2e[1]*P2e[1]+P2e[2]*P2e[2]),
P3.cross(A * p3).dot(P3e) / (P3e[0]*P3e[0]+P3e[1]*P3e[1]+P3e[2]*P3e[2]));
H = A - e_prime * (M.inv() * b).t();
return true;
}
int getNonPlanarSupport (const Mat &F, const std::vector<int> &pts, int num_pts) {
int non_rand_support = 0;
f_error->setModelParameters(F);
for (int pt = 0; pt < num_pts; pt++)
if (f_error->getError(pts[pt]) < f_threshold_sqr)
non_rand_support++;
return non_rand_support;
}
int getNonPlanarSupport (const Mat &F, bool preemptive=false, int max_so_far=0) {
int non_rand_support = 0;
f_error->setModelParameters(F);
if (preemptive) {
const auto preemptive_thr = -num_h_outliers_eval + max_so_far;
for (int pt = 0; pt < num_h_outliers_eval; pt++)
if (f_error->getError(h_outliers_eval[pt]) < f_threshold_sqr)
non_rand_support++;
else if (non_rand_support - pt < preemptive_thr)
break;
} else {
for (int pt = 0; pt < num_h_outliers_eval; pt++)
if (f_error->getError(h_outliers_eval[pt]) < f_threshold_sqr)
non_rand_support++;
if (num_models_used_so_far < MAX_MODELS_TO_TEST && !true_K_given/*for K we know that recovered F cannot be degenerate*/) {
non_planar_supports[num_models_used_so_far++] = non_rand_support;
if (num_models_used_so_far == MAX_MODELS_TO_TEST) {
getLambda(non_planar_supports, 2.32, num_h_outliers_eval, 0, false, estimated_min_non_planar_support);
if (estimated_min_non_planar_support < 3) estimated_min_non_planar_support = 3;
}
}
}
return non_rand_support;
}
inline bool isModelValid(const Mat &F, const std::vector<int> &sample) const override {
return ep_deg.isModelValid(F, sample);
}
bool isFDegenerate (int num_f_inliers_h_outliers) const {
if (num_models_used_so_far < MAX_MODELS_TO_TEST)
// the minimum number of non-planar support has not estimated yet -> use tentative
return num_f_inliers_h_outliers < std::min(TENT_MIN_NON_PLANAR_SUPP, (int)(0.1 * num_h_outliers_eval));
return num_f_inliers_h_outliers < estimated_min_non_planar_support;
}
};
Ptr<FundamentalDegeneracy> FundamentalDegeneracy::create (int state, const Ptr<Quality> &quality_,
const Mat &points_, int sample_size_, int max_iters_plane_and_parallax, double homography_threshold_,
double f_inlier_thr_sqr, const Mat true_K1, const Mat true_K2) {
return makePtr<FundamentalDegeneracyImpl>(state, quality_, points_, sample_size_,
max_iters_plane_and_parallax, homography_threshold_, f_inlier_thr_sqr, true_K1, true_K2);
}
class EssentialDegeneracyImpl : public EssentialDegeneracy {
private:
const EpipolarGeometryDegeneracyImpl ep_deg;
public:
explicit EssentialDegeneracyImpl (const Mat &points, int sample_size_) :
ep_deg (points, sample_size_) {}
inline bool isModelValid(const Mat &E, const std::vector<int> &sample) const override {
return ep_deg.isModelValid(E, sample);
}
};
Ptr<EssentialDegeneracy> EssentialDegeneracy::create (const Mat &points_, int sample_size_) {
return makePtr<EssentialDegeneracyImpl>(points_, sample_size_);
}
}}
|