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

Generated on 30 Jul 2016 for Hugintrunk by  doxygen 1.4.7