Go to the documentation of this file.
33 initializationOptions.PF_options,
34 &initializationOptions.mapsInitializers,
35 &initializationOptions.predictionOptions),
36 m_PF_options(initializationOptions.PF_options),
37 insertionLinDistance(initializationOptions.insertionLinDistance),
38 insertionAngDistance(initializationOptions.insertionAngDistance),
39 localizeLinDistance(initializationOptions.localizeLinDistance),
40 localizeAngDistance(initializationOptions.localizeAngDistance),
41 odoIncrementSinceLastLocalization(),
42 odoIncrementSinceLastMapUpdate()
87 std::lock_guard<std::mutex> csl(
91 static CPose2D nullPose(0, 0, 0);
109 std::lock_guard<std::mutex> csl(
121 "processActionObservation(): Input action is "
122 "CActionRobotMovement3D="
123 << act3D->poseChange.getMeanVal().asString());
130 "processActionObservation(): Input action is "
131 "CActionRobotMovement2D="
132 << act2D->poseChange->getMeanVal().asString());
148 bool do_localization =
165 for (
auto& o : observations)
166 if (o->GetRuntimeClass() == *itCl)
168 do_map_update =
true;
169 do_localization =
true;
173 if (do_map_update) do_localization =
true;
177 "do_map_update=%s do_localization=%s", do_map_update ?
"YES" :
"NO",
178 do_localization ?
"YES" :
"NO"));
206 act2D->motionModelConfiguration);
213 "odoIncrementSinceLastLocalization before resetting = "
231 poseEstimation.
getMean(meanPose);
238 "New pose=" << estPos << std::endl
239 <<
"New ESS:" <<
mapPDF.
ESS() << std::endl);
242 " STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
243 sqrt(
cov(0, 0)), sqrt(
cov(1, 1)), sqrt(
cov(2, 2)),
260 "**No map was updated** after inserting a CSensoryFrame with "
261 << observations.size());
276 it->d->mapTillNow.auxParticleFilterCleanUp();
282 "Split initialize() in two, one generic without pose, one with "
283 "particles-based PDF");
292 "[initialize] Called with " << initialMap.
size()
293 <<
" nodes in fixed map");
297 std::lock_guard<std::mutex> csl(
305 else if (!initialMap.
empty())
308 curPose = initialMap.
rbegin()->first->getMeanVal();
322 mrpt::make_aligned_shared<CPose3DPDFParticles>();
327 for (
auto&
p : posePDF->m_particles)
339 std::deque<TPose3D>& outPath)
const
384 std::deque<TPose3D> path;
385 unsigned int imgHeight = 0;
392 ASSERT_(currentMetricMapEstimation->m_gridMaps.size() > 0);
395 unsigned int bestPath = 0;
396 double bestPathLik = -1;
397 for (i = 0; i < M; i++)
410 bool alreadyCopiedImage =
false;
415 currentMetricMapEstimation->m_gridMaps[0]->getSizeX(),
416 currentMetricMapEstimation->m_gridMaps[0]->getSizeY(), 1,
true);
418 if (!alreadyCopiedImage)
424 currentMetricMapEstimation->m_gridMaps[0]->getAsImage(imgGrid);
430 int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
431 float x_min = currentMetricMapEstimation->m_gridMaps[0]->getXMin();
432 float y_min = currentMetricMapEstimation->m_gridMaps[0]->getYMin();
434 currentMetricMapEstimation->m_gridMaps[0]->getResolution();
439 for (i = 0; i <= M; i++)
441 if (i != bestPath || i == M)
445 size_t nPoses = path.size();
448 x2 =
round((path[0].
x - x_min) / resolution);
449 y2 =
round((path[0].
y - y_min) / resolution);
452 for (
size_t j = 0; j < nPoses; j++)
459 x2 =
round((path[j].
x - x_min) / resolution);
460 y2 =
round((path[j].
y - y_min) / resolution);
464 x1,
round((imgHeight - 1) - y1), x2,
465 round((imgHeight - 1) - y2),
467 :
TColor(0x50, 0x50, 0x50),
496 img.saveToFile(file);
523 : insertionLinDistance(1.0f),
524 insertionAngDistance(
DEG2RAD(30)),
525 localizeLinDistance(0.4f),
526 localizeAngDistance(
DEG2RAD(10)),
538 std::ostream& out)
const
541 "\n----------- [CMetricMapBuilderRBPF::TConstructionOptions] "
542 "------------ \n\n");
545 "insertionLinDistance = %f m\n",
548 "insertionAngDistance = %f deg\n",
551 "localizeLinDistance = %f m\n",
554 "localizeAngDistance = %f deg\n",
557 "verbosity_level = %s\n",
562 PF_options.dumpToTextStream(out);
565 " Now showing 'mapsInitializers' and 'predictionOptions':\n");
568 mapsInitializers.dumpToTextStream(out);
569 predictionOptions.dumpToTextStream(out);
580 PF_options.loadFromConfigFile(
iniFile, section);
591 section,
"verbosity_level", verbosity_level);
593 mapsInitializers.loadFromConfigFile(
iniFile, section);
594 predictionOptions.loadFromConfigFile(
iniFile, section);
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 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,...
bool empty() const
Returns size()!=0.
bool isLoggingLevelVisible(VerbosityLevel level) const
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate
Traveled distance since last map update.
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=nullptr)
Executes a complete prediction + update step of the selected particle filtering algorithm.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const
Returns the map built so far.
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 loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
void getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle).
#define MRPT_LOG_INFO(_STRING)
bool observationsInserted
Whether the SF has been inserted in the metric maps.
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
#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...
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
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 ...
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter.
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...
double getW(size_t i) const override
Access to i'th particle (logarithm) weight, where first one is index 0.
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
std::shared_ptr< CPose3DPDFParticles > Ptr
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...
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i'th particle.
GLsizei GLsizei GLuint * obj
const CMultiMetricMap * getCurrentMostLikelyMetricMap() const
Returns a pointer to the current most likely map (associated to the most likely particle)
Represents a probabilistic 3D (6D) movement.
bool debugForceInsertion
Always insert into map.
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
const_reverse_iterator rbegin() const
MRPT_TODO("Split initialize() in two, one generic without pose, one with " "particles-based PDF")
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Declares a class for storing a collection of robot actions.
double norm() const
Returns the euclidean norm of vector: .
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLubyte GLubyte GLubyte GLubyte w
Represents a probabilistic 2D movement of the robot mobile base.
#define ASSERT_(f)
Defines an assertion mechanism.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
std::shared_ptr< CActionRobotMovement2D > Ptr
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double RAD2DEG(const double x)
Radians to degrees.
mrpt::system::VerbosityLevel verbosity_level
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...
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,...
This namespace contains representation of robot actions and observations.
TConstructionOptions()
Constructor.
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations.
TStats m_statsLastIteration
This structure will hold stats after each execution of processActionObservation.
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.
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.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
This virtual class defines the interface of any object accepting drawing primitives on it.
VerbosityLevel
Enumeration of available verbosity levels.
int round(const T value)
Returns the closer integer (int) to x.
size_t particlesCount() const override
Get the m_particles count.
mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization
Traveled distance since last localization update.
unsigned int getCurrentlyBuiltMapSize()
Returns just how many sensory-frames are stored in the currently build map.
This class allows loading and storing values and vectors of different types from a configuration text...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
This class stores any customizable set of metric maps.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::math::TPose3D asTPose() const
string iniFile(myDataDir+string("benchmark-options.ini"))
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
size_t size() const
Returns the count of pairs (pose,sensory data)
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
double getCurrentJointEntropy()
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...
TEstimationMethod estimationMethod
This fields indicates the way this estimation was obtained.
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
float localizeLinDistance
Distances (linear and angular) for updating the robot pose estimate (and particles weighs,...
A numeric matrix of compile-time fixed size.
#define MRPT_LOG_WARN(_STRING)
A class for storing images as grayscale or RGB bitmaps.
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const
Returns a copy of the current best pose estimation as a pose PDF.
float insertionLinDistance
Distances (linear and angular) for inserting a new observation into the map.
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
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::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
virtual void drawImage(int x, int y, const mrpt::img::CImage &img)
Draws an image as a bitmap at a given position.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
CParticleList m_particles
The array of particles.
void drawCurrentEstimationToImage(mrpt::img::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas
size_t getHeight() const override
Returns the height of the image in pixels.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
double yaw() const
Get the YAW angle (in radians)
void insert(CAction &action)
Add a new object to the list.
std::mutex critZoneChangingMap
Critical zones.
std::shared_ptr< CActionRobotMovement3D > Ptr
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
float localizeAngDistance
This base provides a set of functions for maths stuff.
std::shared_ptr< CPose3DPDF > Ptr
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF),...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
GLsizei const GLchar ** string
void clear()
Clear all elements of the maps.
VerbosityLevel getMinLoggingLevel() const
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...
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
mrpt::system::TTimeStamp timestamp
The associated time-stamp.
virtual ~CMetricMapBuilderRBPF()
Destructor.
float insertionAngDistance
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
CPose3D mean
The mean value.
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
CMetricMapBuilderRBPF & operator=(const CMetricMapBuilderRBPF &src)
Copy Operator.
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
double DEG2RAD(const double x)
Degrees to radians.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |