Skip to content

Commit

Permalink
Move where executeOn is implemented
Browse files Browse the repository at this point in the history 
The template instantiation was overly complicated.
  • Loading branch information
@jolting @jlblancoc
jolting authored and jlblancoc committed Nov 22, 2017
1 parent 7c68726 commit ae45712
Show file tree
Hide file tree
Showing 15 changed files with 157 additions and 148 deletions.
7 changes: 4 additions & 3 deletions apps/pf-localization/pf_localization_main.cpp
View file
Expand Up @@ -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)
Expand Down
22 changes: 10 additions & 12 deletions apps/ro-localization/CPosePDFParticlesExtended.cpp
View file
Expand Up @@ -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:
{
Expand Down
7 changes: 7 additions & 0 deletions apps/ro-localization/CPosePDFParticlesExtended.h
View file
Expand Up @@ -21,6 +21,7 @@

namespace mrpt
{

namespace poses
{
class TExtendedCPose2D
Expand Down Expand Up @@ -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);

Expand Down
7 changes: 4 additions & 3 deletions apps/ro-localization/ro-localization_main.cpp
View file
Expand Up @@ -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(),
Expand Down
6 changes: 0 additions & 6 deletions libs/base/include/mrpt/bayes/CParticleFilter.h
View file
Expand Up @@ -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.
*/
Expand Down
11 changes: 11 additions & 0 deletions libs/hmtslam/include/mrpt/hmtslam/CLocalMetricHypothesis.h
View file
Expand Up @@ -21,6 +21,10 @@

namespace mrpt
{
namespace bayes
{
class CParticleFilter;
}
namespace poses
{
class CPose3DPDFParticles;
Expand Down Expand Up @@ -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:
/** @}
*/
Expand Down
28 changes: 12 additions & 16 deletions libs/hmtslam/src/CHMTSLAM_LSLAM_RBPF_2DLASER.cpp
View file
Expand Up @@ -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:
{
Expand Down Expand Up @@ -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
// ...
Expand Down
14 changes: 10 additions & 4 deletions libs/slam/include/mrpt/maps/CMultiMetricMapPDF.h
View file
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down
7 changes: 7 additions & 0 deletions libs/slam/include/mrpt/slam/CMonteCarloLocalization2D.h
View file
Expand Up @@ -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
Expand Down
7 changes: 7 additions & 0 deletions libs/slam/include/mrpt/slam/CMonteCarloLocalization3D.h
View file
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion libs/slam/src/slam/CMetricMapBuilderRBPF.cpp
View file
Expand Up @@ -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))
{
Expand Down
47 changes: 22 additions & 25 deletions libs/slam/src/slam/CMonteCarloLocalization2D.cpp
View file
Expand Up @@ -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
Expand All @@ -93,7 +89,6 @@ TPose3D CMonteCarloLocalization2D::getLastPose(
return TPose3D(TPose2D(*m_poseParticles.m_particles[i].d));
}


/*---------------------------------------------------------------
PF_SLAM_computeObservationLikelihoodForParticle
---------------------------------------------------------------*/
Expand Down Expand Up @@ -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;
Expand All @@ -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;
}

}

}
}
9 changes: 5 additions & 4 deletions libs/slam/src/slam/CMonteCarloLocalization2D_unittest.cpp
View file
Expand Up @@ -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);
Expand Down

0 comments on commit ae45712

Please sign in to comment.