/// 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, int 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;
}
/// Is \a H orientation preserving near match points of index in \a indices?
/// It is assumed that det(H)>0, otherwise the sign tests should be reversed.
bool HomographyModel::IsOrientationPreserving(const std::vector<size_t>&indices,
const Mat& H) const {
libNumerics::matrix<double> h=H.row(2);
std::vector<size_t>::const_iterator it=indices.begin(), end=indices.end();
for(; it != end; ++it)
if(h(0)*x2_(0,*it)+h(1)*x2_(1,*it)+h(2) <= 0)
return false;
return true;
}
/// 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);
}
}
