00001 #include "AbstractFeatureSetMatcher.h"
00002
00003 AbstractFeatureSetMatcher::AbstractFeatureSetMatcher(double acceptanceThreshold):
00004 m_acceptanceThreshold(acceptanceThreshold)
00005 {
00006
00007 }
00008
00009 OrientedPoint2D AbstractFeatureSetMatcher::generateHypothesis(const std::pair< std::pair<InterestPoint *, InterestPoint *>, std::pair<InterestPoint *, InterestPoint *> > &correspondences) const
00010 {
00011 Point2D baseDelta = correspondences.first.first->getPosition() - correspondences.second.first->getPosition();
00012 Point2D transformedDelta = correspondences.first.second->getPosition() - correspondences.second.second->getPosition();
00013 const Point2D& baseSecond = correspondences.second.first->getPosition();
00014 const Point2D& transformedSecond = correspondences.second.second->getPosition();
00015 double denominator = 1. / (baseDelta * baseDelta);
00016 double cosalpha = denominator * (baseDelta * transformedDelta);
00017 double sinalpha = - denominator * (baseDelta.ortho() * transformedDelta);
00018 double x = transformedSecond.x - cosalpha * baseSecond.x + sinalpha * baseSecond.y;
00019 double y = transformedSecond.y - cosalpha * baseSecond.y - sinalpha * baseSecond.x;
00020 return OrientedPoint2D(x, y, atan2(sinalpha, cosalpha));
00021 }
00022
00023 double AbstractFeatureSetMatcher::verifyHypothesis(const std::vector<InterestPoint *> &reference, const std::vector<InterestPoint *> &data, const OrientedPoint2D& transformation,
00024 std::vector< std::pair<InterestPoint *, InterestPoint *> > &inlierSet) const
00025 {
00026
00027 double maxCorrespondences = data.size();
00028 double score = 0.;
00029 inlierSet.clear();
00030 inlierSet.reserve(maxCorrespondences);
00031 for(unsigned int i = 0; i < data.size(); i++){
00032 double minDistance = 1e17;
00033 const Point2D point1 = transformation.oplus(data[i]->getPosition());
00034 for(unsigned int j = 0; j < reference.size(); j++){
00035 const Point2D& point2 = reference[j]->getPosition();
00036 double currentDistance = (point1 - point2) * (point1 - point2);
00037 if(currentDistance < m_acceptanceThreshold){
00038 inlierSet.push_back(std::make_pair(data[i], reference[j]));
00039 minDistance = minDistance < currentDistance ? minDistance : currentDistance;
00040 } else {
00041 minDistance = minDistance < m_acceptanceThreshold ? minDistance : m_acceptanceThreshold;
00042 }
00043 }
00044 score += minDistance;
00045 }
00046 return score;
00047 }
