RansacFiltering.cpp

Go to the documentation of this file.
00001 /*
00002 * Copyright (C) 2007-2008 Anael Orlinski
00003 *
00004 * This file is part of Panomatic.
00005 *
00006 * Panomatic is free software; you can redistribute it and/or modify
00007 * it under the terms of the GNU General Public License as published by
00008 * the Free Software Foundation; either version 2 of the License, or
00009 * (at your option) any later version.
00010 *
00011 * Panomatic is distributed in the hope that it will be useful,
00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 * GNU General Public License for more details.
00015 *
00016 * You should have received a copy of the GNU General Public License
00017 * along with Panomatic; if not, write to the Free Software
00018 * <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include "RansacFiltering.h"
00022 #include "Homography.h"
00023 
00024 using namespace std;
00025 
00026 static int genint(int x)
00027 {
00028     return (int)((double)rand()*x/(double)RAND_MAX);
00029 }
00030 
00031 namespace lfeat
00032 {
00033 
00034 // distance between estimate and real point in pixels.
00035 
00036 double Ransac::calcError(Homography* aH, PointMatch& aM)
00037 {
00038     double x1p, y1p;
00039     aH->transformPoint(aM._img1_x, aM._img1_y, x1p, y1p);
00040 
00041     double d1 = aM._img2_x - x1p;
00042     double d2 = aM._img2_y - y1p;
00043 
00044     return d1*d1+d2*d2;
00045 }
00046 
00047 
00048 void Ransac::filter(PointMatchVector_t& ioMatches, PointMatchVector_t& ioRemovedMatches)
00049 {
00050     int aRemainingIterations = _nIter;
00051     const double aErrorDistSq = _distanceThres * _distanceThres;
00052 
00053     Homography aCurrentModel;
00054 
00055     unsigned int aMaxInliers = 0;
00056     PointMatchVector_t aBestInliers;
00057     PointMatchVector_t aBestOutliers;
00058 
00059     // normalization  !!!!!!
00060     aCurrentModel.initMatchesNormalization(ioMatches);
00061 
00062 
00063     //std::cout << "gravity " << _v1x << " " << _v1y << " " << _v2x << " " << _v2y << endl;
00064 
00065     for(; aRemainingIterations > 0; aRemainingIterations--)
00066     {
00067         //cout << "RANSAC -- iter " << aRemainingIterations << endl;
00068 
00069         // random select 4 matches to fit the model
00070         // from the input set as maybe_inliers
00071         PointMatchVector_t aMatches(ioMatches);
00072         PointMatchVector_t aInliers, aOutliers;
00073 
00074         //std::cout << aMatches.size() << " matches size" << endl;
00075 
00076         for(int i=0; i<5; i++)
00077         {
00078             int n = genint((int)aMatches.size()-1);
00079             aInliers.push_back(aMatches.at(n));
00080             aMatches.erase(aMatches.begin()+n);
00081         }
00082 
00083         //std::cout << aMatches.size() << " estimate" << endl;
00084 
00085         if (!aCurrentModel.estimate(aInliers))
00086         {
00087             aMatches.clear();
00088             continue;
00089         }
00090 
00091         // for every point remaining in aMatches, add them to aInliers if they fit the model well.
00092         for (size_t i = 0; i < aMatches.size(); ++i)
00093         {
00094             PointMatchPtr aMatchesIter2 = aMatches[i];
00095             if (calcError(&aCurrentModel, *aMatchesIter2) < aErrorDistSq)
00096             {
00097                 aInliers.push_back(aMatchesIter2);
00098             }
00099             else
00100             {
00101                 aOutliers.push_back(aMatchesIter2);
00102             }
00103         }
00104 
00105         if (aInliers.size() > aMaxInliers)
00106         {
00107             //cout << "good found -----------------" << aRemainingIterations << endl;
00108             //cout << aCurrentModel << endl;
00109             for (int i=0; i<3; ++i)
00110                 for(int j=0; j<3; ++j)
00111                 {
00112                     _bestModel._H[i][j] = aCurrentModel._H[i][j];
00113                 }
00114 
00115             _bestModel._v1x = aCurrentModel._v1x;
00116             _bestModel._v2x = aCurrentModel._v2x;
00117             _bestModel._v1y = aCurrentModel._v1y;
00118             _bestModel._v2y = aCurrentModel._v2y;
00119 
00120 
00121             //*ioBestModel = *aCurrentModel;
00122             aMaxInliers = (unsigned int)aInliers.size();
00123             aBestInliers = aInliers;
00124             aBestOutliers = aOutliers;
00125             //cout << "Inliers : " << aInliers.size() << " Outliers : " << aOutliers.size() << endl;
00126 
00127         }
00128 
00129         // if there are 0 outliers then we are done.
00130         if (aOutliers.empty())
00131         {
00132             break;
00133         }
00134 
00135         //cout << "Inliers : " << aInliers.size() << " Outliers : " << aOutliers.size() << endl;
00136         //cout << "end iter" << endl;
00137     }
00138 
00139     ioMatches = aBestInliers;
00140     ioRemovedMatches = aBestOutliers;
00141 
00142 
00143 }
00144 
00145 void Ransac::transform(double iX, double iY, double& oX, double& oY)
00146 {
00147     _bestModel.transformPoint(iX, iY, oX, oY);
00148 }
00149 
00150 }

Generated on 2 Sep 2015 for Hugintrunk by  doxygen 1.4.7