int main(int argc, char* argv[])
{
pointCorrespondences();
//pointCloudCorrespondence();
//checkCalibration();
return 0;
}
double RansacMultiFeatureSetMatcher::matchSets(const std::vector<InterestPoint *> &reference, const std::vector<InterestPoint *> &data, OrientedPoint2D& transformation,
std::vector< std::pair<InterestPoint *, InterestPoint *> > &correspondences) const
{
correspondences.clear();
unsigned int iterations = m_adaptive ? 1e17 : ceil(log(1. - m_successProbability)/log(1. - m_inlierProbability * m_inlierProbability));
// Compute possible correspondences based on thresholding
std::vector< std::pair<InterestPoint *, InterestPoint *> > possibleCorrespondences;
for(unsigned int i = 0; i < data.size(); i++){
for(unsigned int j = 0; j < reference.size(); j++){
double distance = data[i]->getDescriptor()->distance(reference[j]->getDescriptor());
if(distance < m_distanceThreshold){
possibleCorrespondences.push_back(std::make_pair(data[i], reference[j]));
}
}
}
// Check if there are enough absolute matches
if(possibleCorrespondences.size() < 2){
return 1e17;
}
// Check if there are enough matches compared to the inlier probability (FIXME maybe is better to adjust the inlier probability)
if(double(possibleCorrespondences.size()) * m_inlierProbability < 2){
return 1e17;
}
boost::mt19937 rng;
boost::uniform_smallint<int> generator(0, possibleCorrespondences.size() - 1);
// Main loop
double minimumScore = 1e17;
for(unsigned int i = 0; i < iterations; i++){
unsigned int first = generator(rng);
unsigned int second = generator(rng);
while(second == first) second = generator(rng); // avoid useless samples
std::pair< std::pair<InterestPoint *, InterestPoint *>, std::pair<InterestPoint *, InterestPoint *> > minimumSampleSet(possibleCorrespondences[first], possibleCorrespondences[second]);
// Test rigidity
const Point2D& diffFirst = possibleCorrespondences[first].first->getPosition() - possibleCorrespondences[second].first->getPosition();
const Point2D& diffSecond = possibleCorrespondences[first].second->getPosition() - possibleCorrespondences[second].second->getPosition();
double distanceFirst = diffFirst * diffFirst;
double distanceSecond = diffSecond * diffSecond;
if((distanceFirst - distanceSecond)*(distanceFirst - distanceSecond)/(8*(distanceFirst + distanceSecond )) > m_rigidityThreshold){
continue;
}
// Compute hypothesis
std::vector< std::pair<InterestPoint *, InterestPoint *> > inlierSet;
OrientedPoint2D hypothesis = generateHypothesis(minimumSampleSet);
// Verify hypothesis
double score = verifyHypothesis(reference, data, hypothesis, inlierSet);
if(score < minimumScore){
minimumScore = score;
transformation = hypothesis;
correspondences = inlierSet;
// Adapt the number of iterations
if (m_adaptive){
double inlierProbability = double(correspondences.size())/double(data.size());
iterations = ceil(log(1. - m_successProbability)/log(1. - inlierProbability * inlierProbability));
}
}
}
std::vector<std::pair<Point2D, Point2D> > pointCorrespondences(correspondences.size());
for(unsigned int i = 0; i < correspondences.size(); i++){
pointCorrespondences[i] = std::make_pair(correspondences[i].first->getPosition(), correspondences[i].second->getPosition());
}
compute2DPose(pointCorrespondences, transformation);
return verifyHypothesis(reference, data, transformation, correspondences);
}
