Go to the documentation of this file.
35 initializationOptions.PF_options,
36 initializationOptions.mapsInitializers,
37 initializationOptions.predictionOptions),
38 m_PF_options(initializationOptions.PF_options),
39 insertionLinDistance(initializationOptions.insertionLinDistance),
40 insertionAngDistance(initializationOptions.insertionAngDistance),
41 localizeLinDistance(initializationOptions.localizeLinDistance),
42 localizeAngDistance(initializationOptions.localizeAngDistance),
43 odoIncrementSinceLastLocalization(),
44 odoIncrementSinceLastMapUpdate()
89 static CPose2D nullPose(0, 0, 0);
118 "processActionObservation(): Input action is "
119 "CActionRobotMovement3D="
120 << act3D->poseChange.getMeanVal().asString());
127 "processActionObservation(): Input action is "
128 "CActionRobotMovement2D="
129 << act2D->poseChange->getMeanVal().asString());
145 bool do_localization =
162 for (
auto& o : observations)
163 if (o->GetRuntimeClass() == *itCl)
165 do_map_update =
true;
166 do_localization =
true;
170 if (do_map_update) do_localization =
true;
173 "do_map_update=%s do_localization=%s", do_map_update ?
"YES" :
"NO",
174 do_localization ?
"YES" :
"NO"));
202 act2D->motionModelConfiguration);
209 "odoIncrementSinceLastLocalization before resetting = "
227 poseEstimation.
getMean(meanPose);
232 "New pose=" << estPos << std::endl
233 <<
"New ESS:" <<
mapPDF.
ESS() << std::endl);
235 " STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
236 sqrt(
cov(0, 0)), sqrt(
cov(1, 1)), sqrt(
cov(2, 2)),
253 "**No map was updated** after inserting a CSensoryFrame with "
254 << observations.size());
267 m_particle.d->mapTillNow.auxParticleFilterCleanUp();
279 "[initialize] Called with " << initialMap.
size()
280 <<
" nodes in fixed map");
291 else if (!initialMap.
empty())
294 curPose = initialMap.
rbegin()->first->getMeanVal();
312 for (
auto& p : posePDF->m_particles)
324 std::deque<TPose3D>& outPath)
const
369 std::deque<TPose3D> path;
370 unsigned int imgHeight = 0;
379 unsigned int bestPath = 0;
380 double bestPathLik = -1;
381 for (i = 0; i < M; i++)
394 bool alreadyCopiedImage =
false;
397 auto* obj =
dynamic_cast<CImage*
>(img);
400 if (!alreadyCopiedImage)
406 g->getAsImage(imgGrid);
412 int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
413 float x_min = g->getXMin();
414 float y_min = g->getYMin();
415 float resolution = g->getResolution();
420 for (i = 0; i <= M; i++)
422 if (i != bestPath || i == M)
426 size_t nPoses = path.size();
429 x2 =
round((path[0].x - x_min) / resolution);
430 y2 =
round((path[0].y - y_min) / resolution);
433 for (
size_t j = 0; j < nPoses; j++)
440 x2 =
round((path[j].x - x_min) / resolution);
441 y2 =
round((path[j].y - y_min) / resolution);
445 x1,
round((imgHeight - 1) - y1), x2,
446 round((imgHeight - 1) - y2),
448 :
TColor(0x50, 0x50, 0x50),
462 const std::string& file,
bool formatEMF_BMP)
495 const std::string& fil)
504 : insertionAngDistance(30.0_deg),
506 localizeAngDistance(10.0_deg),
518 std::ostream&
out)
const
520 out <<
"\n----------- [CMetricMapBuilderRBPF::TConstructionOptions] "
524 "insertionLinDistance = %f m\n",
527 "insertionAngDistance = %f deg\n",
530 "localizeLinDistance = %f m\n",
533 "localizeAngDistance = %f deg\n",
536 "verbosity_level = %s\n",
541 PF_options.dumpToTextStream(
out);
543 out <<
" Now showing 'mapsInitializers' and 'predictionOptions':\n";
546 mapsInitializers.dumpToTextStream(
out);
547 predictionOptions.dumpToTextStream(
out);
558 PF_options.loadFromConfigFile(
iniFile, section);
569 section,
"verbosity_level", verbosity_level);
571 mapsInitializers.loadFromConfigFile(
iniFile, section);
572 predictionOptions.loadFromConfigFile(
iniFile, section);
void resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content,...
bool insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle's pose and to each particle's metric map.
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
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.
T::Ptr getActionByClass(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 ...
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");
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter.
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.
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.
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
std::shared_ptr< mrpt::poses ::CPose3DPDFParticles > Ptr
mrpt::vision::TStereoCalibResults out
double norm() const
Returns the euclidean norm of vector: .
~CMetricMapBuilderRBPF() override
Destructor.
Declares a class for storing a collection of robot actions.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::shared_ptr< mrpt::obs ::CActionRobotMovement3D > Ptr
Represents a probabilistic 2D movement of the robot mobile base.
#define ASSERT_(f)
Defines an assertion mechanism.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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...
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
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.
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.
This class allows loading and storing values and vectors of different types from a configuration text...
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.
constexpr double RAD2DEG(const double x)
Radians to degrees.
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.
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
double getCurrentJointEntropy()
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.
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
float localizeLinDistance
Distances (linear and angular) for updating the robot pose estimate (and particles weighs,...
#define MRPT_LOG_WARN(_STRING)
A class for storing images as grayscale or RGB bitmaps.
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV).
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,...
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.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
#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
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) override
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
size_t getHeight() const override
Returns the height of the image in pixels.
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
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.
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.
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &observations) override
Appends a new action and observations to update this map: See the description of the class at the top...
std::shared_ptr< CPose3DPDF > Ptr
A class for storing an occupancy grid map.
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)
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.
float insertionAngDistance
virtual void line(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid)
Draws a line.
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].
std::string std::string format(std::string_view fmt, ARGS &&... args)
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...
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020 | |