38 initializationOptions.PF_options,
39 &initializationOptions.mapsInitializers,
40 &initializationOptions.predictionOptions),
41 m_PF_options(initializationOptions.PF_options),
42 insertionLinDistance(initializationOptions.insertionLinDistance),
43 insertionAngDistance(initializationOptions.insertionAngDistance),
44 localizeLinDistance(initializationOptions.localizeLinDistance),
45 localizeAngDistance(initializationOptions.localizeAngDistance),
46 odoIncrementSinceLastLocalization(),
47 odoIncrementSinceLastMapUpdate()
49 setLoggerName(
"CMetricMapBuilderRBPF");
57 this->setLoggerName(
"CMetricMapBuilderRBPF");
92 std::lock_guard<std::mutex> csl(
96 static CPose2D nullPose(0, 0, 0);
114 std::lock_guard<std::mutex> csl(
126 "processActionObservation(): Input action is " 127 "CActionRobotMovement3D=" 128 << act3D->poseChange.getMeanVal().asString());
135 "processActionObservation(): Input action is " 136 "CActionRobotMovement2D=" 137 << act2D->poseChange->getMeanVal().asString());
153 bool do_localization =
171 it != observations.
end(); ++it)
172 if ((*it)->GetRuntimeClass() == *itCl)
174 do_map_update =
true;
175 do_localization =
true;
179 if (do_map_update) do_localization =
true;
183 "do_map_update=%s do_localization=%s", do_map_update ?
"YES" :
"NO",
184 do_localization ?
"YES" :
"NO"));
212 act2D->motionModelConfiguration);
213 newAct.timestamp = act2D->timestamp;
219 "odoIncrementSinceLastLocalization before resetting = " 227 pf.setVerbosityLevel(this->getMinLoggingLevel());
231 if (isLoggingLevelVisible(LVL_INFO))
237 poseEstimation.
getMean(meanPose);
244 "New pose=" << estPos << std::endl
248 " STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
249 sqrt(
cov(0, 0)), sqrt(
cov(1, 1)), sqrt(
cov(2, 2)),
266 "**No map was updated** after inserting a CSensoryFrame with " 267 << observations.
size());
282 it->d->mapTillNow.auxParticleFilterCleanUp();
288 "Split initialize() in two, one generic without pose, one with " 289 "particles-based PDF");
297 std::lock_guard<std::mutex> csl(
301 "[initialize] Called with " << initialMap.
size()
302 <<
" nodes in fixed map");
310 else if (!initialMap.
empty())
313 curPose = initialMap.
rbegin()->first->getMeanVal();
327 mrpt::make_aligned_shared<CPose3DPDFParticles>();
332 for (
auto&
p : posePDF->m_particles)
343 std::deque<TPose3D>& outPath)
const 391 std::deque<TPose3D> path;
392 unsigned int imgHeight = 0;
399 ASSERT_(currentMetricMapEstimation->m_gridMaps.size() > 0);
402 unsigned int bestPath = 0;
403 double bestPathLik = -1;
404 for (i = 0; i < M; i++)
417 bool alreadyCopiedImage =
false;
422 currentMetricMapEstimation->m_gridMaps[0]->getSizeX(),
423 currentMetricMapEstimation->m_gridMaps[0]->getSizeY(), 1,
true);
425 if (!alreadyCopiedImage)
431 currentMetricMapEstimation->m_gridMaps[0]->getAsImage(imgGrid);
437 int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
438 float x_min = currentMetricMapEstimation->m_gridMaps[0]->getXMin();
439 float y_min = currentMetricMapEstimation->m_gridMaps[0]->getYMin();
441 currentMetricMapEstimation->m_gridMaps[0]->getResolution();
446 for (i = 0; i <= M; i++)
448 if (i != bestPath || i == M)
452 size_t nPoses = path.size();
455 x2 =
round((path[0].
x - x_min) / resolution);
456 y2 =
round((path[0].
y - y_min) / resolution);
459 for (
size_t j = 0; j < nPoses; j++)
466 x2 =
round((path[j].
x - x_min) / resolution);
467 y2 =
round((path[j].
y - y_min) / resolution);
471 x1,
round((imgHeight - 1) - y1), x2,
472 round((imgHeight - 1) - y2),
474 :
TColor(0x50, 0x50, 0x50),
503 img.saveToFile(file);
530 : insertionLinDistance(1.0f),
531 insertionAngDistance(
DEG2RAD(30)),
532 localizeLinDistance(0.4f),
533 localizeAngDistance(
DEG2RAD(10)),
537 verbosity_level(
mrpt::utils::LVL_INFO)
548 "\n----------- [CMetricMapBuilderRBPF::TConstructionOptions] " 549 "------------ \n\n");
552 "insertionLinDistance = %f m\n",
555 "insertionAngDistance = %f deg\n",
558 "localizeLinDistance = %f m\n",
561 "localizeAngDistance = %f deg\n",
564 "verbosity_level = %s\n",
569 PF_options.dumpToTextStream(out);
571 out.
printf(
" Now showing 'mapsInitializers' and 'predictionOptions':\n");
574 mapsInitializers.dumpToTextStream(out);
575 predictionOptions.dumpToTextStream(out);
586 PF_options.loadFromConfigFile(iniFile, section);
596 verbosity_level = iniFile.
read_enum<mrpt::utils::VerbosityLevel>(
597 section,
"verbosity_level", verbosity_level);
599 mapsInitializers.loadFromConfigFile(iniFile, section);
600 predictionOptions.loadFromConfigFile(iniFile, section);
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
unsigned int getCurrentlyBuiltMapSize()
Returns just how many sensory-frames are stored in the currently build map.
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
bool empty() const
Returns size()!=0.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose3D mean
The mean value.
void getCovarianceAndMean(mrpt::math::CMatrixDouble66 &cov, CPose3D &mean_point) const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
bool observationsInserted
Whether the SF has been inserted in the metric maps.
CMetricMapBuilderRBPF & operator=(const CMetricMapBuilderRBPF &src)
Copy Operator.
const CMultiMetricMap * getCurrentMostLikelyMetricMap() const
Returns a pointer to the current most likely map (associated to the most likely particle) ...
mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate
Traveled distance since last map update.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
float insertionLinDistance
Distances (linear and angular) for inserting a new observation into the map.
A class for storing images as grayscale or RGB bitmaps.
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
void resize(unsigned int width, unsigned int height, TImageChannels nChannels, bool originTopLeft)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
float localizeLinDistance
Distances (linear and angular) for updating the robot pose estimate (and particles weighs...
mrpt::utils::VerbosityLevel verbosity_level
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
void saveCurrentPathEstimationToTextFile(const std::string &fil)
A logging utility: saves the current path estimation for each particle in a text file (a row per part...
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
MRPT_TODO("Split initialize() in two, one generic without pose, one with " "particles-based PDF")
double yaw() const
Get the YAW angle (in radians)
std::shared_ptr< CPose3DPDFParticles > Ptr
const_reverse_iterator rbegin() const
double getCurrentJointEntropy()
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
GLsizei GLsizei GLuint * obj
TSet::const_iterator const_iterator
Declares a class for storing a collection of robot actions.
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
GLubyte GLubyte GLubyte GLubyte w
size_t particlesCount() const override
Get the m_particles count.
void clear()
Clear all elements of the maps.
ParticleData m_poseParticles
This class allows loading and storing values and vectors of different types from a configuration text...
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const
Returns the map built so far.
TEstimationMethod estimationMethod
This fields indicates the way this estimation was obtained.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
void saveCurrentPathEstimationToTextFile(const std::string &fil)
A logging utility: saves the current path estimation for each particle in a text file (a row per part...
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
Represents a probabilistic 3D (6D) movement.
Represents a probabilistic 2D movement of the robot mobile base.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
CParticleList m_particles
The array of particles.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
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...
std::shared_ptr< CActionRobotMovement2D > Ptr
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
double norm() const
Returns the euclidean norm of vector: .
#define MRPT_LOG_WARN(_STRING)
A helper class that can convert an enum value into its textual representation, and viceversa...
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const
Returns a copy of the current best pose estimation as a pose PDF.
#define MRPT_LOG_INFO(_STRING)
mrpt::utils::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
size_t getHeight() const override
Returns the height of the image in pixels.
std::shared_ptr< CPose3DPDF > Ptr
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
float insertionAngDistance
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
#define MRPT_LOG_DEBUG(_STRING)
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 un...
std::shared_ptr< CActionRobotMovement3D > Ptr
size_t size() const
Returns the count of pairs (pose,sensory data)
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
GLsizei const GLchar ** string
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
virtual ~CMetricMapBuilderRBPF()
Destructor.
float localizeAngDistance
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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)
double getW(size_t i) const override
Access to i'th particle (logarithm) weight, where first one is index 0.
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations.
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
TStats m_statsLastIteration
This structure will hold stats after each execution of processActionObservation.
TConstructionOptions()
Constructor.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM)...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
void getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle)...
bool debugForceInsertion
Always insert into map.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
int round(const T value)
Returns the closer integer (int) to x.
This virtual class defines the interface of any object accepting drawing primitives on it...
mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization
Traveled distance since last localization update.
std::deque< CObservation::Ptr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
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...
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i'th action of a given class, or a nullptr smart pointer if there is no action of that ...
bool insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle's pose and to each particle's metric map...
void drawCurrentEstimationToImage(utils::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
This class stores any customizable set of metric maps.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
size_t size() const
Returns the number of observations in the list.
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
virtual void drawImage(int x, int y, const utils::CImage &img)
Draws an image as a bitmap at a given position.
std::mutex critZoneChangingMap
Critical zones.
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilitie...
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
mrpt::system::TTimeStamp timestamp
The associated time-stamp.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i'th particle.
void insert(CAction &action)
Add a new object to the list.