This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
Internally, the list of particles, each containing a hypothesis for the robot path plus its associated metric map, is stored in an object of class CMultiMetricMapPDF.
This class processes robot actions and observations sequentially (through the method CMetricMapBuilderRBPF::processActionObservation) and exploits the generic design of metric map classes in MRPT to deal with any number and combination of maps simultaneously: the likelihood of observations is the product of the likelihood in the different maps, etc.
A number of particle filter methods are implemented as well, by selecting the appropriate values in TConstructionOptions::PF_options. Not all the PF algorithms are implemented for all kinds of maps.
For an example of usage, check the application "rbpf-slam", in "apps/RBPF-SLAM". See also the wiki page.
Definition at line 57 of file CMetricMapBuilderRBPF.h.
#include <mrpt/slam/CMetricMapBuilderRBPF.h>
Classes | |
struct | TConstructionOptions |
Options for building a CMetricMapBuilderRBPF object, passed to the constructor. More... | |
struct | TStats |
This structure will hold stats after each execution of processActionObservation. More... | |
Public Member Functions | |
CMetricMapBuilderRBPF (const TConstructionOptions &initializationOptions) | |
Constructor. More... | |
CMetricMapBuilderRBPF () | |
This second constructor is created for the situation where a class member needs to be of type CMetricMapBuilderRBPF. More... | |
CMetricMapBuilderRBPF & | operator= (const CMetricMapBuilderRBPF &src) |
Copy Operator. More... | |
virtual | ~CMetricMapBuilderRBPF () |
Destructor. More... | |
void | initialize (const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) |
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left unmodified) and a given fixed, past map. More... | |
void | clear () |
Clear all elements of the maps. More... | |
mrpt::poses::CPose3DPDF::Ptr | getCurrentPoseEstimation () const |
Returns a copy of the current best pose estimation as a pose PDF. More... | |
void | getCurrentMostLikelyPath (std::deque< mrpt::math::TPose3D > &outPath) const |
Returns the current most-likely path estimation (the path associated to the most likely particle). More... | |
void | processActionObservation (mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &observations) |
Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description. More... | |
void | getCurrentlyBuiltMap (mrpt::maps::CSimpleMap &out_map) const |
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map. More... | |
const mrpt::maps::CMultiMetricMap * | getCurrentlyBuiltMetricMap () const |
Returns the map built so far. More... | |
unsigned int | getCurrentlyBuiltMapSize () |
Returns just how many sensory-frames are stored in the currently build map. More... | |
void | saveCurrentEstimationToImage (const std::string &file, bool formatEMF_BMP=true) |
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file. More... | |
void | drawCurrentEstimationToImage (utils::CCanvas *img) |
A useful method for debugging: draws the current map and path hypotheses to a CCanvas. More... | |
void | saveCurrentPathEstimationToTextFile (const std::string &fil) |
A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively). More... | |
double | getCurrentJointEntropy () |
void | enableMapUpdating (bool enable) |
Enables or disables the map updating (default state is enabled) More... | |
void | loadCurrentMapFromFile (const std::string &fileName) |
Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file. More... | |
void | saveCurrentMapToFile (const std::string &fileName, bool compressGZ=true) const |
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file. More... | |
Public Attributes | |
mrpt::maps::CMultiMetricMapPDF | mapPDF |
The map PDF: It includes a path and associated map for each particle. More... | |
TStats | m_statsLastIteration |
This structure will hold stats after each execution of processActionObservation. More... | |
TOptions | options |
Protected Member Functions | |
void | enterCriticalSection () |
Enter critical section for map updating. More... | |
void | leaveCriticalSection () |
Leave critical section for map updating. More... | |
Protected Attributes | |
bayes::CParticleFilter::TParticleFilterOptions | m_PF_options |
The configuration of the particle filter. More... | |
float | insertionLinDistance |
Distances (linear and angular) for inserting a new observation into the map. More... | |
float | insertionAngDistance |
float | localizeLinDistance |
Distances (linear and angular) for updating the robot pose estimate (and particles weighs, if applicable). More... | |
float | localizeAngDistance |
mrpt::poses::CPose3DPDFGaussian | odoIncrementSinceLastLocalization |
Traveled distance since last localization update. More... | |
mrpt::poses::CPose3D | odoIncrementSinceLastMapUpdate |
Traveled distance since last map update. More... | |
std::mutex | critZoneChangingMap |
Critical zones. More... | |
CMetricMapBuilderRBPF::CMetricMapBuilderRBPF | ( | const TConstructionOptions & | initializationOptions | ) |
Constructor.
Definition at line 35 of file CMetricMapBuilderRBPF.cpp.
References clear(), and mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::verbosity_level.
CMetricMapBuilderRBPF::CMetricMapBuilderRBPF | ( | ) |
This second constructor is created for the situation where a class member needs to be of type CMetricMapBuilderRBPF.
Definition at line 55 of file CMetricMapBuilderRBPF.cpp.
References MRPT_LOG_WARN.
|
virtual |
Destructor.
Definition at line 86 of file CMetricMapBuilderRBPF.cpp.
void CMetricMapBuilderRBPF::clear | ( | ) |
Clear all elements of the maps.
Definition at line 90 of file CMetricMapBuilderRBPF.cpp.
References mrpt::maps::CMultiMetricMapPDF::clear(), mrpt::slam::CMetricMapBuilder::critZoneChangingMap, mapPDF, MRPT_LOG_DEBUG, odoIncrementSinceLastLocalization, odoIncrementSinceLastMapUpdate, and mrpt::poses::CPose3D::setFromValues().
Referenced by CMetricMapBuilderRBPF(), and initialize().
void CMetricMapBuilderRBPF::drawCurrentEstimationToImage | ( | utils::CCanvas * | img | ) |
A useful method for debugging: draws the current map and path hypotheses to a CCanvas.
Definition at line 386 of file CMetricMapBuilderRBPF.cpp.
References ASSERT_, mrpt::utils::CCanvas::drawImage(), mrpt::maps::CMultiMetricMapPDF::getCurrentMostLikelyMetricMap(), mrpt::utils::CImage::getHeight(), mrpt::maps::CMultiMetricMapPDF::getPath(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::getW(), mrpt::maps::CMultiMetricMapPDF::m_poseParticles, mapPDF, MRPT_END, MRPT_START, mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::particlesCount(), mrpt::utils::CImage::resize(), and mrpt::utils::round().
Referenced by saveCurrentEstimationToImage().
|
inlineinherited |
Enables or disables the map updating (default state is enabled)
Definition at line 106 of file CMetricMapBuilder.h.
References mrpt::slam::CMetricMapBuilder::TOptions::enableMapUpdating, and mrpt::slam::CMetricMapBuilder::options.
|
inlineprotectedinherited |
Enter critical section for map updating.
Definition at line 41 of file CMetricMapBuilder.h.
References mrpt::slam::CMetricMapBuilder::critZoneChangingMap.
Referenced by mrpt::slam::CMetricMapBuilderICP::getCurrentMapPoints(), and mrpt::slam::CMetricMapBuilderICP::~CMetricMapBuilderICP().
double CMetricMapBuilderRBPF::getCurrentJointEntropy | ( | ) |
Definition at line 512 of file CMetricMapBuilderRBPF.cpp.
References mrpt::maps::CMultiMetricMapPDF::getCurrentJointEntropy(), and mapPDF.
|
virtual |
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
Implements mrpt::slam::CMetricMapBuilder.
Definition at line 363 of file CMetricMapBuilderRBPF.cpp.
References mapPDF, mrpt::maps::CMultiMetricMapPDF::SFs, and mrpt::maps::CMultiMetricMapPDF::updateSensoryFrameSequence().
|
virtual |
Returns just how many sensory-frames are stored in the currently build map.
Implements mrpt::slam::CMetricMapBuilder.
Definition at line 378 of file CMetricMapBuilderRBPF.cpp.
References mapPDF, mrpt::maps::CMultiMetricMapPDF::SFs, and mrpt::maps::CSimpleMap::size().
|
virtual |
Returns the map built so far.
NOTE that for efficiency a pointer to the internal object is passed, DO NOT delete nor modify the object in any way, if desired, make a copy of ir with "clone()".
Implements mrpt::slam::CMetricMapBuilder.
Definition at line 370 of file CMetricMapBuilderRBPF.cpp.
References mrpt::maps::CMultiMetricMapPDF::getCurrentMostLikelyMetricMap(), and mapPDF.
void CMetricMapBuilderRBPF::getCurrentMostLikelyPath | ( | std::deque< mrpt::math::TPose3D > & | outPath | ) | const |
Returns the current most-likely path estimation (the path associated to the most likely particle).
Definition at line 342 of file CMetricMapBuilderRBPF.cpp.
References mrpt::maps::CMultiMetricMapPDF::getPath(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::getW(), mrpt::maps::CMultiMetricMapPDF::m_poseParticles, mapPDF, and mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::particlesCount().
|
virtual |
Returns a copy of the current best pose estimation as a pose PDF.
Implements mrpt::slam::CMetricMapBuilder.
Definition at line 324 of file CMetricMapBuilderRBPF.cpp.
References mrpt::maps::CMultiMetricMapPDF::getEstimatedPosePDF(), mapPDF, mrpt::poses::CPose3DPDFGaussian::mean, and odoIncrementSinceLastLocalization.
|
virtual |
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left unmodified) and a given fixed, past map.
Implements mrpt::slam::CMetricMapBuilder.
Definition at line 294 of file CMetricMapBuilderRBPF.cpp.
References clear(), mrpt::maps::CMultiMetricMapPDF::clear(), mrpt::slam::CMetricMapBuilder::critZoneChangingMap, mrpt::maps::CSimpleMap::empty(), mrpt::utils::CProbabilityDensityFunction< TDATA, STATE_LEN >::getMeanVal(), mapPDF, MRPT_LOG_INFO_STREAM, mrpt::maps::CSimpleMap::rbegin(), and mrpt::maps::CSimpleMap::size().
|
inlineprotectedinherited |
Leave critical section for map updating.
Definition at line 43 of file CMetricMapBuilder.h.
References mrpt::slam::CMetricMapBuilder::critZoneChangingMap.
Referenced by mrpt::slam::CMetricMapBuilderICP::getCurrentMapPoints(), and mrpt::slam::CMetricMapBuilderICP::~CMetricMapBuilderICP().
|
inherited |
Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file.
Definition at line 54 of file CMetricMapBuilder.cpp.
References mrpt::maps::CSimpleMap::clear(), mrpt::system::fileExists(), mrpt::slam::CMetricMapBuilder::initialize(), MRPT_LOG_INFO_STREAM, and MRPT_LOG_WARN_STREAM.
Referenced by mrpt::slam::CMetricMapBuilderICP::setCurrentMapFile().
CMetricMapBuilderRBPF & CMetricMapBuilderRBPF::operator= | ( | const CMetricMapBuilderRBPF & | src | ) |
Copy Operator.
Definition at line 64 of file CMetricMapBuilderRBPF.cpp.
References insertionAngDistance, insertionLinDistance, localizeAngDistance, localizeLinDistance, m_PF_options, m_statsLastIteration, mapPDF, odoIncrementSinceLastLocalization, and odoIncrementSinceLastMapUpdate.
|
virtual |
Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.
action | The incremental 2D pose change in the robot pose. This value is deterministic. |
observations | The set of observations that robot senses at the new pose. Statistics will be saved to statsLastIteration |
Implements mrpt::slam::CMetricMapBuilder.
Definition at line 110 of file CMetricMapBuilderRBPF.cpp.
References mrpt::slam::CMetricMapBuilder::TOptions::alwaysInsertByClass, ASSERT_, mrpt::utils::CListOfClasses::begin(), mrpt::obs::CSensoryFrame::begin(), mrpt::obs::CActionRobotMovement2D::computeFromOdometry(), mrpt::poses::CPose3DPDFGaussian::cov, mrpt::math::cov(), mrpt::slam::CMetricMapBuilder::critZoneChangingMap, mrpt::slam::CMetricMapBuilder::TOptions::debugForceInsertion, mrpt::utils::CListOfClasses::end(), mrpt::obs::CSensoryFrame::end(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::ESS(), mrpt::obs::CActionRobotMovement3D::estimationMethod, mrpt::maps::CMultiMetricMapPDF::executeOn(), mrpt::format(), mrpt::mrpt::format(), mrpt::obs::CActionCollection::getActionByClass(), mrpt::poses::CPose3DPDFParticles::getCovarianceAndMean(), mrpt::maps::CMultiMetricMapPDF::getEstimatedPosePDF(), mrpt::poses::CPose3DPDFParticles::getMean(), mrpt::obs::CActionCollection::insert(), insertionAngDistance, insertionLinDistance, mrpt::maps::CMultiMetricMapPDF::insertObservation(), localizeAngDistance, localizeLinDistance, mrpt::bayes::CParticleFilter::m_options, mrpt::bayes::CParticleFilterData< T >::m_particles, m_PF_options, mrpt::maps::CMultiMetricMapPDF::m_poseParticles, m_statsLastIteration, mapPDF, mrpt::poses::CPose3DPDFGaussian::mean, MRPT_END, MRPT_LOG_DEBUG, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_INFO, MRPT_LOG_INFO_STREAM, MRPT_LOG_WARN, MRPT_LOG_WARN_STREAM, MRPT_START, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::slam::CMetricMapBuilderRBPF::TStats::observationsInserted, odoIncrementSinceLastLocalization, odoIncrementSinceLastMapUpdate, mrpt::slam::CMetricMapBuilder::options, mrpt::bayes::CParticleFilter::TParticleFilterOptions::PF_algorithm, mrpt::obs::CActionRobotMovement3D::poseChange, RAD2DEG, mrpt::poses::CPose3D::setFromValues(), mrpt::maps::CMultiMetricMapPDF::SFs, mrpt::maps::CSimpleMap::size(), mrpt::obs::CSensoryFrame::size(), mrpt::obs::CAction::timestamp, and mrpt::poses::CPose3D::yaw().
|
virtual |
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
file | The output file name |
formatEMF_BMP | Output format = true:EMF, false:BMP |
Implements mrpt::slam::CMetricMapBuilder.
Definition at line 487 of file CMetricMapBuilderRBPF.cpp.
References CH_GRAY, drawCurrentEstimationToImage(), MRPT_END, and MRPT_START.
|
inherited |
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
Definition at line 86 of file CMetricMapBuilder.cpp.
References CFileGZOutputStream, mrpt::slam::CMetricMapBuilder::getCurrentlyBuiltMap(), and MRPT_LOG_INFO_STREAM.
Referenced by mrpt::slam::CMetricMapBuilderICP::setCurrentMapFile().
void CMetricMapBuilderRBPF::saveCurrentPathEstimationToTextFile | ( | const std::string & | fil | ) |
A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively).
Definition at line 520 of file CMetricMapBuilderRBPF.cpp.
References mapPDF, and mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile().
|
protectedinherited |
Critical zones.
Definition at line 39 of file CMetricMapBuilder.h.
Referenced by clear(), mrpt::slam::CMetricMapBuilder::enterCriticalSection(), mrpt::slam::CMetricMapBuilderICP::initialize(), initialize(), mrpt::slam::CMetricMapBuilder::leaveCriticalSection(), processActionObservation(), and mrpt::slam::CMetricMapBuilderICP::processObservation().
|
protected |
Definition at line 69 of file CMetricMapBuilderRBPF.h.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::dumpToTextStream(), mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::loadFromConfigFile(), operator=(), and processActionObservation().
|
protected |
Distances (linear and angular) for inserting a new observation into the map.
Definition at line 69 of file CMetricMapBuilderRBPF.h.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::dumpToTextStream(), mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::loadFromConfigFile(), operator=(), and processActionObservation().
|
protected |
Definition at line 73 of file CMetricMapBuilderRBPF.h.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::dumpToTextStream(), mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::loadFromConfigFile(), operator=(), and processActionObservation().
|
protected |
Distances (linear and angular) for updating the robot pose estimate (and particles weighs, if applicable).
Definition at line 73 of file CMetricMapBuilderRBPF.h.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::dumpToTextStream(), mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions::loadFromConfigFile(), operator=(), and processActionObservation().
|
protected |
The configuration of the particle filter.
Definition at line 65 of file CMetricMapBuilderRBPF.h.
Referenced by operator=(), and processActionObservation().
TStats mrpt::slam::CMetricMapBuilderRBPF::m_statsLastIteration |
This structure will hold stats after each execution of processActionObservation.
Definition at line 204 of file CMetricMapBuilderRBPF.h.
Referenced by operator=(), and processActionObservation().
mrpt::maps::CMultiMetricMapPDF mrpt::slam::CMetricMapBuilderRBPF::mapPDF |
The map PDF: It includes a path and associated map for each particle.
Definition at line 61 of file CMetricMapBuilderRBPF.h.
Referenced by clear(), drawCurrentEstimationToImage(), getCurrentJointEntropy(), getCurrentlyBuiltMap(), getCurrentlyBuiltMapSize(), getCurrentlyBuiltMetricMap(), getCurrentMostLikelyPath(), getCurrentPoseEstimation(), initialize(), operator=(), processActionObservation(), and saveCurrentPathEstimationToTextFile().
|
protected |
Traveled distance since last localization update.
Definition at line 76 of file CMetricMapBuilderRBPF.h.
Referenced by clear(), getCurrentPoseEstimation(), operator=(), and processActionObservation().
|
protected |
Traveled distance since last map update.
Definition at line 78 of file CMetricMapBuilderRBPF.h.
Referenced by clear(), operator=(), and processActionObservation().
|
inherited |
Definition at line 143 of file CMetricMapBuilder.h.
Referenced by mrpt::slam::CMetricMapBuilder::enableMapUpdating(), processActionObservation(), and mrpt::slam::CMetricMapBuilderICP::processObservation().
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019 |