1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 #include "precomp.hpp" 44 45 using namespace cv; 46 47 namespace { 48 49 template<typename _Tp> static inline bool 50 decomposeCholesky(_Tp* A, size_t astep, int m) 51 { 52 if (!hal::Cholesky(A, astep, m, 0, 0, 0)) 53 return false; 54 astep /= sizeof(A[0]); 55 for (int i = 0; i < m; ++i) 56 A[i*astep + i] = (_Tp)(1./A[i*astep + i]); 57 return true; 58 } 59 60 } // namespace 61 62 63 namespace cv { 64 namespace detail { 65 66 void focalsFromHomography(const Mat& H, double &f0, double &f1, bool &f0_ok, bool &f1_ok) 67 { 68 CV_Assert(H.type() == CV_64F && H.size() == Size(3, 3)); 69 70 const double* h = H.ptr<double>(); 71 72 double d1, d2; // Denominators 73 double v1, v2; // Focal squares value candidates 74 75 f1_ok = true; 76 d1 = h[6] * h[7]; 77 d2 = (h[7] - h[6]) * (h[7] + h[6]); 78 v1 = -(h[0] * h[1] + h[3] * h[4]) / d1; 79 v2 = (h[0] * h[0] + h[3] * h[3] - h[1] * h[1] - h[4] * h[4]) / d2; 80 if (v1 < v2) std::swap(v1, v2); 81 if (v1 > 0 && v2 > 0) f1 = std::sqrt(std::abs(d1) > std::abs(d2) ? v1 : v2); 82 else if (v1 > 0) f1 = std::sqrt(v1); 83 else f1_ok = false; 84 85 f0_ok = true; 86 d1 = h[0] * h[3] + h[1] * h[4]; 87 d2 = h[0] * h[0] + h[1] * h[1] - h[3] * h[3] - h[4] * h[4]; 88 v1 = -h[2] * h[5] / d1; 89 v2 = (h[5] * h[5] - h[2] * h[2]) / d2; 90 if (v1 < v2) std::swap(v1, v2); 91 if (v1 > 0 && v2 > 0) f0 = std::sqrt(std::abs(d1) > std::abs(d2) ? v1 : v2); 92 else if (v1 > 0) f0 = std::sqrt(v1); 93 else f0_ok = false; 94 } 95 96 97 void estimateFocal(const std::vector<ImageFeatures> &features, const std::vector<MatchesInfo> &pairwise_matches, 98 std::vector<double> &focals) 99 { 100 const int num_images = static_cast<int>(features.size()); 101 focals.resize(num_images); 102 103 std::vector<double> all_focals; 104 105 for (int i = 0; i < num_images; ++i) 106 { 107 for (int j = 0; j < num_images; ++j) 108 { 109 const MatchesInfo &m = pairwise_matches[i*num_images + j]; 110 if (m.H.empty()) 111 continue; 112 double f0, f1; 113 bool f0ok, f1ok; 114 focalsFromHomography(m.H, f0, f1, f0ok, f1ok); 115 if (f0ok && f1ok) 116 all_focals.push_back(std::sqrt(f0 * f1)); 117 } 118 } 119 120 if (static_cast<int>(all_focals.size()) >= num_images - 1) 121 { 122 double median; 123 124 std::sort(all_focals.begin(), all_focals.end()); 125 if (all_focals.size() % 2 == 1) 126 median = all_focals[all_focals.size() / 2]; 127 else 128 median = (all_focals[all_focals.size() / 2 - 1] + all_focals[all_focals.size() / 2]) * 0.5; 129 130 for (int i = 0; i < num_images; ++i) 131 focals[i] = median; 132 } 133 else 134 { 135 LOGLN("Can't estimate focal length, will use naive approach"); 136 double focals_sum = 0; 137 for (int i = 0; i < num_images; ++i) 138 focals_sum += features[i].img_size.width + features[i].img_size.height; 139 for (int i = 0; i < num_images; ++i) 140 focals[i] = focals_sum / num_images; 141 } 142 } 143 144 145 bool calibrateRotatingCamera(const std::vector<Mat> &Hs, Mat &K) 146 { 147 int m = static_cast<int>(Hs.size()); 148 CV_Assert(m >= 1); 149 150 std::vector<Mat> Hs_(m); 151 for (int i = 0; i < m; ++i) 152 { 153 CV_Assert(Hs[i].size() == Size(3, 3) && Hs[i].type() == CV_64F); 154 Hs_[i] = Hs[i] / std::pow(determinant(Hs[i]), 1./3.); 155 } 156 157 const int idx_map[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}}; 158 Mat_<double> A(6*m, 6); 159 A.setTo(0); 160 161 int eq_idx = 0; 162 for (int k = 0; k < m; ++k) 163 { 164 Mat_<double> H(Hs_[k]); 165 for (int i = 0; i < 3; ++i) 166 { 167 for (int j = i; j < 3; ++j, ++eq_idx) 168 { 169 for (int l = 0; l < 3; ++l) 170 { 171 for (int s = 0; s < 3; ++s) 172 { 173 int idx = idx_map[l][s]; 174 A(eq_idx, idx) += H(i,l) * H(j,s); 175 } 176 } 177 A(eq_idx, idx_map[i][j]) -= 1; 178 } 179 } 180 } 181 182 Mat_<double> wcoef; 183 SVD::solveZ(A, wcoef); 184 185 Mat_<double> W(3,3); 186 for (int i = 0; i < 3; ++i) 187 for (int j = i; j < 3; ++j) 188 W(i,j) = W(j,i) = wcoef(idx_map[i][j], 0) / wcoef(5,0); 189 if (!decomposeCholesky(W.ptr<double>(), W.step, 3)) 190 return false; 191 W(0,1) = W(0,2) = W(1,2) = 0; 192 K = W.t(); 193 return true; 194 } 195 196 } // namespace detail 197 } // namespace cv 198
