/// If H is not orientation preserving at the point, the error is infinite.
/// For this test, it is assumed that det(H)>0.
/// \param H The homography matrix.
/// \param index The correspondence index
/// \param side In which image is the error measured?
/// \return The square reprojection error.
double HomographyModel::Error(const Mat &H, size_t index, int* side) const {
double err = std::numeric_limits<double>::infinity();
if(side) *side=1;
libNumerics::vector<double> x(3);
// Transfer error in image 2
x(0) = x1_(0,index);
x(1) = x1_(1,index);
x(2) = 1.0;
x = H * x;
if(x(2)>0) {
x /= x(2);
err = sqr(x2_(0,index)-x(0)) + sqr(x2_(1,index)-x(1));
}
// Transfer error in image 1
if(symError_) { // ... but only if requested
x(0) = x2_(0,index);
x(1) = x2_(1,index);
x(2) = 1.0;
x = H.inv() * x;
if(x(2)>0) {
x /= x(2);
double err1 = sqr(x1_(0,index)-x(0)) + sqr(x1_(1,index)-x(1));
if(err1>err) { // Keep worse error
err=err1;
if(side) *side=0;
}
}
}
return err;
}
/// \param F The fundamental matrix.
/// \param index The point correspondence.
/// \param side In which image is the error measured?
/// \return The square reprojection error.
double FundamentalModel::Error(const Mat &F, size_t index, int* side) const {
double xa = x1_(0,index), ya = x1_(1,index);
double xb = x2_(0,index), yb = x2_(1,index);
double a, b, c, d;
// Transfer error in image 2
if(side) *side=1;
a = F(0,0) * xa + F(0,1) * ya + F(0,2);
b = F(1,0) * xa + F(1,1) * ya + F(1,2);
c = F(2,0) * xa + F(2,1) * ya + F(2,2);
d = a*xb + b*yb + c;
double err = (d*d) / (a*a + b*b);
// Transfer error in image 1
if(symError_) { // ... but only if requested
a = F(0,0) * xb + F(1,0) * yb + F(2,0);
b = F(0,1) * xb + F(1,1) * yb + F(2,1);
c = F(0,2) * xb + F(1,2) * yb + F(2,2);
d = a*xa + b*ya + c;
double err1 = (d*d) / (a*a + b*b);
if(err1>err) {
err = err1;
if(side) *side=0;
}
}
return err;
}
/// 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);
}
}
