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
decomposeCholesky(_Tp * A,size_t astep,int m)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
focalsFromHomography(const Mat & H,double & f0,double & f1,bool & f0_ok,bool & f1_ok)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
estimateFocal(const std::vector<ImageFeatures> & features,const std::vector<MatchesInfo> & pairwise_matches,std::vector<double> & focals)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
calibrateRotatingCamera(const std::vector<Mat> & Hs,Mat & K)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