diff --git a/apps/ro-localization/CPosePDFParticlesExtended.cpp b/apps/ro-localization/CPosePDFParticlesExtended.cpp index 659a2bce41..86aae1c804 100644 --- a/apps/ro-localization/CPosePDFParticlesExtended.cpp +++ b/apps/ro-localization/CPosePDFParticlesExtended.cpp @@ -39,20 +39,18 @@ namespace mrpt { namespace poses { - class MyStandardProposal { -public: + public: static constexpr bool DoesResampling = true; }; class MyAuxiliaryPfOptimal { -public: + public: static constexpr bool DoesResampling = false; }; - IMPLEMENTS_SERIALIZABLE(CPosePDFParticlesExtended, CPosePDF, mrpt::poses) /*--------------------------------------------------------------- @@ -102,8 +100,7 @@ void CPosePDFParticlesExtended::copyFrom(const CPosePDF& o) m_particles.clear(); m_particles.resize(M); - for (itDest = m_particles.begin(), - partsIt = parts.begin(); + for (itDest = m_particles.begin(), partsIt = parts.begin(); itDest != m_particles.end(); itDest++, partsIt++) { itDest->log_w = 0; @@ -274,8 +271,7 @@ void CPosePDFParticlesExtended::getCovarianceAndMean( double lin_w_sum = 0; - for (i = 0; i < n; i++) - lin_w_sum += exp(m_particles[i].log_w); + for (i = 0; i < n; i++) lin_w_sum += exp(m_particles[i].log_w); if (lin_w_sum == 0) lin_w_sum = 1; for (i = 0; i < n; i++) @@ -285,8 +281,8 @@ void CPosePDFParticlesExtended::getCovarianceAndMean( // Manage 1 PI range: double err_x = m_particles[i].d->pose.x() - mean.x(); double err_y = m_particles[i].d->pose.y() - mean.y(); - double err_phi = wrapToPi( - fabs(m_particles[i].d->pose.phi() - mean_phi)); + double err_phi = + wrapToPi(fabs(m_particles[i].d->pose.phi() - mean_phi)); var_x += square(err_x) * w; var_y += square(err_y) * w; @@ -325,8 +321,7 @@ void CPosePDFParticlesExtended::writeToStream( // The data n = uint32_t(m_particles.size()); out << n; - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) out << it->log_w << it->d->pose << it->d->state; } } @@ -350,8 +345,7 @@ void CPosePDFParticlesExtended::readFromStream( // The data in >> n; m_particles.resize(n); - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) in >> it->log_w >> it->d->pose >> it->d->state; } break; @@ -365,9 +359,11 @@ void CPosePDFParticlesExtended::readFromStream( ---------------------------------------------------------------*/ void CPosePDFParticlesExtendedPF::offsetTransitionModel(double& val) { - if (getRandomGenerator().drawUniform(0.0, 1.0) < options.probabilityChangingBias) + if (getRandomGenerator().drawUniform(0.0, 1.0) < + options.probabilityChangingBias) { - val = getRandomGenerator().drawUniform(-options.changingBiasUnifRange, 1.0); + val = getRandomGenerator().drawUniform( + -options.changingBiasUnifRange, 1.0); // val = min( val, 0.5f*options.changingBiasUnifRange ); // val = max( val,-0.5f*options.changingBiasUnifRange ); } @@ -379,7 +375,8 @@ void CPosePDFParticlesExtendedPF::offsetTransitionModel(double& val) ---------------------------------------------------------------*/ template <> -void CPosePDFParticlesExtendedPF::prediction_and_update( +void CPosePDFParticlesExtendedPF::prediction_and_update< + mrpt::poses::MyStandardProposal>( const mrpt::obs::CActionCollection* actions, const mrpt::obs::CSensoryFrame* sf, const bayes::CParticleFilter::TParticleFilterOptions& PF_options) @@ -450,7 +447,7 @@ void CPosePDFParticlesExtendedPF::prediction_and_updatelog_w += auxiliarComputeObservationLikelihood( - PF_options, &m_poseParticles, i, sf, it->d.get()) * + PF_options, i, sf, it->d.get()) * PF_options.powFactor; }; @@ -463,7 +460,8 @@ void CPosePDFParticlesExtendedPF::prediction_and_update -void CPosePDFParticlesExtendedPF::prediction_and_update( +void CPosePDFParticlesExtendedPF::prediction_and_update< + mrpt::poses::MyAuxiliaryPfOptimal>( const mrpt::obs::CActionCollection* actions, const mrpt::obs::CSensoryFrame* sf, const bayes::CParticleFilter::TParticleFilterOptions& PF_options) @@ -495,7 +493,7 @@ void CPosePDFParticlesExtendedPF::prediction_and_update maxLik_k) { maxLik_k = lik; @@ -627,7 +620,7 @@ void CPosePDFParticlesExtendedPF::prediction_and_update= MAX_TIMEOUT) newPose = bestNewPose; @@ -703,13 +697,11 @@ void CPosePDFParticlesExtended::resetDeterministic( { clear(); m_particles.resize(particlesCount); - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) it->d.reset(new TExtendedCPose2D()); } - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) { *it->d = location; it->log_w = 0; @@ -770,8 +762,7 @@ void CPosePDFParticlesExtended::saveToTextFile(const std::string& file) const for (unsigned int i = 0; i < m_particles.size(); i++) os::fprintf( f, "%f %f %f %e\n", m_particles[i].d->pose.x(), - m_particles[i].d->pose.y(), - m_particles[i].d->pose.phi(), + m_particles[i].d->pose.y(), m_particles[i].d->pose.phi(), m_particles[i].log_w); os::fclose(f); @@ -794,8 +785,7 @@ void CPosePDFParticlesExtended::changeCoordinatesReference( CPose2D newReferenceBase = CPose2D(newReferenceBase_); CParticleList::iterator it; - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) it->d->pose = newReferenceBase + it->d->pose; } @@ -808,8 +798,7 @@ void CPosePDFParticlesExtended::drawSingleSample(CPose2D& outPart) const double cum = 0; CParticleList::const_iterator it; - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) { cum += exp(it->log_w); if (uni <= cum) @@ -856,8 +845,7 @@ void CPosePDFParticlesExtended::operator+=(const CPose2D& Ap) { CParticleList::iterator it; - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) it->d->pose = it->d->pose + Ap; } @@ -873,8 +861,7 @@ void CPosePDFParticlesExtended::inverse(CPosePDF& o) const static CPose2D nullPose(0, 0, 0); for (unsigned int i = 0; i < out->m_particles.size(); i++) - out->m_particles[i].d->pose = - nullPose - out->m_particles[i].d->pose; + out->m_particles[i].d->pose = nullPose - out->m_particles[i].d->pose; } /*--------------------------------------------------------------- @@ -882,12 +869,10 @@ void CPosePDFParticlesExtended::inverse(CPosePDF& o) const ---------------------------------------------------------------*/ CPose2D CPosePDFParticlesExtended::getMostLikelyParticle() const { - CParticleList::const_iterator it, - itMax = m_particles.begin(); + CParticleList::const_iterator it, itMax = m_particles.begin(); double max_w = -1e300; - for (it = m_particles.begin(); - it != m_particles.end(); it++) + for (it = m_particles.begin(); it != m_particles.end(); it++) { if (it->log_w > max_w) { @@ -984,8 +969,8 @@ CPosePDFParticlesExtendedPF::TPredictionParams::TPredictionParams() ---------------------------------------------------------------*/ double CPosePDFParticlesExtendedPF::auxiliarComputeObservationLikelihood( const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const CParticleFilterCapable* obj, size_t particleIndexForMap, - const CSensoryFrame* observation, const TExtendedCPose2D* x) + size_t particleIndexForMap, const CSensoryFrame* observation, + const TExtendedCPose2D* x) { double ret = 1; CMetricMap* map; // The map: @@ -1035,8 +1020,7 @@ double CPosePDFParticlesExtendedPF::auxiliarComputeObservationLikelihood( ---------------------------------------------------------------*/ double CPosePDFParticlesExtendedPF::particlesEvaluator_AuxPFOptimal( const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const CParticleFilterCapable* obj, size_t index, const void* action, - const void* observation) + size_t index, const CPose2D* action, const CSensoryFrame* observation) { MRPT_START @@ -1052,15 +1036,13 @@ double CPosePDFParticlesExtendedPF::particlesEvaluator_AuxPFOptimal( // , take the mean of the posterior density: // -------------------------------------------- TExtendedCPose2D x_predict = *m_poseParticles.m_particles[index].d; - x_predict.pose = x_predict.pose + *static_cast(action); + x_predict.pose = x_predict.pose + *action; // and compute the obs. likelihood: // -------------------------------------------- return ret + (m_pfAuxiliaryPFOptimal_estimatedProb[index] = auxiliarComputeObservationLikelihood( - PF_options, obj, index, - static_cast(observation), - &x_predict)); + PF_options, index, observation, &x_predict)); MRPT_END } @@ -1078,12 +1060,14 @@ void CParticleFilter::executeOn( { case CParticleFilter::pfStandardProposal: executeOn< - mrpt::poses::CPosePDFParticlesExtendedPF, mrpt::poses::MyStandardProposal>( + mrpt::poses::CPosePDFParticlesExtendedPF, + mrpt::poses::MyStandardProposal>( obj, action, observation, stats); break; case CParticleFilter::pfAuxiliaryPFOptimal: executeOn< - mrpt::poses::CPosePDFParticlesExtendedPF, mrpt::poses::MyAuxiliaryPfOptimal>( + mrpt::poses::CPosePDFParticlesExtendedPF, + mrpt::poses::MyAuxiliaryPfOptimal>( obj, action, observation, stats); break; default: diff --git a/apps/ro-localization/CPosePDFParticlesExtended.h b/apps/ro-localization/CPosePDFParticlesExtended.h index 903241a6f9..f6180c750c 100644 --- a/apps/ro-localization/CPosePDFParticlesExtended.h +++ b/apps/ro-localization/CPosePDFParticlesExtended.h @@ -50,19 +50,18 @@ class CPosePDFParticlesExtended DEFINE_SERIALIZABLE(CPosePDFParticlesExtended) public: - /** Free all the memory associated to particles, and set the number of parts * = 0 */ void clear(); public: /** Constructor - * \param M The number of particles. - */ + * \param M The number of particles. + */ CPosePDFParticlesExtended(size_t M = 1); /** Copy constructor: - */ + */ CPosePDFParticlesExtended(const CPosePDFParticlesExtended& obj) { copyFrom(obj); @@ -74,7 +73,7 @@ class CPosePDFParticlesExtended /** Copy operator, translating if necesary (for example, between particles * and gaussian representations) - */ + */ void copyFrom(const CPosePDF& o) override; /** Reset the PDF to a single point: All particles will be set exactly to @@ -89,10 +88,10 @@ class CPosePDFParticlesExtended /** Reset the PDF to an uniformly distributed one, inside of the defined * cube. - * If particlesCount is set to -1 the number of particles remains + * If particlesCount is set to -1 the number of particles remains * unchanged. - * \sa resetDeterministic, resetUniformFreeSpace - */ + * \sa resetDeterministic, resetUniformFreeSpace + */ void resetUniform( float x_min, float x_max, float y_min, float y_max, mrpt::math::CVectorFloat state_min, mrpt::math::CVectorFloat state_max, @@ -110,12 +109,12 @@ class CPosePDFParticlesExtended /** Returns an estimate of the pose covariance matrix (3x3 cov.matrix for * x,y,phi variables) - */ + */ void getCovarianceAndMean( mrpt::math::CMatrixDouble33& C, CPose2D& p) const override; /** Returns the pose of the i'th particle. - */ + */ CPose2D getParticlePose(int i) const; /** Save PDF's particles to a text file. In each line it will go: "x y phi @@ -128,53 +127,53 @@ class CPosePDFParticlesExtended size_t size() const { return m_particles.size(); } /** This can be used to convert a PDF from local coordinates to global, * providing the point (newReferenceBase) from which - * "to proyect" the current pdf. Result PDF substituted the currently + * "to proyect" the current pdf. Result PDF substituted the currently * stored one in the object. - */ + */ void changeCoordinatesReference( const mrpt::poses::CPose3D& newReferenceBase) override; /** Draws a single sample from the distribution (WARNING: weights are * assumed to be normalized!) - */ + */ void drawSingleSample(mrpt::poses::CPose2D& outPart) const override; /** Draws a number of samples from the distribution, and saves as a list of * 1x3 vectors, where each row contains a (x,y,phi) datum. - */ + */ void drawManySamples( size_t N, std::vector& outSamples) const override; /** Appends (pose-composition) a given pose "p" to each particle - */ + */ void operator+=(const mrpt::poses::CPose2D& Ap); /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF - */ + */ void inverse(mrpt::poses::CPosePDF& o) const override; /** Returns the particle with the highest weight. - */ + */ CPose2D getMostLikelyParticle() const; /** Bayesian fusion. - */ + */ void bayesianFusion( const mrpt::poses::CPosePDF& p1, const mrpt::poses::CPosePDF& p2, const double& minMahalanobisDistToDrop = 0) override; /** Evaluates the PDF at a given arbitrary point as reconstructed by a * Parzen window. - * \sa saveParzenPDFToTextFile - */ + * \sa saveParzenPDFToTextFile + */ double evaluatePDF_parzen( float x, float y, float phi, float stdXY, float stdPhi) const; /** Save a text file (compatible with matlab) representing the 2D evaluation * of the PDF as reconstructed by a Parzen window. - * \sa evaluatePDF_parzen - */ + * \sa evaluatePDF_parzen + */ void saveParzenPDFToTextFile( const char* fileName, float x_min, float x_max, float y_min, float y_max, float phi, float stepSizeXY, float stdXY, @@ -187,12 +186,11 @@ class CPosePDFParticlesExtended THROW_EXCEPTION("Not implemented"); } - }; // End of class def. class CPosePDFParticlesExtendedPF { -public: + public: using CParticleList = CPosePDFParticlesExtended::CParticleList; CPosePDFParticlesExtended m_poseParticles; CPosePDFParticlesExtendedPF(size_t M = 1) : m_poseParticles(M) {} @@ -205,50 +203,50 @@ class CPosePDFParticlesExtendedPF struct TPredictionParams { /** Default settings method. - */ + */ TPredictionParams(); /** [update stage] Must be set to a metric map used to estimate the * likelihood of observations - */ + */ mrpt::maps::CMetricMap* metricMap; /** [update stage] Alternative way (if metricMap==nullptr): A metric map * is supplied for each particle: There must be the same maps here as * pose particles. - */ + */ mrpt::maps::TMetricMapList metricMaps; /** Parameters for the KLD adaptive sample size algorithm (see Dieter * Fox's papers), which is used only if the CParticleFilter is created * with the "adaptiveSampleSize" flag set to true. - */ + */ float KLD_binSize_XY, KLD_binSize_PHI, KLD_delta, KLD_epsilon; /** Parameters for the KLD adaptive sample size algorithm (see Dieter * Fox's papers), which is used only if the CParticleFilter is created * with the "adaptiveSampleSize" flag set to true. - */ + */ unsigned int KLD_minSampleSize, KLD_maxSampleSize; /** In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", the number * of samples for searching the maximum likelihood value (see papers!) - */ + */ unsigned int pfAuxFilterOptimal_MaximumSearchSamples; /** The probability [0,1] of changing the UWB bias (individual for each * beacon's bias). - */ + */ float probabilityChangingBias; /** The bias of each beacon will be added a uniform random variable * [-changingBiasUnifRange/2,changingBiasUnifRange/2] with a probability * "probabilityChangingBias". - */ + */ float changingBiasUnifRange; /** A number between 0 and 1 (0=standard proposal). - */ + */ float mixtureProposalRatio; } options; @@ -271,39 +269,38 @@ class CPosePDFParticlesExtendedPF const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options); -private: + private: void offsetTransitionModel(double& val); /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. - */ + */ mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb; /** Auxiliary function that evaluates the likelihood of an observation, * given a robot pose, and according to the options in * "CPosePDFParticlesExtended::options". - */ + */ double auxiliarComputeObservationLikelihood( const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const bayes::CParticleFilterCapable* obj, size_t particleIndexForMap, - const mrpt::obs::CSensoryFrame* observation, const TExtendedCPose2D* x); + size_t particleIndexForMap, const mrpt::obs::CSensoryFrame* observation, + const TExtendedCPose2D* x); /** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal" - */ + */ double particlesEvaluator_AuxPFOptimal( const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const bayes::CParticleFilterCapable* obj, size_t index, - const void* action, const void* observation); - + size_t index, const CPose2D* action, + const mrpt::obs::CSensoryFrame* observation); }; /** Auxiliary structure - */ + */ struct TPoseBin { int x, y, phi; }; /** Auxiliary structure - */ + */ struct lt_TPoseBin { bool operator()(const TPoseBin& s1, const TPoseBin& s2) const @@ -311,7 +308,7 @@ struct lt_TPoseBin return s1.x < s2.x; } }; -} // End of namespace -} // End of namespace +} // namespace poses +} // namespace mrpt #endif diff --git a/libs/base/include/mrpt/bayes/CParticleFilter.h b/libs/base/include/mrpt/bayes/CParticleFilter.h index 48a0e57faa..23fc523d8e 100644 --- a/libs/base/include/mrpt/bayes/CParticleFilter.h +++ b/libs/base/include/mrpt/bayes/CParticleFilter.h @@ -27,8 +27,6 @@ class CActionCollection; */ namespace bayes { -class CParticleFilterCapable; - /** This class acts as a common interface to the different interfaces (see *CParticleFilter::TParticleFilterAlgorithm) any bayes::CParticleFilterCapable *class can implement: it is the invoker of particle filter algorithms. diff --git a/libs/base/include/mrpt/bayes/CParticleFilterCapable.h b/libs/base/include/mrpt/bayes/CParticleFilterCapable.h index 772bdcf4e5..fb54fedd57 100644 --- a/libs/base/include/mrpt/bayes/CParticleFilterCapable.h +++ b/libs/base/include/mrpt/bayes/CParticleFilterCapable.h @@ -57,10 +57,7 @@ class CParticleFilterCapable * flexibility. * \sa prepareFastDrawSample */ - using TParticleProbabilityEvaluator = std::function; + using TParticleProbabilityEvaluator = std::function; /** The default evaluator function, which simply returns the particle * weight. @@ -68,15 +65,10 @@ class CParticleFilterCapable * flexibility. * \sa prepareFastDrawSample */ - static double defaultEvaluator( - const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const CParticleFilterCapable* obj, size_t index, const void* action, - const void* observation) + void prepareFastDrawSample( + const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const { - MRPT_UNUSED_PARAM(PF_options); - MRPT_UNUSED_PARAM(action); - MRPT_UNUSED_PARAM(observation); - return obj->getW(index); + prepareFastDrawSample(PF_options, [this](size_t i) { return getW(i); }); } /** Prepares data structures for calling fastDrawSample method next. @@ -121,8 +113,7 @@ class CParticleFilterCapable */ void prepareFastDrawSample( const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - TParticleProbabilityEvaluator partEvaluator = defaultEvaluator, - const void* action = nullptr, const void* observation = nullptr) const; + TParticleProbabilityEvaluator partEvaluator) const; /** Draws a random sample from the particle filter, in such a way that each *particle has a probability proportional to its weight (in the standard PF diff --git a/libs/base/include/mrpt/bayes/CParticleFilterData.h b/libs/base/include/mrpt/bayes/CParticleFilterData.h index c27186bf3e..e7986e4bf2 100644 --- a/libs/base/include/mrpt/bayes/CParticleFilterData.h +++ b/libs/base/include/mrpt/bayes/CParticleFilterData.h @@ -13,6 +13,7 @@ #include #include + #include #include @@ -20,8 +21,6 @@ namespace mrpt { namespace bayes { -class CParticleFilterCapable; - /** A curiously recurring template pattern (CRTP) approach to providing the * basic functionality of any CParticleFilterData<> class. * Users should inherit from CParticleFilterData<>, which in turn will diff --git a/libs/base/include/mrpt/poses/CPointPDFParticles.h b/libs/base/include/mrpt/poses/CPointPDFParticles.h index 3b542699c9..bc08a37011 100644 --- a/libs/base/include/mrpt/poses/CPointPDFParticles.h +++ b/libs/base/include/mrpt/poses/CPointPDFParticles.h @@ -11,7 +11,6 @@ #include #include -#include #include #include diff --git a/libs/base/include/mrpt/poses/CPose3DPDFParticles.h b/libs/base/include/mrpt/poses/CPose3DPDFParticles.h index 46a85c37c9..2279abb345 100644 --- a/libs/base/include/mrpt/poses/CPose3DPDFParticles.h +++ b/libs/base/include/mrpt/poses/CPose3DPDFParticles.h @@ -11,7 +11,6 @@ #include #include -#include #include namespace mrpt diff --git a/libs/base/include/mrpt/poses/CPosePDFParticles.h b/libs/base/include/mrpt/poses/CPosePDFParticles.h index 79b8d1fe23..2ed8f82544 100644 --- a/libs/base/include/mrpt/poses/CPosePDFParticles.h +++ b/libs/base/include/mrpt/poses/CPosePDFParticles.h @@ -11,7 +11,6 @@ #include #include -#include #include namespace mrpt diff --git a/libs/base/src/bayes/CParticleFilter.cpp b/libs/base/src/bayes/CParticleFilter.cpp index 05268aa5e4..0bcdd88528 100644 --- a/libs/base/src/bayes/CParticleFilter.cpp +++ b/libs/base/src/bayes/CParticleFilter.cpp @@ -11,7 +11,6 @@ #include // for exp #include // for CParticleFilter::TPar... -#include // for CParticleFilterCapable #include // for CConfigFileBase, MRPT... #include // for CStream #include // for size_t diff --git a/libs/base/src/bayes/CParticleFilterCapable.cpp b/libs/base/src/bayes/CParticleFilterCapable.cpp index 7f30142408..d7851b10ac 100644 --- a/libs/base/src/bayes/CParticleFilterCapable.cpp +++ b/libs/base/src/bayes/CParticleFilterCapable.cpp @@ -211,7 +211,8 @@ void CParticleFilterCapable::computeResampling( double T_offset = 0; for (i = 0; i < M; i++) { - T[i] = T_offset + getRandomGenerator().drawUniform(0.0, _1_M_eps); + T[i] = + T_offset + getRandomGenerator().drawUniform(0.0, _1_M_eps); T_offset += _1_M; } T[M] = 1; @@ -262,9 +263,8 @@ void CParticleFilterCapable::computeResampling( } break; default: - THROW_EXCEPTION( - format( - "ERROR: Unknown resampling method selected: %i", method)); + THROW_EXCEPTION(format( + "ERROR: Unknown resampling method selected: %i", method)); }; MRPT_END @@ -275,8 +275,7 @@ void CParticleFilterCapable::computeResampling( ---------------------------------------------------------------*/ void CParticleFilterCapable::prepareFastDrawSample( const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - TParticleProbabilityEvaluator partEvaluator, const void* action, - const void* observation) const + TParticleProbabilityEvaluator partEvaluator) const { MRPT_START @@ -309,9 +308,7 @@ void CParticleFilterCapable::prepareFastDrawSample( // ------------------------------------------------------------------- double SUM = 0; // Save the log likelihoods: - for (i = 0; i < M; i++) - m_fastDrawAuxiliary.PDF[i] = - partEvaluator(PF_options, this, i, action, observation); + for (i = 0; i < M; i++) m_fastDrawAuxiliary.PDF[i] = partEvaluator(i); // "Normalize": m_fastDrawAuxiliary.PDF += -math::maximum(m_fastDrawAuxiliary.PDF); for (i = 0; i < M; i++) @@ -345,11 +342,12 @@ void CParticleFilterCapable::prepareFastDrawSample( // Done! #if !defined(_MSC_VER) || (_MSC_VER > 1400) // <=VC2005 doesn't work with this! - MRPT_END_WITH_CLEAN_UP( \ - /* Debug: */ \ - cout << "j=" << j << "\nm_fastDrawAuxiliary.CDF_indexes:" << m_fastDrawAuxiliary.CDF_indexes << endl; \ - cout << "m_fastDrawAuxiliary.CDF:" << m_fastDrawAuxiliary.CDF << endl; \ - ); + MRPT_END_WITH_CLEAN_UP(/* Debug: */ + cout << "j=" << j + << "\nm_fastDrawAuxiliary.CDF_indexes:" + << m_fastDrawAuxiliary.CDF_indexes << endl; + cout << "m_fastDrawAuxiliary.CDF:" + << m_fastDrawAuxiliary.CDF << endl;); #else MRPT_END #endif @@ -364,10 +362,7 @@ void CParticleFilterCapable::prepareFastDrawSample( // selected: size_t i, M = particlesCount(); vector PDF(M, 0); - for (i = 0; i < M; i++) - PDF[i] = partEvaluator( - PF_options, this, i, action, - observation); // Default evaluator: takes current weight. + for (i = 0; i < M; i++) PDF[i] = partEvaluator(i); vector idxs; diff --git a/libs/hmtslam/include/mrpt/hmtslam/CLocalMetricHypothesis.h b/libs/hmtslam/include/mrpt/hmtslam/CLocalMetricHypothesis.h index 1e1929a480..d9d31dddbb 100644 --- a/libs/hmtslam/include/mrpt/hmtslam/CLocalMetricHypothesis.h +++ b/libs/hmtslam/include/mrpt/hmtslam/CLocalMetricHypothesis.h @@ -9,8 +9,6 @@ #ifndef CLocalMetricHypothesis_H #define CLocalMetricHypothesis_H -#include - #include #include @@ -265,8 +263,7 @@ class CLocalMetricHypothesis : public mrpt::utils::CSerializable */ double particlesEvaluator_AuxPFOptimal( const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const mrpt::bayes::CParticleFilterCapable* obj, size_t index, - const void* action, const void* observation); + size_t index, const mrpt::obs::CSensoryFrame* observation); /** Auxiliary function that evaluates the likelihood of an observation, * given a robot pose, and according to the options in diff --git a/libs/hmtslam/src/CHMTSLAM_LSLAM_RBPF_2DLASER.cpp b/libs/hmtslam/src/CHMTSLAM_LSLAM_RBPF_2DLASER.cpp index f233833866..10e2cfcb91 100644 --- a/libs/hmtslam/src/CHMTSLAM_LSLAM_RBPF_2DLASER.cpp +++ b/libs/hmtslam/src/CHMTSLAM_LSLAM_RBPF_2DLASER.cpp @@ -393,9 +393,10 @@ void CLocalMetricHypothesis::prediction_and_update( tictac.Tic(); using namespace std::placeholders; m_poseParticles.prepareFastDrawSample( - PF_options, std::bind(&CLocalMetricHypothesis::particlesEvaluator_AuxPFOptimal, this, - _1,_2, _3, _4, _5), - robotMovement, sf); + PF_options, [&](size_t i) + { + return this->particlesEvaluator_AuxPFOptimal(PF_options, i, sf); + }); printf("[prepareFastDrawSample] Done in %.06f ms\n", tictac.Tac() * 1e3f); #if 0 @@ -631,12 +632,9 @@ void CLocalMetricHypothesis::prediction_and_update( particlesEvaluator_AuxPFOptimal ---------------------------------------------------------------*/ double CLocalMetricHypothesis::particlesEvaluator_AuxPFOptimal( - const bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const CParticleFilterCapable* obj, size_t index, const void* action, - const void* observation) + const bayes::CParticleFilter::TParticleFilterOptions& PF_options, size_t index, + const CSensoryFrame* observation) { - MRPT_UNUSED_PARAM(action); - MRPT_START // Compute the quantity: @@ -671,7 +669,7 @@ double CLocalMetricHypothesis::particlesEvaluator_AuxPFOptimal( // Estimate the mean... indivLik = auxiliarComputeObservationLikelihood( - PF_options, index, ((CSensoryFrame*)observation), &x_predict); + PF_options, index, observation, &x_predict); MRPT_CHECK_NORMAL_NUMBER(indivLik); vectLiks[q] = indivLik; diff --git a/libs/slam/include/mrpt/maps/CMultiMetricMapPDF.h b/libs/slam/include/mrpt/maps/CMultiMetricMapPDF.h index 201fe80fcc..33088f7189 100644 --- a/libs/slam/include/mrpt/maps/CMultiMetricMapPDF.h +++ b/libs/slam/include/mrpt/maps/CMultiMetricMapPDF.h @@ -16,7 +16,6 @@ #include -#include #include #include diff --git a/libs/slam/include/mrpt/slam/CMetricMapBuilderRBPF.h b/libs/slam/include/mrpt/slam/CMetricMapBuilderRBPF.h index c437298526..4557a93def 100644 --- a/libs/slam/include/mrpt/slam/CMetricMapBuilderRBPF.h +++ b/libs/slam/include/mrpt/slam/CMetricMapBuilderRBPF.h @@ -14,7 +14,6 @@ #include #include -#include #include #include @@ -81,7 +80,7 @@ class CMetricMapBuilderRBPF : public mrpt::slam::CMetricMapBuilder public: /** Options for building a CMetricMapBuilderRBPF object, passed to the * constructor. - */ + */ struct TConstructionOptions : public utils::CLoadableOptions { public: @@ -128,16 +127,16 @@ of type CMetricMapBuilderRBPF */ const mrpt::poses::CPosePDF* x0 = nullptr); /** Clear all elements of the maps. - */ + */ void clear(); /** Returns a copy of the current best pose estimation as a pose PDF. - */ + */ mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const; /** Returns the current most-likely path estimation (the path associated to * the most likely particle). - */ + */ void getCurrentMostLikelyPath( std::deque& outPath) const; @@ -156,25 +155,25 @@ of type CMetricMapBuilderRBPF */ /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so * far built map. - */ + */ void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap& out_map) const; /** Returns the map built so far. NOTE that for efficiency a pointer to the * internal object is passed, DO NOT delete nor modify the object in any * way, if desired, make a copy of ir with "clone()". - */ + */ const mrpt::maps::CMultiMetricMap* getCurrentlyBuiltMetricMap() const; /** Returns just how many sensory-frames are stored in the currently build * map. - */ + */ unsigned int getCurrentlyBuiltMapSize(); /** A useful method for debugging: the current map (and/or poses) estimation * is dumped to an image file. - * \param file The output file name - * \param formatEMF_BMP Output format = true:EMF, false:BMP - */ + * \param file The output file name + * \param formatEMF_BMP Output format = true:EMF, false:BMP + */ void saveCurrentEstimationToImage( const std::string& file, bool formatEMF_BMP = true); @@ -185,14 +184,14 @@ of type CMetricMapBuilderRBPF */ /** A logging utility: saves the current path estimation for each particle * in a text file (a row per particle, each 3-column-entry is a set * [x,y,phi], respectively). - */ + */ void saveCurrentPathEstimationToTextFile(const std::string& fil); double getCurrentJointEntropy(); /** This structure will hold stats after each execution of * processActionObservation - */ + */ struct TStats { TStats() : observationsInserted(false) {} @@ -206,6 +205,6 @@ of type CMetricMapBuilderRBPF */ }; // End of class def. -} // end NS -} // end NS +} // namespace slam +} // namespace mrpt #endif diff --git a/libs/slam/include/mrpt/slam/CMonteCarloLocalization2D.h b/libs/slam/include/mrpt/slam/CMonteCarloLocalization2D.h index eb09f9830d..16ceac12e1 100644 --- a/libs/slam/include/mrpt/slam/CMonteCarloLocalization2D.h +++ b/libs/slam/include/mrpt/slam/CMonteCarloLocalization2D.h @@ -34,13 +34,11 @@ namespace slam * application "app/pf-localization" for an example of usage. * * \sa CMonteCarloLocalization3D, CPose2D, CPosePDF, CPoseGaussianPDF, - * CParticleFilterCapable * \ingroup mrpt_slam_grp */ -class CMonteCarloLocalization2D : - // public mrpt::bayes::CParticleFilterCapable, - public PF_implementation< - mrpt::poses::CPose2D, mrpt::poses::CPosePDFParticles> +class CMonteCarloLocalization2D + : public PF_implementation< + mrpt::poses::CPose2D, mrpt::poses::CPosePDFParticles> { public: using CParticleDataContent = @@ -152,7 +150,7 @@ class CMonteCarloLocalization2D : }; // End of class def. -} // End of namespace -} // End of namespace +} // namespace slam +} // namespace mrpt #endif diff --git a/libs/slam/include/mrpt/slam/CMonteCarloLocalization3D.h b/libs/slam/include/mrpt/slam/CMonteCarloLocalization3D.h index dab19929f5..1b6c594569 100644 --- a/libs/slam/include/mrpt/slam/CMonteCarloLocalization3D.h +++ b/libs/slam/include/mrpt/slam/CMonteCarloLocalization3D.h @@ -25,12 +25,11 @@ namespace slam * the MRPT application "app/pf-localization" for an example of usage. * * \sa CMonteCarloLocalization2D, CPose2D, CPosePDF, CPoseGaussianPDF, - * CParticleFilterCapable * \ingroup mrpt_slam_grp */ class CMonteCarloLocalization3D : public PF_implementation< - mrpt::poses::CPose3D, mrpt::poses::CPose3DPDFParticles> + mrpt::poses::CPose3D, mrpt::poses::CPose3DPDFParticles> { public: using CParticleDataContent = @@ -97,7 +96,7 @@ class CMonteCarloLocalization3D }; // End of class def. -} // End of namespace -} // End of namespace +} // namespace slam +} // namespace mrpt #endif diff --git a/libs/slam/include/mrpt/slam/PF_implementations.h b/libs/slam/include/mrpt/slam/PF_implementations.h index 0d8495075c..4a34a79548 100644 --- a/libs/slam/include/mrpt/slam/PF_implementations.h +++ b/libs/slam/include/mrpt/slam/PF_implementations.h @@ -254,8 +254,8 @@ class AuxPFOptimalEvaluator template static double evaluate( const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const mrpt::bayes::CParticleFilterCapable* obj, unsigned long index, - const void* action, const void* observation, const MYSELF& me) + unsigned long index, const mrpt::poses::CPose3D* action, + const mrpt::obs::CSensoryFrame* observation, const MYSELF& me) { MRPT_UNUSED_PARAM(action); MRPT_START @@ -283,9 +283,7 @@ class AuxPFOptimalEvaluator // Estimate the mean... indivLik = me.PF_SLAM_computeObservationLikelihoodForParticle( - PF_options, index, - *static_cast(observation), - x_predict); + PF_options, index, *observation, x_predict); MRPT_CHECK_NORMAL_NUMBER(indivLik); vectLiks[q] = indivLik; @@ -330,8 +328,8 @@ class AuxPFStandardEvaluator template static double evaluate( const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options, - const mrpt::bayes::CParticleFilterCapable* obj, unsigned long index, - const void* action, const void* observation, const MYSELF& me) + unsigned long index, const mrpt::poses::CPose3D* action, + const mrpt::obs::CSensoryFrame* observation, const MYSELF& me) { MRPT_START @@ -347,16 +345,13 @@ class AuxPFStandardEvaluator // Just use the mean: // , take the mean of the posterior density: mrpt::poses::CPose3D x_predict; - x_predict.composeFrom( - oldPose, *static_cast(action)); + x_predict.composeFrom(oldPose, *action); // and compute the obs. likelihood: // -------------------------------------------- me.m_pfAuxiliaryPFStandard_estimatedProb[index] = me.PF_SLAM_computeObservationLikelihoodForParticle( - PF_options, index, - *static_cast(observation), - x_predict); + PF_options, index, *observation, x_predict); // Combined log_likelihood: Previous weight * obs_likelihood: return cur_logweight + @@ -385,9 +380,7 @@ class AuxPFStandardEvaluator // Estimate the mean... indivLik = me.PF_SLAM_computeObservationLikelihoodForParticle( - PF_options, index, - *static_cast(observation), - x_predict); + PF_options, index, *observation, x_predict); MRPT_CHECK_NORMAL_NUMBER(indivLik); vectLiks[q] = indivLik; @@ -478,18 +471,12 @@ class AuxiliaryPFStandardAndOptimal me.m_movementDrawer.getSamplingMean3D(meanRobotMovement); mrpt::bayes::CParticleFilterCapable::TParticleProbabilityEvaluator - func = [&me]( - const bayes::CParticleFilter::TParticleFilterOptions& - PF_options, - const mrpt::bayes::CParticleFilterCapable* obj, - size_t index, const void* action, - const void* observation) -> double { - return EVALUATOR::template evaluate( - PF_options, obj, index, action, observation, me); - }; - - me.m_poseParticles.prepareFastDrawSample( - PF_options, func, &meanRobotMovement, sf); + func = [&](size_t i) { + return EVALUATOR::template evaluate( + PF_options, i, &meanRobotMovement, sf, me); + }; + + me.m_poseParticles.prepareFastDrawSample(PF_options, func); // For USE_OPTIMAL_SAMPLING=1, m_pfAuxiliaryPFOptimal_maxLikelihood is // now computed. @@ -722,7 +709,8 @@ class AuxiliaryPFStandardAndOptimal permutationPathsAuxVector.size() - 1); const size_t idx = - mrpt::random::getRandomGenerator().drawUniform32bit() % + mrpt::random::getRandomGenerator() + .drawUniform32bit() % stateSpaceBinsLastTimestepParticles[idxBinSpacePath] .size(); k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath] @@ -1064,10 +1052,10 @@ class AuxiliaryPFStandardAndOptimal // acceptanceProb = 0; // Keep searching or keep // this one? } - } while ( - ++timeout < maxTries && - acceptanceProb < - mrpt::random::getRandomGenerator().drawUniform(0.0, 0.999)); + } while (++timeout < maxTries && + acceptanceProb < + mrpt::random::getRandomGenerator().drawUniform( + 0.0, 0.999)); if (timeout >= maxTries) {