/// 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); } }