Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
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;
00053 size_t hardIterLimit = 100;
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
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
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);
00094 p = std::min( 1-eps, p);
00095 softIterLimit = log(1-p) / log(p);
00096 }
00097
00098 iter++;
00099 }
00100
00101 return true;
00102 }
00103
00104
00105
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
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
00142 population.clear();
00143 int i = 0;
00144
00145
00146 for(; i < elderCnt; i++ )
00147 {
00148 population.push_back( sortedPopulation[i] );
00149 }
00150
00151
00152 int se = (int)speciesAlive;
00153 for( ; i < elderCnt + siblingCnt ; i++ )
00154 {
00155 Species* sibling = sortedPopulation[--se];
00156 population.push_back( sibling );
00157
00158
00159
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
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
00174 sampleSet.insert( rand() % sampleCount );
00175 pickRandomIndex( sampleSet, p_kernelSize, sibling->sample );
00176 }
00177
00178
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
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
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
00218 return false;
00219 }
00220
00221
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 }
00234 }
00235
00236 #endif // math_modelsearch_h