27 #include "../common/PlatformUtils.h"
41 #include <boost/math/special_functions/binomial.hpp>
42 #include <boost/random.hpp>
44 #define RANSACGETMAXINVALIDITERATIONS( assurance, dynamicMaxIterations ) \
46 (RansacIntegerCounterT)1 \
49 (RansacIntegerCounterT)( \
50 ((RansacFloatCounterT)dynamicMaxIterations) \
52 ((RansacFloatCounterT)assurance) \
57 #define RANSACGETMAXITERATIONS( goodTPNumber, totalTPNumber, modelRequiredTPNumber, procsNumber, assurance ) \
60 (RansacIntegerCounterT) \
63 (RansacFloatCounterT)( 1.0 - ((RansacFloatCounterT)assurance) ) \
67 ((RansacFloatCounterT)1.0) \
71 ((RansacFloatCounterT)goodTPNumber ) \
73 ((RansacFloatCounterT)totalTPNumber) \
76 ((RansacFloatCounterT)modelRequiredTPNumber) \
82 ((RansacIntegerCounterT)procsNumber) \
85 #define RANSACSYNCTHREAD \
88 if( bestTransfPtr->initialize( bestParams ) ) \
91 ( paramsPtr->m_bestTransformationPtrPtr->get() == 0 ) \
94 ( bestTiePoins.size() > \
95 (*paramsPtr->m_bestTransformationPtrPtr)->getParameters().m_tiePoints.size() ) \
98 ( bestTiePoins.size() == \
99 (*paramsPtr->m_bestTransformationPtrPtr)->getParameters().m_tiePoints.size() ) \
102 ( bestParamsConvexHullArea > (*paramsPtr->m_bestParamsConvexHullAreaPtr) ) \
105 ( bestParamsConvexHullArea == (*paramsPtr->m_bestParamsConvexHullAreaPtr) ) \
108 ( bestParamsMaxDMapErrorPtr < (*paramsPtr->m_bestParamsMaxDMapErrorPtr) ) \
110 ( bestParamsMaxIMapErrorPtr < (*paramsPtr->m_bestParamsMaxIMapErrorPtr) ) \
118 (*paramsPtr->m_bestTransformationPtrPtr).reset( \
119 bestTransfPtr->clone() ); \
120 (*paramsPtr->m_bestParamsMaxDMapErrorPtr) = bestParamsMaxDMapErrorPtr; \
121 (*paramsPtr->m_bestParamsMaxIMapErrorPtr) = bestParamsMaxIMapErrorPtr; \
122 (*paramsPtr->m_bestParamsConvexHullAreaPtr) = bestParamsConvexHullArea; \
123 (*paramsPtr->m_bestTiePoinsPtr) = bestTiePoins; \
126 if( dynamicMaxIterations < globalDynamicMaxIterations ) \
127 globalDynamicMaxIterations = dynamicMaxIterations; \
201 const double maxDirectMapError,
202 const double maxInverseMapError,
204 const double& assurance,
205 const bool enableMultiThread,
206 const std::vector<double>& tiePointsWeights,
207 std::vector< te::gm::GTParameters::TiePoint >& outTiePoints,
208 std::auto_ptr< GeometricTransformation >& outTransf
211 if(maxDirectMapError < 0)
214 if(maxInverseMapError < 0)
217 if((assurance <= 0.0) || (assurance > 1.0))
220 outTiePoints.clear();
225 std::map< double, GTParameters::TiePoint > tpsMap;
227 const unsigned int inputTPNmb = (
unsigned int)
229 double tiePointsWeightsSum = 0;
231 if( tiePointsWeights.size() > 0 )
233 if( tiePointsWeights.size() != inputParams.
m_tiePoints.size () )
238 unsigned int tpIdx = 0;
240 for( tpIdx = 0 ; tpIdx < inputTPNmb ; ++tpIdx )
242 if( tiePointsWeights[ tpIdx ] < 0.0 )
return false;
244 tiePointsWeightsSum += tiePointsWeights[ tpIdx ];
247 if( tiePointsWeightsSum <= 0.0 )
return false;
251 double newWSum = 0.0;
254 for( tpIdx = 0 ; tpIdx < inputTPNmb ; ++tpIdx )
256 newW = tiePointsWeights[ tpIdx ];
257 newW /= tiePointsWeightsSum;
261 tpsMap[ newWSum ] = inputParams.
m_tiePoints[ tpIdx ];
267 const double increment = 1.0 /
270 for(
unsigned int tpIdx = 0 ; tpIdx < inputTPNmb ; ++tpIdx )
280 std::auto_ptr< GeometricTransformation > bestTransformationPtr(
282 if( bestTransformationPtr.get() == 0 )
return false;
285 boost::mutex syncMutex;
286 double bestParamsMaxDMapError = maxDirectMapError;
287 double bestParamsMaxIMapError = maxInverseMapError;
288 double bestParamsConvexHullArea = -1.0;
289 bool returnValue =
true;
290 bool keepRunningFlag =
true;
297 enableMultiThread ? procsNumber : 1
302 bestTransformationPtr->getMinRequiredTiePoints(),
304 bestTransformationPtr->getMinRequiredTiePoints(),
305 enableMultiThread ? procsNumber : 1,
310 baseThreadParams.m_inputGTParamsPtr = &inputParams;
311 baseThreadParams.m_maxDirectMapError = maxDirectMapError;
312 baseThreadParams.m_maxInverseMapError = maxInverseMapError;
313 baseThreadParams.m_assurance = assurance;
314 baseThreadParams.m_useDynamicIterationsNumber = ( maxIterations == 0 );
315 baseThreadParams.m_dynamicMaxIterationsPtr = &dynamicMaxIterations;
316 baseThreadParams.m_procsNumber = enableMultiThread ?
318 baseThreadParams.m_returnValuePtr = &returnValue;
319 baseThreadParams.m_mutexPtr = &syncMutex;
320 baseThreadParams.m_keepRunningFlagPtr = &keepRunningFlag;
321 baseThreadParams.m_tpsMapPtr = &tpsMap;
322 baseThreadParams.m_bestTransformationPtrPtr = &bestTransformationPtr;
323 baseThreadParams.m_bestParamsMaxDMapErrorPtr = &bestParamsMaxDMapError;
324 baseThreadParams.m_bestParamsMaxIMapErrorPtr = &bestParamsMaxIMapError;
325 baseThreadParams.m_bestParamsConvexHullAreaPtr = &bestParamsConvexHullArea;
326 baseThreadParams.m_bestTiePoinsPtr = &outTiePoints;
330 if( enableMultiThread )
332 std::vector< ApplyRansacThreadEntryThreadParams > threadsParameters;
333 threadsParameters.resize( procsNumber, baseThreadParams );
334 boost::thread_group threads;
336 for(
unsigned int threadIdx = 0 ; threadIdx < procsNumber ;
340 &threadsParameters[ threadIdx ] ) );
352 if( bestTransformationPtr.get() )
354 outTransf.reset( bestTransformationPtr.release() );
376 std::auto_ptr< te::gm::GeometricTransformation > bestTransfPtr(
378 if( bestTransfPtr.get() == 0 )
386 const std::vector< te::gm::GTParameters::TiePoint > tiePoints =
388 std::map< double, GTParameters::TiePoint > tpsMap = *(paramsPtr->
m_tpsMapPtr);
392 boost::random::mt19937 generator( (boost::random::mt19937::result_type)time(0) );
405 boost::mutex& mutex = (*(paramsPtr->
m_mutexPtr));
409 const unsigned int reqTPsNmb = bestTransfPtr->getMinRequiredTiePoints();
410 const unsigned int inputTPNmb = (
unsigned int)tiePoints.size();
412 if( inputTPNmb < reqTPsNmb )
426 double bestParamsConvexHullArea = -1.0;
427 std::vector< te::gm::GTParameters::TiePoint > bestTiePoins;
431 boost::random::uniform_01< double > distribution;
434 std::vector< te::gm::GTParameters::TiePoint > consensusSetTiePoints;
435 consensusSetParams.
m_tiePoints.reserve( tiePoints.size() );
436 double consensusSetMaxDMapError = 0;
437 double consensusSetMaxIMapError = 0;
438 unsigned int consensusSetSize = 0;
439 double consensusSetConvexHullArea = 0.0;
440 std::vector< Coord2D > consensusSetPt1ConvexHullPoits;
442 double tiePointDMapErr = 0;
443 double tiePointIMapErr = 0;
445 std::map< double, GTParameters::TiePoint >::const_iterator tpsMapIt;
446 unsigned int inputTpIdx = 0;
449 std::vector< GTParameters::TiePoint* > selectedTpsPtrsVec;
450 selectedTpsPtrsVec.resize( reqTPsNmb, 0 );
451 unsigned int selectedTpsPtrsVecIdx = 0;
452 bool selectedTpsNotSelectedBefore =
false;
462 double randomValue = 0;
464 while( keepRunningFlag && ( currentIteration < dynamicMaxIterations ) &&
465 ( consecutiveInvalidIterations < dynamicMaxConsecutiveInvalidIterations ) )
471 consensusSetParams.
reset();;
472 selectedTpsCounter = 0;
474 while( selectedTpsCounter < reqTPsNmb )
476 randomValue = distribution( generator );
477 tpsMapIt = tpsMap.upper_bound( randomValue );
478 assert( tpsMapIt != tpsMap.end() );
482 selectedTpsNotSelectedBefore =
true;
484 for( selectedTpsPtrsVecIdx = 0 ; selectedTpsPtrsVecIdx <
485 selectedTpsCounter ; ++selectedTpsPtrsVecIdx )
487 if( selectedTpsPtrsVec[ selectedTpsPtrsVecIdx ] ==
488 &(tpsMapIt->second) )
490 selectedTpsNotSelectedBefore =
false;
495 if( selectedTpsNotSelectedBefore )
497 consensusSetParams.
m_tiePoints.push_back( tpsMapIt->second );
500 ++selectedTpsCounter;
506 if( bestTransfPtr->computeParameters( consensusSetParams ) )
511 consensusSetTiePoints.clear();
512 consensusSetMaxDMapError = 0;
513 consensusSetMaxIMapError = 0;
515 for( inputTpIdx = 0 ; inputTpIdx < inputTPNmb ; ++inputTpIdx )
519 tiePointDMapErr = bestTransfPtr->getDirectMappingError( curTP, consensusSetParams );
520 tiePointIMapErr = bestTransfPtr->getInverseMappingError( curTP, consensusSetParams );
522 if( ( tiePointDMapErr <= maxDirectMapError ) &&
523 ( tiePointIMapErr <= maxInverseMapError ) )
525 consensusSetTiePoints.push_back( curTP );
527 if( tiePointDMapErr > consensusSetMaxDMapError )
528 consensusSetMaxDMapError = tiePointDMapErr;
529 if( tiePointIMapErr > consensusSetMaxIMapError )
530 consensusSetMaxIMapError = tiePointIMapErr;
534 consensusSetSize = (
unsigned int)consensusSetTiePoints.size();
536 consensusSetTiePoints );
541 ( consensusSetSize >= reqTPsNmb )
544 ( consensusSetSize > bestTiePoins.size() )
547 ( consensusSetSize == bestTiePoins.size() )
550 ( consensusSetConvexHullArea > bestParamsConvexHullArea )
553 ( consensusSetConvexHullArea == bestParamsConvexHullArea )
556 ( bestParamsMaxDMapErrorPtr > consensusSetMaxDMapError )
558 ( bestParamsMaxIMapErrorPtr > consensusSetMaxIMapError )
566 bestParams = consensusSetParams;
567 bestParamsMaxDMapErrorPtr = consensusSetMaxDMapError;
568 bestParamsMaxIMapErrorPtr = consensusSetMaxIMapError;
569 bestParamsConvexHullArea = consensusSetConvexHullArea;
570 bestTiePoins = consensusSetTiePoints;
572 consecutiveInvalidIterations = 0;
576 ++consecutiveInvalidIterations;
581 ++consecutiveInvalidIterations;
586 if( useDynamicIterationsNumber && ( currentIteration != 0 ) )
588 if( bestTiePoins.size() == inputTPNmb )
592 keepRunningFlag =
false;
598 else if( ! bestTiePoins.empty() )
607 if( dynamicMaxIterationsEstimation < dynamicMaxIterations )
609 dynamicMaxIterations =
610 ( dynamicMaxIterations - ( ( dynamicMaxIterations - dynamicMaxIterationsEstimation ) / 2 ) );
612 dynamicMaxConsecutiveInvalidIterations =
616 if( globalDynamicMaxIterations < dynamicMaxIterations )
618 dynamicMaxIterations = globalDynamicMaxIterations;
620 dynamicMaxConsecutiveInvalidIterations =
629 ( threadSyncIteration >= threadSyncMaxIterations )
631 ( currentIteration >= dynamicMaxIterations )
633 ( consecutiveInvalidIterations >= dynamicMaxConsecutiveInvalidIterations )
636 threadSyncIteration = 0;
641 ++threadSyncIteration;
652 if( tiePoints.size() < 3 )
660 for(
unsigned int tiePointsIdx = 0 ; tiePointsIdx < tiePoints.size() ;
664 tiePoints[ tiePointsIdx ].first.y ) );
667 std::auto_ptr< te::gm::Geometry > convexHullPtr( points.
convexHull() );
669 if( dynamic_cast< te::gm::Surface* >( convexHullPtr.get() ) )
671 return dynamic_cast< te::gm::Surface*
>( convexHullPtr.get() )->getArea();
boost::mutex * m_mutexPtr
static double getPt1ConvexHullArea(const std::vector< GTParameters::TiePoint > &tiePoints)
Returns the tie-points convex hull area (GTParameters::TiePoint::first).
std::vector< TiePoint > m_tiePoints
Tie points.
GTFilter()
Default constructor.
double * m_bestParamsMaxDMapErrorPtr
unsigned long int RansacIntegerCounterT
RANSAC integer counter type.
~ApplyRansacThreadEntryThreadParams()
double m_maxDirectMapError
A point with x and y coordinate values.
std::map< double, GTParameters::TiePoint > const * m_tpsMapPtr
A map from accumulated probabilities (normalized between 0 and 1) to tie-points data.
RansacIntegerCounterT m_procsNumber
MultiPoint is a GeometryCollection whose elements are restricted to points.
std::pair< Coord2D, Coord2D > TiePoint
Tie point type definition.
bool applyRansac(const std::string &transfName, const GTParameters &inputParams, const double maxDirectMapError, const double maxInverseMapError, const RansacIntegerCounterT &maxIterations, const double &assurance, const bool enableMultiThread, const std::vector< double > &tiePointsWeights, std::vector< te::gm::GTParameters::TiePoint > &outTiePoints, std::auto_ptr< GeometricTransformation > &outTransf)
Apply a RANSAC based outliers remotion strategy.
TECOMMONEXPORT unsigned int GetPhysProcNumber()
Returns the number of physical processors.
std::vector< te::gm::GTParameters::TiePoint > * m_bestTiePoinsPtr
MultiPoint is a GeometryCollection whose elements are restricted to points.
#define RANSACGETMAXINVALIDITERATIONS(assurance, dynamicMaxIterations)
Parameters used by the GTFilter::applyRansacThreadEntry method.
A point with x and y coordinate values.
#define RANSACGETMAXITERATIONS(goodTPNumber, totalTPNumber, modelRequiredTPNumber, procsNumber, assurance)
2D Geometric transformation outliers remotion filter.
Surface is an abstract class that represents a 2-dimensional geometric objects.
bool m_useDynamicIterationsNumber
const ApplyRansacThreadEntryThreadParams & operator=(const ApplyRansacThreadEntryThreadParams &other)
static GeometricTransformation * make(const std::string &factoryKey)
It creates an object with the appropriated factory.
static void applyRansacThreadEntry(te::gm::GTFilter::ApplyRansacThreadEntryThreadParams *paramsPtr)
Surf locator thread entry.
GTParameters const * m_inputGTParamsPtr
double * m_bestParamsConvexHullAreaPtr
double m_maxInverseMapError
void add(Geometry *g)
It adds the geometry into the collection.
void reset()
Clear all internal allocated resources and reset the parameters instance to its initial state...
std::auto_ptr< GeometricTransformation > * m_bestTransformationPtrPtr
ApplyRansacThreadEntryThreadParams()
2D Geometric transformation factory.
virtual Geometry * convexHull() const
This method calculates the Convex Hull of a geometry.
2D Geometric transformation parameters.
bool * m_keepRunningFlagPtr
double * m_bestParamsMaxIMapErrorPtr
RansacIntegerCounterT * m_dynamicMaxIterationsPtr
Surface is an abstract class that represents a 2-dimensional geometric objects.
std::string const * m_transfNamePtr