Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
This class is used internally in mrpt::slam::CHMTSLAM
Definition at line 512 of file CHMTSLAM.h.
#include <mrpt/hmtslam/CHMTSLAM.h>
Classes | |
struct | TPathBin |
Auxiliary structure. More... | |
Public Member Functions | |
CLSLAM_RBPF_2DLASER (CHMTSLAM *parent) | |
Constructor. More... | |
virtual | ~CLSLAM_RBPF_2DLASER () |
Destructor. More... | |
void | processOneLMH (CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollectionPtr &act, const mrpt::obs::CSensoryFramePtr &sf) |
Main entry point from HMT-SLAM: process some actions & observations. More... | |
void | prediction_and_update_pfAuxiliaryPFOptimal (CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. More... | |
void | prediction_and_update_pfOptimalProposal (CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. More... | |
Protected Member Functions | |
void | loadTPathBinFromPath (TPathBin &outBin, TMapPoseID2Pose3D *path=NULL, mrpt::poses::CPose2D *newPose=NULL) |
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options". More... | |
int | findTPathBinIntoSet (TPathBin &desiredBin, std::deque< TPathBin > &theSet) |
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found. More... | |
Static Protected Member Functions | |
static double | particlesEvaluator_AuxPFOptimal (const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation) |
Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal". More... | |
static double | auxiliarComputeObservationLikelihood (const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t particleIndexForMap, const mrpt::obs::CSensoryFrame *observation, const mrpt::poses::CPose2D *x) |
Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options". More... | |
Protected Attributes | |
bool | m_insertNewRobotPose |
For use within PF callback methods. More... | |
mrpt::utils::safe_ptr< CHMTSLAM > | m_parent |
Friends | |
class | CLocalMetricHypothesis |
CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER | ( | CHMTSLAM * | parent | ) |
Constructor.
Definition at line 40 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
|
virtual |
Destructor.
Definition at line 44 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
|
staticprotected |
Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".
Definition at line 620 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
References mrpt::maps::CMetricMap::computeObservationsLikelihood(), mrpt::bayes::CParticleFilterData< T >::m_particles, and MRPT_UNUSED_PARAM.
Referenced by particlesEvaluator_AuxPFOptimal(), prediction_and_update_pfAuxiliaryPFOptimal(), and prediction_and_update_pfOptimalProposal().
|
protected |
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found.
Definition at line 704 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
References mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::phi, mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::x, and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::y.
|
protected |
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options".
Definition at line 658 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
References mrpt::hmtslam::CLSLAMAlgorithmBase::m_parent, mrpt::poses::CPose2D::phi(), mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::phi, mrpt::utils::round(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::x, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::y.
|
staticprotected |
Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".
Definition at line 530 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
References ASSERT_, auxiliarComputeObservationLikelihood(), mrpt::math::maximum(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, MRPT_UNUSED_PARAM, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples.
Referenced by prediction_and_update_pfAuxiliaryPFOptimal().
|
virtual |
The PF algorithm implementation.
The PF algorithm implementation for "optimal sampling for non-parametric observation models".
Implements mrpt::hmtslam::CLSLAMAlgorithmBase.
Definition at line 213 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, ASSERT_, auxiliarComputeObservationLikelihood(), mrpt::poses::CPose2D::composeFrom(), mrpt::obs::CActionRobotMovement2D::computeFromOdometry(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::ESS(), mrpt::bayes::CParticleFilterCapable::fastDrawSample(), mrpt::obs::CActionRobotMovement2D::fastDrawSingleSample(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::gaussianModel, mrpt::obs::CActionCollection::getBestMovementEstimation(), mrpt::hmtslam::CLocalMetricHypothesis::getCurrentPose(), mrpt::utils::keep_max(), mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovement, mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovementIsValid, mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w_metric_history, mrpt::hmtslam::CLocalMetricHypothesis::m_maxLikelihood, mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawMaximumLikelihood, mrpt::hmtslam::CLocalMetricHypothesis::m_movementDraws, mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawsIdx, mrpt::hmtslam::CLocalMetricHypothesis::m_parent, mrpt::bayes::CParticleFilterData< T >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_pfAuxiliaryPFOptimal_estimatedProb, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, mrpt::math::maximum(), mrpt::math::mean(), min, mrpt::math::minimum(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdPHI, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdXY, mrpt::obs::CActionRobotMovement2D::motionModelConfiguration, MRPT_END, MRPT_START, mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::normalizeWeights(), particlesEvaluator_AuxPFOptimal(), mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples, mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor, mrpt::bayes::CParticleFilterCapable::prepareFastDrawSample(), mrpt::obs::CActionRobotMovement2D::prepareFastDrawSingleSamples(), mrpt::random::randomGenerator, mrpt::obs::CActionRobotMovement2D::rawOdometryIncrementReading, mrpt::utils::CTicTac::Tac(), THROW_EXCEPTION, and mrpt::utils::CTicTac::Tic().
|
virtual |
The PF algorithm implementation.
The PF algorithm implementation for "optimal sampling" approximated with scan matching (Stachniss method)
Implements mrpt::hmtslam::CLSLAMAlgorithmBase.
Definition at line 727 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, mrpt::slam::CICP::TConfigParams::ALFA, mrpt::slam::CMetricMapsAlignmentAlgorithm::Align(), ASSERT_, auxiliarComputeObservationLikelihood(), mrpt::maps::CMetricMap::clear(), mrpt::poses::CPose3D::composeFrom(), mrpt::poses::CPosePDFGaussian::copyFrom(), mrpt::bayes::CProbabilityParticle< T >::d, DEG2RAD, mrpt::slam::CICP::TConfigParams::doRANSAC, mrpt::poses::CPose3DPDFGaussian::drawSingleSample(), mrpt::obs::CActionCollection::getBestMovementEstimation(), mrpt::hmtslam::CLocalMetricHypothesis::getCurrentPose(), mrpt::maps::CPointsMap::insertionOptions, mrpt::obs::CSensoryFrame::insertObservationsInto(), mrpt::bayes::CProbabilityParticle< T >::log_w, mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovement, mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovementIsValid, mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w_metric_history, mrpt::bayes::CParticleFilterData< T >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, mrpt::slam::CICP::TConfigParams::maxIterations, mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints, mrpt::obs::CActionRobotMovement2D::motionModelConfiguration, MRPT_END, MRPT_START, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::normalizeWeights(), mrpt::slam::CICP::TConfigParams::onlyClosestCorrespondences, mrpt::slam::CICP::options, mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor, mrpt::obs::CActionRobotMovement2D::rawOdometryIncrementReading, mrpt::slam::CICP::TConfigParams::smallestThresholdDist, mrpt::math::square(), mrpt::slam::CICP::TConfigParams::thresholdAng, mrpt::slam::CICP::TConfigParams::thresholdDist, THROW_EXCEPTION, and mrpt::poses::CPose3D::yaw().
|
virtual |
Main entry point from HMT-SLAM: process some actions & observations.
The passed action/observation will be deleted, so a copy must be made if necessary. This method must be in charge of updating the robot pose estimates and also to update the map when required.
LMH | The local metric hypothesis which must be updated by this SLAM algorithm. |
act | The action to process (or NULL). |
sf | The observations to process (or NULL). |
Implements mrpt::hmtslam::CLSLAMAlgorithmBase.
Definition at line 62 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.
References ASSERT_, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::distanceTo(), mrpt::bayes::CParticleFilter::executeOn(), mrpt::hmtslam::CHMTSLAM::generatePoseID(), mrpt::hmtslam::CLocalMetricHypothesis::getMeans(), mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, m_insertNewRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_nodeIDmemberships, mrpt::bayes::CParticleFilter::m_options, mrpt::hmtslam::CLSLAMAlgorithmBase::m_parent, mrpt::bayes::CParticleFilterData< T >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_posesPendingAddPartitioner, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, min, MRPT_END, MRPT_START, NODE_ANNOTATION_REF_POSEID, POSEID_INVALID, mrpt::math::wrapToPi(), and mrpt::poses::CPose3D::yaw().
|
friend |
Definition at line 514 of file CHMTSLAM.h.
|
protected |
For use within PF callback methods.
Definition at line 554 of file CHMTSLAM.h.
Referenced by processOneLMH().
|
protectedinherited |
Definition at line 465 of file CHMTSLAM.h.
Referenced by loadTPathBinFromPath(), and processOneLMH().
Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019 |