/// 2D homography estimation from point correspondences.
void HomographyModel::Fit(const std::vector<size_t> &indices,
std::vector<Model> *H,std::vector<float> & weight,int method) const {
if(4 > indices.size())
return;
const size_t n = indices.size();
libNumerics::matrix<double> A = libNumerics::matrix<double>::zeros(n*2,9);
for (size_t i = 0; i < n; ++i) {
size_t index = indices[i];
size_t j = 2*i;
A(j,0) = x1_(0, index);
A(j,1) = x1_(1, index);
A(j,2) = 1.0;
A(j,6) = -x2_(0, index) * x1_(0, index);
A(j,7) = -x2_(0, index) * x1_(1, index);
A(j,8) = -x2_(0, index);
++j;
A(j,3) = x1_(0, index);
A(j,4) = x1_(1, index);
A(j,5) = 1.0;
A(j,6) = -x2_(1, index) * x1_(0, index);
A(j,7) = -x2_(1, index) * x1_(1, index);
A(j,8) = -x2_(1, index);
}
libNumerics::vector<double> vecNullspace(9);
if( SVDWrapper::Nullspace(A,&vecNullspace) )
{
libNumerics::matrix<double> M(3,3);
M.read(vecNullspace);
if(M.det() < 0)
M = -M;
M /= M(2,2);
if(SVDWrapper::InvCond(M)>=ICOND_MIN &&
IsOrientationPreserving(indices,M) )
H->push_back(M);
}
}
void FundamentalModel::Fit(const std::vector<size_t> &indices,
std::vector<Mat> *Fs) const {
assert(2 == x1_.nrow());
assert(7 <= x1_.ncol());
assert(x1_.nrow() == x2_.nrow());
assert(x1_.ncol() == x2_.ncol());
if (indices.size() < 7){
return;
}
// Set up the homogeneous system Af = 0 from the equations x'T*F*x = 0.
Mat A(indices.size(), 9);
EncodeEpipolarEquation(x1_, x2_, indices, &A);
if(indices.size() >= 8) { // 8-point algorithm
orsa::vector<double> vecNullspace(9);
SVDWrapper::Nullspace(A, &vecNullspace);
orsa::matrix<double> F(3,3);
F.read(vecNullspace);
// Force the fundamental property if the A matrix has full rank.
orsa::matrix<double> FRank2(3,3);
//SVDWrapper::EnforceRank2_3x3(F, &FRank2);
//Fs->push_back(FRank2);
Fs->push_back(F);
}
else
{
// Find the two F matrices in the nullspace of A.
Mat F1(3,3), F2(3,3);
SVDWrapper::Nullspace2_Remap33(A,F1,F2);
// Then, use the condition det(F) = 0 to determine F. In other words, solve
// det(F1 + a*F2) = 0 for a.
double a = F1(0, 0), j = F2(0, 0),
b = F1(0, 1), k = F2(0, 1),
c = F1(0, 2), l = F2(0, 2),
d = F1(1, 0), m = F2(1, 0),
e = F1(1, 1), n = F2(1, 1),
f = F1(1, 2), o = F2(1, 2),
g = F1(2, 0), p = F2(2, 0),
h = F1(2, 1), q = F2(2, 1),
i = F1(2, 2), r = F2(2, 2);
// The coefficients are in ascending powers of alpha, i.e. P[N]*x^N.
double P[4] = {
a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g,
a*e*r + a*i*n + b*f*p + b*g*o + c*d*q + c*h*m + d*h*l + e*i*j + f*g*k -
a*f*q - a*h*o - b*d*r - b*i*m - c*e*p - c*g*n - d*i*k - e*g*l - f*h*j,
a*n*r + b*o*p + c*m*q + d*l*q + e*j*r + f*k*p + g*k*o + h*l*m + i*j*n -
a*o*q - b*m*r - c*n*p - d*k*r - e*l*p - f*j*q - g*l*n - h*j*o - i*k*m,
j*n*r + k*o*p + l*m*q - j*o*q - k*m*r - l*n*p,
};
// Solve for the roots of P[3]*x^3 + P[2]*x^2 + P[1]*x + P[0] = 0.
double roots[3];
int num_roots = SolveCubicPolynomial(P, roots);
// Build the fundamental matrix for each solution.
for (int s = 0; s < num_roots; ++s)
Fs->push_back(F1 + roots[s] * F2);
}
}
