Program Listing for File CMonteCarloLocalization2D.h

↰ Return to documentation for file (include/mrpt/slam/CMonteCarloLocalization2D.h)

/*                    _
                     | |    Mobile Robot Programming Toolkit (MRPT)
 _ __ ___  _ __ _ __ | |_
| '_ ` _ \| '__| '_ \| __|          https://www.mrpt.org/
| | | | | | |  | |_) | |_
|_| |_| |_|_|  | .__/ \__|     https://github.com/MRPT/mrpt/
               | |
               |_|

 Copyright (c) 2005-2026, Individual contributors, see AUTHORS file
 See: https://www.mrpt.org/Authors - All rights reserved.
 SPDX-License-Identifier: BSD-3-Clause
*/
#pragma once

#include <mrpt/obs/obs_frwds.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/slam/PF_implementations_data.h>
#include <mrpt/slam/TMonteCarloLocalizationParams.h>
#include <mrpt/viz/CSetOfObjects.h>

namespace mrpt
{
namespace maps
{
class COccupancyGridMap2D;
}

namespace slam
{
class CMonteCarloLocalization2D :
    public mrpt::poses::CPosePDFParticles,
    public PF_implementation<
        mrpt::math::TPose2D,
        CMonteCarloLocalization2D,
        mrpt::poses::CPosePDFParticles::PARTICLE_STORAGE>
{
 public:
  TMonteCarloLocalizationParams options;

  CMonteCarloLocalization2D(size_t M = 1);

  ~CMonteCarloLocalization2D() override;

  void resetUniformFreeSpace(
      mrpt::maps::COccupancyGridMap2D* theMap,
      const double freeCellsThreshold = 0.7,
      const int particlesCount = -1,
      const double x_min = -1e10f,
      const double x_max = 1e10f,
      const double y_min = -1e10f,
      const double y_max = 1e10f,
      const double phi_min = -M_PI,
      const double phi_max = M_PI);

  void prediction_and_update_pfStandardProposal(
      const mrpt::obs::CActionCollection* action,
      const mrpt::obs::CSensoryFrame* observation,
      const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override;

  void prediction_and_update_pfAuxiliaryPFStandard(
      const mrpt::obs::CActionCollection* action,
      const mrpt::obs::CSensoryFrame* observation,
      const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override;

  void prediction_and_update_pfAuxiliaryPFOptimal(
      const mrpt::obs::CActionCollection* action,
      const mrpt::obs::CSensoryFrame* observation,
      const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override;

  mrpt::viz::CSetOfObjects::Ptr getVisualization() const
  {
    return mrpt::viz::CSetOfObjects::posePDF2opengl(*this);
  }

  // protected:
  mrpt::math::TPose3D getLastPose(size_t i, bool& is_valid_pose) const override;

  void PF_SLAM_implementation_custom_update_particle_with_new_pose(
      mrpt::math::TPose2D* particleData, const mrpt::math::TPose3D& newPose) const override;

  // We'll redefine this one:
  void PF_SLAM_implementation_replaceByNewParticleSet(
      CParticleList& old_particles,
      const std::vector<mrpt::math::TPose3D>& newParticles,
      const std::vector<double>& newParticlesWeight,
      const std::vector<size_t>& newParticlesDerivedFromIdx) const override;

  double PF_SLAM_computeObservationLikelihoodForParticle(
      const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
      size_t particleIndexForMap,
      const mrpt::obs::CSensoryFrame& observation,
      const mrpt::poses::CPose3D& x) const override;

};  // End of class def.

}  // namespace slam
}  // namespace mrpt