From 60e1eda1495ec7bc64b41220e92bcaf23ece05db Mon Sep 17 00:00:00 2001 From: Alexey Spizhevoy Date: Wed, 18 May 2011 08:56:48 +0000 Subject: [PATCH] modified focal estimation function in opencv_stitching --- modules/stitching/autocalib.cpp | 130 ++++++++++++-------------------- modules/stitching/autocalib.hpp | 5 +- modules/stitching/motion_estimators.cpp | 38 +--------- 3 files changed, 55 insertions(+), 118 deletions(-) diff --git a/modules/stitching/autocalib.cpp b/modules/stitching/autocalib.cpp index 3c1eca0..ad1ac0a 100644 --- a/modules/stitching/autocalib.cpp +++ b/modules/stitching/autocalib.cpp @@ -8,99 +8,63 @@ void focalsFromHomography(const Mat& H, double &f0, double &f1, bool &f0_ok, boo { CV_Assert(H.type() == CV_64F && H.size() == Size(3, 3)); - const double h[9] = - { - H.at(0, 0), H.at(0, 1), H.at(0, 2), - H.at(1, 0), H.at(1, 1), H.at(1, 2), - H.at(2, 0), H.at(2, 1), H.at(2, 2) - }; + const double* h = reinterpret_cast(H.data); + + double d1, d2; // Denominators + double v1, v2; // Focal squares value candidates f1_ok = true; - double denom1 = h[6] * h[7]; - double denom2 = (h[7] - h[6]) * (h[7] + h[6]); - if (max(abs(denom1), abs(denom2)) < 1e-5) - f1_ok = false; - else - { - double val1 = -(h[0] * h[1] + h[3] * h[4]) / denom1; - double val2 = (h[0] * h[0] + h[3] * h[3] - h[1] * h[1] - h[4] * h[4]) / denom2; - if (val1 < val2) - swap(val1, val2); - if (val1 > 0 && val2 > 0) - f1 = sqrt(abs(denom1) > abs(denom2) ? val1 : val2); - else if (val1 > 0) - f1 = sqrt(val1); - else - f1_ok = false; - } + d1 = h[6] * h[7]; + d2 = (h[7] - h[6]) * (h[7] + h[6]); + v1 = -(h[0] * h[1] + h[3] * h[4]) / d1; + v2 = (h[0] * h[0] + h[3] * h[3] - h[1] * h[1] - h[4] * h[4]) / d2; + if (v1 < v2) swap(v1, v2); + if (v1 > 0 && v2 > 0) f1 = sqrt(abs(d1) > abs(d2) ? v1 : v2); + else if (v1 > 0) f1 = sqrt(v1); + else f1_ok = false; f0_ok = true; - denom1 = h[0] * h[3] + h[1] * h[4]; - denom2 = h[0] * h[0] + h[1] * h[1] - h[3] * h[3] - h[4] * h[4]; - if (max(abs(denom1), abs(denom2)) < 1e-5) - f0_ok = false; - else - { - double val1 = -h[2] * h[5] / denom1; - double val2 = (h[5] * h[5] - h[2] * h[2]) / denom2; - if (val1 < val2) - swap(val1, val2); - if (val1 > 0 && val2 > 0) - f0 = sqrt(abs(denom1) > abs(denom2) ? val1 : val2); - else if (val1 > 0) - f0 = sqrt(val1); - else - f0_ok = false; - } + d1 = h[0] * h[3] + h[1] * h[4]; + d2 = h[0] * h[0] + h[1] * h[1] - h[3] * h[3] - h[4] * h[4]; + v1 = -h[2] * h[5] / d1; + v2 = (h[5] * h[5] - h[2] * h[2]) / d2; + if (v1 < v2) swap(v1, v2); + if (v1 > 0 && v2 > 0) f0 = sqrt(abs(d1) > abs(d2) ? v1 : v2); + else if (v1 > 0) f0 = sqrt(v1); + else f0_ok = false; } -bool focalsFromFundamental(const Mat &F, double &f0, double &f1) +double estimateFocal(const vector &images, const vector &/*features*/, + const vector &pairwise_matches) { - CV_Assert(F.type() == CV_64F); - CV_Assert(F.size() == Size(3, 3)); - - Mat Ft = F.t(); - Mat k = Mat::zeros(3, 1, CV_64F); - k.at(2, 0) = 1.f; + const int num_images = static_cast(images.size()); - // 1. Compute quantities - double a = normL2sq(F*Ft*k) / normL2sq(Ft*k); - double b = normL2sq(Ft*F*k) / normL2sq(F*k); - double c = sqr(k.dot(F*k)) / (normL2sq(Ft*k) * normL2sq(F*k)); - double d = k.dot(F*Ft*F*k) / k.dot(F*k); - double A = 1/c + a - 2*d; - double B = 1/c + b - 2*d; - double P = 2*(1/c - 2*d + 0.5*normL2sq(F)); - double Q = -(A + B)/c + 0.5*(normL2sq(F*Ft) - 0.5*sqr(normL2sq(F))); - - // 2. Solve quadratic equation Z*Z*a_ + Z*b_ + c_ = 0 - double a_ = 1 + c*P; - double b_ = -(c*P*P + 2*P + 4*c*Q); - double c_ = P*P + 4*c*P*Q + 12*A*B; - double D = b_*b_ - 4*a_*c_; - if (abs(D) < 1e-5) - D = 0; - else if (D < 0) - return false; - double D_sqrt = sqrt(D); - double Z0 = (-b_ - D_sqrt) / (2*a_); - double Z1 = (-b_ + D_sqrt) / (2*a_); - - // 3. Choose solution - double w0 = abs(Z0*Z0*Z0 - 3*P*Z0*Z0 + 2*(P*P + 2*Q)*Z0 - 4*(P*Q + 4*A*B/c)); - double w1 = abs(Z1*Z1*Z1 - 3*P*Z1*Z1 + 2*(P*P + 2*Q)*Z1 - 4*(P*Q + 4*A*B/c)); - double Z = Z0; - if (w1 < w0) - Z = Z1; + vector focals; + for (int src_idx = 0; src_idx < num_images; ++src_idx) + { + for (int dst_idx = 0; dst_idx < num_images; ++dst_idx) + { + const MatchesInfo &m = pairwise_matches[src_idx*num_images + dst_idx]; + if (m.H.empty()) + continue; - // 4. - double X = -1/c*(1 + 2*B/(Z - P)); - double Y = -1/c*(1 + 2*A/(Z - P)); + double f0, f1; + bool f0ok, f1ok; + focalsFromHomography(m.H, f0, f1, f0ok, f1ok); + if (f0ok && f1ok) focals.push_back(sqrt(f0*f1)); + } + } - // 5. Compute focal lengths - f0 = 1/sqrt(1 + X/normL2sq(Ft*k)); - f1 = 1/sqrt(1 + Y/normL2sq(F*k)); + if (focals.size() + 1 >= images.size()) + { + nth_element(focals.begin(), focals.end(), focals.begin() + focals.size()/2); + return focals[focals.size()/2]; + } - return true; + LOGLN("Can't estimate focal length, will use naive approach"); + double focals_sum = 0; + for (int i = 0; i < num_images; ++i) + focals_sum += images[i].rows + images[i].cols; + return focals_sum / num_images; } diff --git a/modules/stitching/autocalib.hpp b/modules/stitching/autocalib.hpp index b269a38..81652b4 100644 --- a/modules/stitching/autocalib.hpp +++ b/modules/stitching/autocalib.hpp @@ -1,12 +1,15 @@ #ifndef __OPENCV_AUTOCALIB_HPP__ #define __OPENCV_AUTOCALIB_HPP__ +#include #include +#include "matchers.hpp" // See "Construction of Panoramic Image Mosaics with Global and Local Alignment" // by Heung-Yeung Shum and Richard Szeliski. void focalsFromHomography(const cv::Mat &H, double &f0, double &f1, bool &f0_ok, bool &f1_ok); -bool focalsFromFundamental(const cv::Mat &F, double &f0, double &f1); +double estimateFocal(const std::vector &images, const std::vector &features, + const std::vector &pairwise_matches); #endif // __OPENCV_AUTOCALIB_HPP__ diff --git a/modules/stitching/motion_estimators.cpp b/modules/stitching/motion_estimators.cpp index bdfdf15..90638fd 100644 --- a/modules/stitching/motion_estimators.cpp +++ b/modules/stitching/motion_estimators.cpp @@ -64,46 +64,16 @@ struct CalcRotation }; -void HomographyBasedEstimator::estimate(const vector &images, const vector &/*features*/, +void HomographyBasedEstimator::estimate(const vector &images, const vector &features, const vector &pairwise_matches, vector &cameras) { const int num_images = static_cast(images.size()); - // Find focals from pair-wise homographies - vector is_focal_estimated(num_images, false); - vector focals; - for (int i = 0; i < num_images; ++i) - { - for (int j = 0; j < num_images; ++j) - { - int pair_idx = i * num_images + j; - if (pairwise_matches[pair_idx].H.empty()) - continue; - - double f_to, f_from; - bool f_to_ok, f_from_ok; - focalsFromHomography(pairwise_matches[pair_idx].H.inv(), f_to, f_from, f_to_ok, f_from_ok); - - if (f_from_ok) focals.push_back(f_from); - if (f_to_ok) focals.push_back(f_to); - - if (f_from_ok && f_to_ok) - { - is_focal_estimated[i] = true; - is_focal_estimated[j] = true; - } - } - } - - is_focals_estimated_ = true; - for (int i = 0; i < num_images; ++i) - is_focals_estimated_ = is_focals_estimated_ && is_focal_estimated[i]; - - // Find focal median and use it as true focal length - nth_element(focals.begin(), focals.end(), focals.begin() + focals.size() / 2); + // Estimate focal length and set it for all cameras + double focal = estimateFocal(images, features, pairwise_matches); cameras.resize(num_images); for (int i = 0; i < num_images; ++i) - cameras[i].focal = focals[focals.size() / 2]; + cameras[i].focal = focal; // Restore global motion Graph span_tree; -- 2.7.4