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 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019 */
00020 
00021 #include "RansacFiltering.h"
00022 #include "Homography.h"
00023 
00024 #include <boost/foreach.hpp>
00025 
00026 using namespace std;
00027 
00028 static int genint(int x)
00029 {
00030     return (int)((double)rand()*x/(double)RAND_MAX);
00031 }
00032 
00033 namespace lfeat
00034 {
00035 
00036 // distance between estimate and real point in pixels.
00037 
00038 double Ransac::calcError(Homography* aH, PointMatch& aM)
00039 {
00040     double x1p, y1p;
00041     aH->transformPoint(aM._img1_x, aM._img1_y, x1p, y1p);
00042 
00043     double d1 = aM._img2_x - x1p;
00044     double d2 = aM._img2_y - y1p;
00045 
00046     return d1*d1+d2*d2;
00047 }
00048 
00049 
00050 void Ransac::filter(PointMatchVector_t& ioMatches, PointMatchVector_t& ioRemovedMatches)
00051 {
00052     int aRemainingIterations = _nIter;
00053     const double aErrorDistSq = _distanceThres * _distanceThres;
00054 
00055     Homography aCurrentModel;
00056 
00057     unsigned int aMaxInliers = 0;
00058     PointMatchVector_t aBestInliers;
00059     PointMatchVector_t aBestOutliers;
00060 
00061     // normalization  !!!!!!
00062     aCurrentModel.initMatchesNormalization(ioMatches);
00063 
00064 
00065     //std::cout << "gravity " << _v1x << " " << _v1y << " " << _v2x << " " << _v2y << endl;
00066 
00067     for(; aRemainingIterations > 0; aRemainingIterations--)
00068     {
00069         //cout << "RANSAC -- iter " << aRemainingIterations << endl;
00070 
00071         // random select 4 matches to fit the model
00072         // from the input set as maybe_inliers
00073         PointMatchVector_t aMatches(ioMatches);
00074         PointMatchVector_t aInliers, aOutliers;
00075 
00076         //std::cout << aMatches.size() << " matches size" << endl;
00077 
00078         for(int i=0; i<5; i++)
00079         {
00080             int n = genint((int)aMatches.size()-1);
00081             aInliers.push_back(aMatches.at(n));
00082             aMatches.erase(aMatches.begin()+n);
00083         }
00084 
00085         //std::cout << aMatches.size() << " estimate" << endl;
00086 
00087         if (!aCurrentModel.estimate(aInliers))
00088         {
00089             aMatches.clear();
00090             continue;
00091         }
00092 
00093         // for every point remaining in aMatches, add them to aInliers if they fit the model well.
00094         BOOST_FOREACH(PointMatchPtr aMatchesIter2, aMatches)
00095         {
00096             if (calcError(&aCurrentModel, *aMatchesIter2) < aErrorDistSq)
00097             {
00098                 aInliers.push_back(aMatchesIter2);
00099             }
00100             else
00101             {
00102                 aOutliers.push_back(aMatchesIter2);
00103             }
00104         }
00105 
00106         if (aInliers.size() > aMaxInliers)
00107         {
00108             //cout << "good found -----------------" << aRemainingIterations << endl;
00109             //cout << aCurrentModel << endl;
00110             for (int i=0; i<3; ++i)
00111                 for(int j=0; j<3; ++j)
00112                 {
00113                     _bestModel._H[i][j] = aCurrentModel._H[i][j];
00114                 }
00115 
00116             _bestModel._v1x = aCurrentModel._v1x;
00117             _bestModel._v2x = aCurrentModel._v2x;
00118             _bestModel._v1y = aCurrentModel._v1y;
00119             _bestModel._v2y = aCurrentModel._v2y;
00120 
00121 
00122             //*ioBestModel = *aCurrentModel;
00123             aMaxInliers = (unsigned int)aInliers.size();
00124             aBestInliers = aInliers;
00125             aBestOutliers = aOutliers;
00126             //cout << "Inliers : " << aInliers.size() << " Outliers : " << aOutliers.size() << endl;
00127 
00128         }
00129 
00130         // if there are 0 outliers then we are done.
00131         if (aOutliers.empty())
00132         {
00133             break;
00134         }
00135 
00136         //cout << "Inliers : " << aInliers.size() << " Outliers : " << aOutliers.size() << endl;
00137         //cout << "end iter" << endl;
00138     }
00139 
00140     ioMatches = aBestInliers;
00141     ioRemovedMatches = aBestOutliers;
00142 
00143 
00144 }
00145 
00146 void Ransac::transform(double iX, double iY, double& oX, double& oY)
00147 {
00148     _bestModel.transformPoint(iX, iY, oX, oY);
00149 }
00150 
00151 }

Generated on Wed Apr 23 01:25:41 2014 for Hugintrunk by  doxygen 1.3.9.1