|
|
|
@ -38,80 +38,80 @@ |
|
|
|
|
// or tort (including negligence or otherwise) arising in any way out of
|
|
|
|
|
// the use of this software, even if advised of the possibility of such damage.
|
|
|
|
|
//
|
|
|
|
|
//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*/
|
|
|
|
|
#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) |
|
|
|
|
{ |
|
|
|
|
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]; |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
LOGLN("Can't estimate focal length, will use naive 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; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|