Permalink
Browse files

Move where executeOn is implemented

The template instantiation was overly complicated.
  • Loading branch information...
jolting authored and jlblancoc committed Nov 22, 2017
1 parent 7c68726 commit ae4571287e36c5b76702c064a9c63125e2d46cad
@@ -737,11 +737,12 @@ void do_pf_localization(
(unsigned int)step,
(unsigned int)pdf.m_poseParticles.particlesCount());
PF.executeOn(
pdf,
pdf.executeOn(
PF,
action.get(), // Action
observations.get(), // Obs.
&PF_stats // Output statistics
&PF_stats, // Output statistics
pfOptions.PF_algorithm
);
if (!SAVE_STATS_ONLY)
@@ -1047,28 +1047,26 @@ double CPosePDFParticlesExtendedPF::particlesEvaluator_AuxPFOptimal(
MRPT_END
}
} // namespace poses
namespace bayes
{
template <>
void CParticleFilter::executeOn<mrpt::poses::CPosePDFParticlesExtendedPF>(
mrpt::poses::CPosePDFParticlesExtendedPF& obj,
void CPosePDFParticlesExtendedPF::executeOn(
CParticleFilter &pf,
const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation, TParticleFilterStats* stats)
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm)
{
switch (m_options.PF_algorithm)
switch (PF_algorithm)
{
case CParticleFilter::pfStandardProposal:
executeOn<
pf.executeOn<
mrpt::poses::CPosePDFParticlesExtendedPF,
mrpt::poses::MyStandardProposal>(
obj, action, observation, stats);
*this, action, observation, stats);
break;
case CParticleFilter::pfAuxiliaryPFOptimal:
executeOn<
pf.executeOn<
mrpt::poses::CPosePDFParticlesExtendedPF,
mrpt::poses::MyAuxiliaryPfOptimal>(
obj, action, observation, stats);
*this, action, observation, stats);
break;
default:
{
@@ -21,6 +21,7 @@
namespace mrpt
{
namespace poses
{
class TExtendedCPose2D
@@ -269,6 +270,12 @@ class CPosePDFParticlesExtendedPF
const mrpt::obs::CSensoryFrame* observation,
const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
void executeOn(
mrpt::bayes::CParticleFilter& pf, const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithms);
private:
void offsetTransitionModel(double& val);
@@ -433,11 +433,12 @@ void TestParticlesLocalization()
"Executing ParticleFilter on %u particles....",
(unsigned int)pf.m_poseParticles.particlesCount());
PF.executeOn(
pf,
pf.executeOn(
PF,
action.get(), // Action
observations.get(), // Obs.
&PF_stats // Output statistics
&PF_stats, // Output statistics
pfOptions.PF_algorithm
);
printf(
" Done! in %.03fms, ESS=%f\n", 1000.0f * tictac.Tac(),
@@ -211,12 +211,6 @@ class CParticleFilter : public mrpt::utils::COutputLogger
const mrpt::obs::CSensoryFrame* observation,
TParticleFilterStats* stats = nullptr);
template <class PARTICLEFILTERCAPABLE>
void executeOn(
PARTICLEFILTERCAPABLE& obj, const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
TParticleFilterStats* stats = nullptr);
/** The options to be used in the PF, must be set before executing any step
* of the particle filter.
*/
@@ -21,6 +21,10 @@
namespace mrpt
{
namespace bayes
{
class CParticleFilter;
}
namespace poses
{
class CPose3DPDFParticles;
@@ -274,6 +278,13 @@ class CLocalMetricHypothesis : public mrpt::utils::CSerializable
size_t particleIndexForMap, const mrpt::obs::CSensoryFrame* observation,
const mrpt::poses::CPose2D* x);
void executeOn(
mrpt::bayes::CParticleFilter& pf, const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm);
protected:
/** @}
*/
@@ -55,27 +55,23 @@ class LSLAMOptimalProposal
MRPT_TODO("MOVE prediction_and_update here");
};
}
namespace bayes
{
template <>
void CParticleFilter::executeOn<CLocalMetricHypothesis>(
CLocalMetricHypothesis& obj, const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation, TParticleFilterStats* stats)
void CLocalMetricHypothesis::executeOn(
mrpt::bayes::CParticleFilter& pf, const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm)
{
switch (m_options.PF_algorithm)
switch (PF_algorithm)
{
case CParticleFilter::pfOptimalProposal:
executeOn<
pf.executeOn<
CLocalMetricHypothesis, mrpt::hmtslam::LSLAMOptimalProposal>(
obj, action, observation, stats);
*this, action, observation, stats);
break;
case CParticleFilter::pfAuxiliaryPFOptimal:
executeOn<
pf.executeOn<
CLocalMetricHypothesis, mrpt::hmtslam::LSLAMAuxiliaryPFOptimal>(
obj, action, observation, stats);
*this, action, observation, stats);
break;
default:
{
@@ -203,9 +199,9 @@ void CLSLAM_RBPF_2DLASER::processOneLMH(
// 1) PROCESS ACTION
// 2) PROCESS OBSERVATIONS
// ------------------------------------------------
CParticleFilter pf;
mrpt::bayes::CParticleFilter pf;
pf.m_options = m_parent->m_options.pf_options;
pf.executeOn(*LMH, actions.get(), sf.get());
LMH->executeOn(pf, actions.get(), sf.get(), nullptr, pf.m_options.PF_algorithm);
// 3) The appearance observation: update the log likelihood
// ...
@@ -64,10 +64,9 @@ class CMultiMetricMapPDFParticles
* \sa mrpt::slam::CMetricMapBuilderRBPF
* \ingroup metric_slam_grp
*/
class CMultiMetricMapPDF
: public mrpt::utils::CSerializable,
public mrpt::slam::PF_implementation<
CRBPFParticleData, CMultiMetricMapPDFParticles>
class CMultiMetricMapPDF : public mrpt::utils::CSerializable,
public mrpt::slam::PF_implementation<
CRBPFParticleData, CMultiMetricMapPDFParticles>
{
public:
using ParticleData = CMultiMetricMapPDFParticles;
@@ -283,6 +282,13 @@ class CMultiMetricMapPDF
const mrpt::poses::CPose3D& x) const override;
/** @} */
void executeOn(
mrpt::bayes::CParticleFilter& pf,
const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm);
}; // End of class def.
} // namespace maps
@@ -148,6 +148,13 @@ class CMonteCarloLocalization2D
const mrpt::poses::CPose3D& x) const override;
/** @} */
void executeOn(
mrpt::bayes::CParticleFilter& pf,
const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm);
}; // End of class def.
} // namespace slam
@@ -94,6 +94,13 @@ class CMonteCarloLocalization3D
const mrpt::poses::CPose3D& x) const;
/** @} */
void executeOn(
mrpt::bayes::CParticleFilter& pf,
const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm);
}; // End of class def.
} // namespace slam
@@ -226,7 +226,7 @@ void CMetricMapBuilderRBPF::processActionObservation(
pf.m_options = m_PF_options;
pf.setVerbosityLevel(this->getMinLoggingLevel());
pf.executeOn(mapPDF, &fakeActs, &observations);
mapPDF.executeOn(pf, &fakeActs, &observations, nullptr, m_PF_options.PF_algorithm);
if (isLoggingLevelVisible(LVL_INFO))
{
@@ -63,10 +63,6 @@ void KLF_loadBinFromParticle(
outBin.phi = round(currentParticleValue->phi() / opts.KLD_binSize_PHI);
}
}
} // namespace slam
} // namespace mrpt
#include <mrpt/slam/PF_implementations.h>
/*---------------------------------------------------------------
ctor
@@ -93,7 +89,6 @@ TPose3D CMonteCarloLocalization2D::getLastPose(
return TPose3D(TPose2D(*m_poseParticles.m_particles[i].d));
}
/*---------------------------------------------------------------
PF_SLAM_computeObservationLikelihoodForParticle
---------------------------------------------------------------*/
@@ -221,12 +216,15 @@ void CMonteCarloLocalization2D::resetUniformFreeSpace(
// Generate pose m_particles:
for (size_t i = 0; i < M; i++)
{
int idx = round(getRandomGenerator().drawUniform(0.0, nFreeCells - 1.001));
int idx =
round(getRandomGenerator().drawUniform(0.0, nFreeCells - 1.001));
m_poseParticles.m_particles[i].d->x(
freeCells_x[idx] + getRandomGenerator().drawUniform(-gridRes, gridRes));
freeCells_x[idx] +
getRandomGenerator().drawUniform(-gridRes, gridRes));
m_poseParticles.m_particles[i].d->y(
freeCells_y[idx] + getRandomGenerator().drawUniform(-gridRes, gridRes));
freeCells_y[idx] +
getRandomGenerator().drawUniform(-gridRes, gridRes));
m_poseParticles.m_particles[i].d->phi(
getRandomGenerator().drawUniform(phi_min, phi_max));
m_poseParticles.m_particles[i].log_w = 0;
@@ -235,37 +233,36 @@ void CMonteCarloLocalization2D::resetUniformFreeSpace(
MRPT_END
}
namespace mrpt
{
namespace bayes
void CMonteCarloLocalization2D::executeOn(
mrpt::bayes::CParticleFilter& pf,
const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm)
{
template <>
void CParticleFilter::executeOn<CMonteCarloLocalization2D>(
CMonteCarloLocalization2D& obj, const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation, TParticleFilterStats* stats)
{
switch (m_options.PF_algorithm)
switch (PF_algorithm)
{
case CParticleFilter::pfStandardProposal:
executeOn<CMonteCarloLocalization2D, mrpt::slam::StandardProposal>(
obj, action, observation, stats);
pf.executeOn<
CMonteCarloLocalization2D, mrpt::slam::StandardProposal>(
*this, action, observation, stats);
break;
case CParticleFilter::pfAuxiliaryPFStandard:
executeOn<CMonteCarloLocalization2D, mrpt::slam::AuxiliaryPFStandard>(
obj, action, observation, stats);
pf.executeOn<
CMonteCarloLocalization2D, mrpt::slam::AuxiliaryPFStandard>(
*this, action, observation, stats);
break;
case CParticleFilter::pfAuxiliaryPFOptimal:
executeOn<CMonteCarloLocalization2D, mrpt::slam::AuxiliaryPFOptimal>(
obj, action, observation, stats);
pf.executeOn<
CMonteCarloLocalization2D, mrpt::slam::AuxiliaryPFOptimal>(
*this, action, observation, stats);
break;
default:
{
THROW_EXCEPTION("Invalid particle filter algorithm selection!");
}
break;
}
}
}
}
@@ -253,12 +253,13 @@ void run_test_pf_localization(CPose2D& meanPose, CMatrixDouble33& cov)
// ----------------------------------------
tictac.Tic();
PF.executeOn(
pdf,
pdf.executeOn(
PF,
action.get(), // Action
observations.get(), // Obs.
&PF_stats // Output statistics
);
&PF_stats, // Output statistics
pfOptions.PF_algorithm
);
}
pdf.m_poseParticles.getCovarianceAndMean(cov, meanPose);
Oops, something went wrong.

0 comments on commit ae45712

Please sign in to comment.