void CMonteCarloLocalization2D::PF_SLAM_implementation_replaceByNewParticleSet(
CParticleList& old_particles, const vector<TPose3D>& newParticles,
const vector<double>& newParticlesWeight,
const vector<size_t>& newParticlesDerivedFromIdx) const
{
MRPT_UNUSED_PARAM(newParticlesDerivedFromIdx);
ASSERT_EQUAL_(
size_t(newParticlesWeight.size()), size_t(newParticles.size()));
// ---------------------------------------------------------------------------------
// Substitute old by new particle set:
// Old are in "m_particles"
// New are in "newParticles",
// "newParticlesWeight","newParticlesDerivedFromIdx"
// ---------------------------------------------------------------------------------
// Free old m_particles: not needed since "d" is now a smart ptr
// Copy into "m_particles"
const size_t N = newParticles.size();
old_particles.resize(N);
for (size_t i = 0; i < N; i++)
{
old_particles[i].log_w = newParticlesWeight[i];
old_particles[i].d.reset(new CPose2D(TPose2D(newParticles[i])));
}
}
void CMonteCarloLocalization3D::PF_SLAM_implementation_replaceByNewParticleSet(
CParticleList &old_particles,
const vector<TPose3D> &newParticles,
const vector<double> &newParticlesWeight,
const vector<size_t> &newParticlesDerivedFromIdx ) const
{
MRPT_UNUSED_PARAM(newParticlesDerivedFromIdx);
ASSERT_(size_t(newParticlesWeight.size())==newParticles.size())
// ---------------------------------------------------------------------------------
// Substitute old by new particle set:
// Old are in "m_particles"
// New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
// ---------------------------------------------------------------------------------
// Free old m_particles:
for (size_t i=0;i<old_particles.size();i++)
mrpt::utils::delete_safe( old_particles[ i ].d );
// Copy into "m_particles"
const size_t N = newParticles.size();
old_particles.resize(N);
for (size_t i=0;i<N;i++)
{
old_particles[i].log_w = newParticlesWeight[i];
old_particles[i].d = new CPose3D( newParticles[i] );
}
}