Main MRPT website > C++ reference
MRPT logo

model_search_impl.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 
00029 #ifndef math_modelsearch_impl_h
00030 #define math_modelsearch_impl_h
00031 
00032 #ifndef math_modelsearch_h
00033 #       include "model_search.h"
00034 #endif
00035 
00036 #include <limits>
00037 
00038 namespace mrpt {
00039         namespace math {
00040 
00041 //----------------------------------------------------------------------
00042 //! Run the ransac algorithm searching for a single model
00043 template<typename TModelFit>
00044 bool ModelSearch::ransacSingleModel( const TModelFit& p_state,
00045                                                                          size_t p_kernelSize,
00046                                                                          const typename TModelFit::Real& p_fitnessThreshold,
00047                                                                          typename TModelFit::Model& p_bestModel,
00048                                                                          vector_size_t& p_inliers )
00049 {
00050         size_t bestScore = 0;
00051         size_t iter = 0;
00052         size_t softIterLimit = 1; // will be updated by the size of inliers
00053         size_t hardIterLimit = 100; // a fixed iteration step
00054         size_t nSamples = p_state.getSampleCount();
00055         vector_size_t ind( p_kernelSize );
00056 
00057         while ( iter < softIterLimit && iter < hardIterLimit )
00058         {
00059                 bool degenerate = true;
00060                 typename TModelFit::Model currentModel;
00061                 size_t i = 0;
00062                 while ( degenerate )
00063                 {
00064                         pickRandomIndex( nSamples, p_kernelSize, ind );
00065                         degenerate = !p_state.fitModel( ind, currentModel );
00066                         i++;
00067                         if( i > 100 )
00068                                 return false;
00069                 }
00070 
00071                 vector_size_t inliers;
00072 
00073                 for( size_t i = 0; i < nSamples; i++ )
00074                 {
00075                         if( p_state.testSample( i, currentModel ) < p_fitnessThreshold )
00076                                 inliers.push_back( i );
00077                 }
00078                 ASSERT_( inliers.size() > 0 );
00079 
00080                 // Find the number of inliers to this model.
00081                 const size_t ninliers = inliers.size();
00082 
00083                 if ( ninliers > bestScore )
00084                 {
00085                         bestScore = ninliers;
00086                         p_bestModel = currentModel;
00087                         p_inliers = inliers;
00088 
00089                         // Update the estimation of maxIter to pick dataset with no outliers at propability p
00090                         float f =  ninliers / static_cast<float>( nSamples );
00091                         float p = 1 -  pow( f, static_cast<float>( p_kernelSize ) );
00092                         float eps = std::numeric_limits<float>::epsilon();
00093                         p = std::max( eps, p);  // Avoid division by -Inf
00094                         p = std::min( 1-eps, p);        // Avoid division by 0.
00095                         softIterLimit = log(1-p) / log(p);
00096                 }
00097 
00098                 iter++;
00099         }
00100 
00101         return true;
00102 }
00103 
00104 //----------------------------------------------------------------------
00105 //! Run a generic programming version of ransac searching for a single model
00106 template<typename TModelFit>
00107 bool ModelSearch::geneticSingleModel( const TModelFit& p_state,
00108                                                                           size_t p_kernelSize,
00109                                                                           const typename TModelFit::Real& p_fitnessThreshold,
00110                                                                           size_t p_populationSize,
00111                                                                           size_t p_maxIteration,
00112                                                                           typename TModelFit::Model& p_bestModel,
00113                                                                           vector_size_t& p_inliers )
00114 {
00115         // a single specie of the population
00116         typedef TSpecies<TModelFit> Species;
00117         std::vector<Species> storage;
00118         std::vector<Species*> population;
00119         std::vector<Species*> sortedPopulation;
00120 
00121         size_t sampleCount = p_state.getSampleCount();
00122         int elderCnt = (int)p_populationSize/3;
00123         int siblingCnt = (p_populationSize - elderCnt) / 2;
00124         int speciesAlive = 0;
00125 
00126         storage.resize( p_populationSize );
00127         population.reserve( p_populationSize );
00128         sortedPopulation.reserve( p_populationSize );
00129         for( typename std::vector<Species>::iterator it = storage.begin(); it != storage.end(); it++ )
00130         {
00131                 pickRandomIndex( sampleCount, p_kernelSize, it->sample );
00132                 population.push_back( &*it );
00133                 sortedPopulation.push_back( &*it );
00134         }
00135 
00136         size_t iter = 0;
00137         while ( iter < p_maxIteration )
00138         {
00139                 if( iter > 0 )
00140                 {
00141                         //generate new population: old, siblings,  new
00142                         population.clear();
00143                         int i = 0;
00144 
00145                         //copy the best elders
00146                         for(; i < elderCnt; i++ )
00147                         {
00148                                 population.push_back( sortedPopulation[i] );
00149                         }
00150 
00151                         // mate elders to make siblings
00152                         int se = (int)speciesAlive; //dead species cannot mate
00153                         for( ; i < elderCnt + siblingCnt ; i++ )
00154                         {
00155                                 Species* sibling = sortedPopulation[--se];
00156                                 population.push_back( sibling );
00157 
00158                                 //pick two parents, from the species not yet refactored
00159                                 //better elders has more chance to mate as they are removed later from the list
00160                                 int r1 = rand();
00161                                 int r2 = rand();
00162                                 int p1 = r1 % se;
00163                                 int p2 = (p1 > se / 2) ? (r2 % p1) : p1 + 1 + (r2 % (se-p1-1));
00164                                 ASSERT_( p1 != p2 && p1 < se && p2 < se );
00165                                 ASSERT_( se >= elderCnt );
00166                                 Species* a = sortedPopulation[p1];
00167                                 Species* b = sortedPopulation[p2];
00168 
00169                                 // merge the sample candidates
00170                                 std::set<size_t> sampleSet;
00171                                 sampleSet.insert( a->sample.begin(), a->sample.end() );
00172                                 sampleSet.insert( b->sample.begin(), b->sample.end() );
00173                                 //mutate - add a random sample that will be selected with some (non-zero) probability
00174                                 sampleSet.insert( rand() % sampleCount );
00175                                 pickRandomIndex( sampleSet, p_kernelSize, sibling->sample );
00176                         }
00177 
00178                         // generate some new random species
00179                         for( ; i < (int)p_populationSize; i++ )
00180                         {
00181                                 Species* s = sortedPopulation[i];
00182                                 population.push_back( s );
00183                                 pickRandomIndex( sampleCount, p_kernelSize, s->sample );
00184                         }
00185                 }
00186 
00187                 //evaluate species
00188                 speciesAlive = 0;
00189                 for( typename std::vector<Species*>::iterator it = population.begin(); it != population.end(); it++ )
00190                 {
00191                         Species& s = **it;
00192                         if( p_state.fitModel( s.sample, s.model ) )
00193                         {
00194                                 s.fitness = 0;
00195                                 for( size_t i = 0; i < p_state.getSampleCount(); i++ )
00196                                 {
00197                                         typename TModelFit::Real f = p_state.testSample( i, s.model );
00198                                         if( f < p_fitnessThreshold )
00199                                         {
00200                                                 s.fitness += f;
00201                                                 s.inliers.push_back( i );
00202                                         }
00203                                 }
00204                                 ASSERT_( s.inliers.size() > 0 );
00205 
00206                                 s.fitness /= s.inliers.size();
00207                                 // scale by the number of outliers
00208                                 s.fitness *= (sampleCount - s.inliers.size());
00209                                 speciesAlive++;
00210                         }
00211                         else
00212                                 s.fitness = std::numeric_limits<typename TModelFit::Real>::max();
00213                 }
00214 
00215                 if( !speciesAlive )
00216                 {
00217                         // the world is dead, no model was found
00218                         return false;
00219                 }
00220 
00221                 //sort by fitness (ascending)
00222                 std::sort( sortedPopulation.begin(), sortedPopulation.end(), Species::compare );
00223 
00224                 iter++;
00225         }
00226 
00227         p_bestModel = sortedPopulation[0]->model;
00228         p_inliers = sortedPopulation[0]->inliers;
00229 
00230         return !p_inliers.empty();
00231 }
00232 
00233 } // namespace math
00234 } // namespace mrpt
00235 
00236 #endif // math_modelsearch_h



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011