// or tort (including negligence or otherwise) arising in any way out of\r
// the use of this software, even if advised of the possibility of such damage.\r
//\r
-//M*/
-#include "autocalib.hpp"
-#include "util.hpp"
-
-using namespace std;
-using namespace cv;
-
-void focalsFromHomography(const Mat& H, double &f0, double &f1, bool &f0_ok, bool &f1_ok)
-{
- CV_Assert(H.type() == CV_64F && H.size() == Size(3, 3));
-
- const double* h = reinterpret_cast<const double*>(H.data);
-
- double d1, d2; // Denominators
- double v1, v2; // Focal squares value candidates
-
- f1_ok = true;
- 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;
- 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;
-}
-
-
-void estimateFocal(const vector<ImageFeatures> &features, const vector<MatchesInfo> &pairwise_matches,
- vector<double> &focals)
-{
- const int num_images = static_cast<int>(features.size());
- focals.resize(num_images);
-
- vector<double> all_focals;
-
- for (int i = 0; i < num_images; ++i)
- {
- for (int j = 0; j < num_images; ++j)
- {
- const MatchesInfo &m = pairwise_matches[i*num_images + j];
- if (m.H.empty())
- continue;
- double f0, f1;
- bool f0ok, f1ok;
- focalsFromHomography(m.H, f0, f1, f0ok, f1ok);
- if (f0ok && f1ok)
- all_focals.push_back(sqrt(f0 * f1));
- }
- }
-
- if (static_cast<int>(all_focals.size()) < num_images - 1)
- {
- LOGLN("Can't estimate focal length, will use anaive approach");
- double focals_sum = 0;
- for (int i = 0; i < num_images; ++i)
- focals_sum += features[i].img_size.width + features[i].img_size.height;
- for (int i = 0; i < num_images; ++i)
- focals[i] = focals_sum / num_images;
- }
- else
- {
- nth_element(all_focals.begin(), all_focals.begin() + all_focals.size()/2, all_focals.end());
- for (int i = 0; i < num_images; ++i)
- focals[i] = all_focals[all_focals.size()/2];
- }
-}
+//M*/\r
+#include "autocalib.hpp"\r
+#include "util.hpp"\r
+\r
+using namespace std;\r
+using namespace cv;\r
+\r
+void focalsFromHomography(const Mat& H, double &f0, double &f1, bool &f0_ok, bool &f1_ok)\r
+{\r
+ CV_Assert(H.type() == CV_64F && H.size() == Size(3, 3));\r
+\r
+ const double* h = reinterpret_cast<const double*>(H.data);\r
+\r
+ double d1, d2; // Denominators\r
+ double v1, v2; // Focal squares value candidates\r
+\r
+ f1_ok = true;\r
+ d1 = h[6] * h[7];\r
+ d2 = (h[7] - h[6]) * (h[7] + h[6]);\r
+ v1 = -(h[0] * h[1] + h[3] * h[4]) / d1;\r
+ v2 = (h[0] * h[0] + h[3] * h[3] - h[1] * h[1] - h[4] * h[4]) / d2;\r
+ if (v1 < v2) swap(v1, v2);\r
+ if (v1 > 0 && v2 > 0) f1 = sqrt(abs(d1) > abs(d2) ? v1 : v2);\r
+ else if (v1 > 0) f1 = sqrt(v1);\r
+ else f1_ok = false;\r
+\r
+ f0_ok = true;\r
+ d1 = h[0] * h[3] + h[1] * h[4];\r
+ d2 = h[0] * h[0] + h[1] * h[1] - h[3] * h[3] - h[4] * h[4];\r
+ v1 = -h[2] * h[5] / d1;\r
+ v2 = (h[5] * h[5] - h[2] * h[2]) / d2;\r
+ if (v1 < v2) swap(v1, v2);\r
+ if (v1 > 0 && v2 > 0) f0 = sqrt(abs(d1) > abs(d2) ? v1 : v2);\r
+ else if (v1 > 0) f0 = sqrt(v1);\r
+ else f0_ok = false;\r
+}\r
+\r
+\r
+void estimateFocal(const vector<ImageFeatures> &features, const vector<MatchesInfo> &pairwise_matches, \r
+ vector<double> &focals)\r
+{\r
+ const int num_images = static_cast<int>(features.size());\r
+ focals.resize(num_images);\r
+\r
+ vector<double> all_focals;\r
+\r
+ for (int i = 0; i < num_images; ++i)\r
+ { \r
+ for (int j = 0; j < num_images; ++j)\r
+ {\r
+ const MatchesInfo &m = pairwise_matches[i*num_images + j];\r
+ if (m.H.empty())\r
+ continue;\r
+ double f0, f1;\r
+ bool f0ok, f1ok;\r
+ focalsFromHomography(m.H, f0, f1, f0ok, f1ok);\r
+ if (f0ok && f1ok) \r
+ all_focals.push_back(sqrt(f0 * f1));\r
+ }\r
+ }\r
+\r
+ if (static_cast<int>(all_focals.size()) >= num_images - 1)\r
+ {\r
+ nth_element(all_focals.begin(), all_focals.begin() + all_focals.size()/2, all_focals.end());\r
+ for (int i = 0; i < num_images; ++i)\r
+ focals[i] = all_focals[all_focals.size()/2];\r
+ }\r
+ else\r
+ {\r
+ LOGLN("Can't estimate focal length, will use naive approach");\r
+ double focals_sum = 0;\r
+ for (int i = 0; i < num_images; ++i)\r
+ focals_sum += features[i].img_size.width + features[i].img_size.height;\r
+ for (int i = 0; i < num_images; ++i)\r
+ focals[i] = focals_sum / num_images;\r
+ }\r
+}\r