51 static constexpr
bool DoesResampling =
true;
61 const TPose3D* newPoseToBeInserted)
64 if (newPoseToBeInserted)
73 currentParticleValue && !currentParticleValue->
robotPath.empty())
88 const TPose3D* newPoseToBeInserted)
90 const size_t lenBinPath = (currentParticleValue !=
nullptr)
95 outBin.
bins.resize(lenBinPath + (newPoseToBeInserted !=
nullptr ? 1 : 0));
98 if (currentParticleValue !=
nullptr)
99 for (
size_t i = 0; i < lenBinPath; ++i)
110 if (newPoseToBeInserted !=
nullptr)
113 outBin.
bins[lenBinPath].x =
115 outBin.
bins[lenBinPath].y =
117 outBin.
bins[lenBinPath].phi =
128 : sensorLocationOnRobot(),
144 return a.nGaussiansInMap <
b.nGaussiansInMap;
168 void CMultiMetricMapPDF::prediction_and_update<mrpt::slam::OptimalProposal>(
179 size_t M = m_poseParticles.m_particles.size();
180 bool updateStageAlreadyDone =
false;
181 CPose3D initialPose, incrPose, finalPose;
197 actions->getBestMovementEstimation();
203 robotMovement2D->poseChange.get_ptr());
204 motionModelMeanIncr =
213 robotActionSampler.
setPosePDF(robotMovement3D->poseChange);
214 robotMovement3D->poseChange.getMean(motionModelMeanIncr);
224 averageMapIsUpdated =
false;
232 printf(
" 1) Prediction...");
233 M = m_poseParticles.m_particles.size();
236 size_t particleWithHighestW = 0;
237 for (
size_t i = 0; i < M; i++)
238 if (m_poseParticles.getW(i) >
239 m_poseParticles.getW(particleWithHighestW))
240 particleWithHighestW = i;
243 ASSERT_(!m_poseParticles.m_particles[0].d->robotPath.empty())
248 bool built_map_points =
false;
249 bool built_map_lms =
false;
253 for (i = 0, partIt = m_poseParticles.m_particles.begin();
254 partIt != m_poseParticles.m_particles.end(); partIt++, i++)
256 double extra_log_lik = 0;
260 *partIt->d->robotPath.rbegin());
262 CPose3D initialPoseEstimation = ith_last_pose + motionModelMeanIncr;
265 if (options.pfOptimalProposal_mapSelection == 0 ||
266 options.pfOptimalProposal_mapSelection == 1 ||
267 options.pfOptimalProposal_mapSelection == 3)
272 if (partIt->d->mapTillNow.m_pointsMaps.size())
274 ASSERT_(partIt->d->mapTillNow.m_pointsMaps.size() == 1);
281 if (options.pfOptimalProposal_mapSelection == 0)
283 ASSERT_(!partIt->d->mapTillNow.m_gridMaps.empty());
286 if (!built_map_points)
288 built_map_points =
true;
290 localMapPoints.insertionOptions.minDistBetweenLaserPoints =
293 localMapPoints.insertionOptions.isPlanarMap =
true;
294 sf->insertObservationsInto(&localMapPoints);
297 map_to_align_to = partIt->d->mapTillNow.m_gridMaps[0].get();
299 else if (options.pfOptimalProposal_mapSelection == 3)
302 ASSERT_(!partIt->d->mapTillNow.m_pointsMaps.empty());
305 if (!built_map_points)
307 built_map_points =
true;
309 localMapPoints.insertionOptions.minDistBetweenLaserPoints =
312 localMapPoints.insertionOptions.isPlanarMap =
true;
313 sf->insertObservationsInto(&localMapPoints);
316 map_to_align_to = partIt->d->mapTillNow.m_pointsMaps[0].get();
320 ASSERT_(partIt->d->mapTillNow.m_landmarksMap);
325 built_map_lms =
true;
326 sf->insertObservationsInto(&localMapLandmarks);
329 map_to_align_to = partIt->d->mapTillNow.m_landmarksMap.get();
332 ASSERT_(map_to_align_to !=
nullptr);
337 map_to_align_to, &localMapPoints,
338 CPose2D(initialPoseEstimation),
nullptr, &icpInfo);
342 if (i == particleWithHighestW)
344 newInfoIndex = 1 - icpInfo.
goodness;
353 if (icpInfo.
goodness < options.ICPGlobalAlign_MinQuality &&
357 "[rbpf-slam] Warning: gridICP[%u]: %.02f%% -> Using " 358 "odometry instead!\n",
359 (
unsigned int)i, 100 * icpInfo.
goodness);
360 icpEstimation.
mean =
CPose2D(initialPoseEstimation);
380 #if 0 // Use hacked ICP covariance: 381 CPose3D Ap = finalEstimatedPoseGauss.
mean - ith_last_pose;
382 const double Ap_dist = Ap.
norm();
384 finalEstimatedPoseGauss.
cov.zeros();
385 finalEstimatedPoseGauss.
cov(0,0) =
square( fabs(Ap_dist)*0.01 );
386 finalEstimatedPoseGauss.
cov(1,1) =
square( fabs(Ap_dist)*0.01 );
387 finalEstimatedPoseGauss.
cov(2,2) =
square( fabs(Ap.
yaw())*0.02 );
400 finalEstimatedPoseGauss.
mean;
402 rndSamples, finalEstimatedPoseGauss.
cov);
404 finalPose.setFromValues(
405 finalPose.x() + rndSamples[0], finalPose.y() + rndSamples[1],
406 finalPose.z(), finalPose.yaw() + rndSamples[2],
407 finalPose.pitch(), finalPose.roll());
409 else if (options.pfOptimalProposal_mapSelection == 2)
417 ASSERT_(partIt->d->mapTillNow.m_beaconMap);
420 updateStageAlreadyDone =
433 bool methodSOGorGrid =
false;
435 float firstEstimateRobotHeading = std::numeric_limits<float>::max();
438 CPoint3D centerPositionPrior(ith_last_pose);
439 float centerPositionPriorRadius = 2.0f;
443 firstEstimateRobotHeading = ith_last_pose.
yaw();
448 if (!beacMap->size())
451 cerr <<
"[RO-SLAM] Optimal filtering without map & " 452 "odometry...this message should appear only the " 461 if (beacMap->get(0).m_typePDF == CBeacon::pdfSOG)
463 cerr <<
"[RO-SLAM] Optimal filtering without map & " 464 "odometry->FIXING ONE BEACON!" 466 ASSERT_(beacMap->get(0).m_locationSOG.size() > 0)
469 beacMap->get(0).m_locationSOG[0].val.mean);
472 beacMap->get(0).m_typePDF = CBeacon::pdfGauss;
473 beacMap->get(0).m_locationSOG.clear();
474 beacMap->get(0).m_locationGauss.mean = fixedBeacon;
475 beacMap->get(0).m_locationGauss.cov.unit(3, 1e-6);
482 deque<TAuxRangeMeasInfo> lstObservedRanges;
485 itObs != sf->end(); ++itObs)
492 deque<CObservationBeaconRanges::TMeasurement>::
493 const_iterator itRanges;
495 itRanges != obs->
sensedData.end(); itRanges++)
500 itBeacs != beacMap->end(); ++itBeacs)
502 if ((itBeacs)->m_ID == itRanges->beaconID)
505 newMeas.
beaconID = itRanges->beaconID;
507 itRanges->sensedDistance;
509 itRanges->sensorLocationOnRobot;
512 (itBeacs)->m_typePDF == CBeacon::pdfGauss ||
513 (itBeacs)->m_typePDF == CBeacon::pdfSOG)
515 (itBeacs)->m_typePDF == CBeacon::pdfSOG
516 ? (itBeacs)->m_locationSOG.size()
519 lstObservedRanges.push_back(newMeas);
532 lstObservedRanges.begin(), lstObservedRanges.end(),
539 fusedObsModels.
clear();
556 firstEstimateRobotHeading = auxPose.
yaw();
566 poseCOV.setSize(2, 2);
567 poseCOV.setSize(3, 3);
568 newMode.
val.
cov = poseCOV;
575 itBeacs != beacMap->end(); ++itBeacs)
580 lstObservedRanges.begin();
581 itObs != lstObservedRanges.end(); ++itObs)
583 if ((itBeacs)->m_ID == itObs->beaconID)
586 float sensedRange = itObs->sensedDistance;
589 (itBeacs)->generateObservationModelDistribution(
590 sensedRange, newObsModel,
592 itObs->sensorLocationOnRobot,
595 centerPositionPrior, centerPositionPriorRadius);
597 if (!fusedObsModels.
size())
601 fusedObsModels = newObsModel;
608 fusedObsModels, newObsModel,
611 fusedObsModels = tempFused;
618 cout <<
"#modes bef: " << fusedObsModels.
size()
619 <<
" ESS: " << fusedObsModels.
ESS()
621 double max_w = -1e100;
626 for (it = fusedObsModels.
begin();
627 it != fusedObsModels.
end(); it++)
629 max(max_w, (it)->log_w);
633 for (it = fusedObsModels.
begin();
634 it != fusedObsModels.
end();)
636 if (max_w - (it)->log_w >
638 it = fusedObsModels.
erase(it);
644 <<
"#modes after: " << fusedObsModels.
size()
653 currentCov(0, 0) > 0 && currentCov(1, 1) > 0)
654 if (sqrt(currentCov(0, 0)) < 0.10f &&
655 sqrt(currentCov(1, 1)) < 0.10f &&
656 sqrt(currentCov(2, 2)) < 0.10f)
660 fusedObsModels.
getMean(currentMean);
661 fusedObsModels[0].log_w = 0;
662 fusedObsModels[0].val.mean = currentMean;
663 fusedObsModels[0].val.cov = currentCov;
703 float grid_min_x = ith_last_pose.
x() - 0.5f;
704 float grid_max_x = ith_last_pose.
x() + 0.5f;
705 float grid_min_y = ith_last_pose.
y() - 0.5f;
706 float grid_max_y = ith_last_pose.
y() + 0.5f;
707 float grid_resXY = 0.02f;
709 bool repeatGridCalculation =
false;
714 grid_min_x, grid_max_x, grid_min_y, grid_max_y,
715 grid_resXY,
DEG2RAD(180), 0, 0);
722 itBeacs != beacMap->end(); ++itBeacs)
727 lstObservedRanges.begin();
728 itObs != lstObservedRanges.end(); ++itObs)
730 if ((itBeacs)->m_ID == itObs->beaconID)
733 float sensedRange = itObs->sensedDistance;
749 for (
size_t idxX = 0;
752 float grid_x = pdfGrid->
idx2x(idxX);
753 for (
size_t idxY = 0;
756 double grid_y = pdfGrid->
idx2y(idxY);
765 switch ((itBeacs)->m_typePDF)
767 case CBeacon::pdfSOG:
770 &(itBeacs)->m_locationSOG;
771 double sensorSTD2 =
square(
772 beacMap->likelihoodOptions
776 for (it = sog->
begin();
777 it != sog->
end(); it++)
783 (it)->
val.mean.distance2DTo(
786 ->sensorLocationOnRobot
790 ->sensorLocationOnRobot
814 float maxX = 0, maxY = 0;
815 for (
size_t idxX = 0; idxX < pdfGrid->
getSizeX();
819 for (
size_t idxY = 0; idxY < pdfGrid->
getSizeY();
829 maxX = pdfGrid->
idx2x(idxX);
830 maxY = pdfGrid->
idx2y(idxY);
834 newDrawnPosition.
x(maxX);
835 newDrawnPosition.
y(maxY);
843 outMat *= 1.0f/outMat.maxCoeff();
844 CImage imgF(outMat,
true);
845 static int autocount=0;
847 printf(
"grid res: %f MAX: %f,%f\n",grid_resXY,maxX,maxY);
852 if (grid_resXY > 0.01f)
854 grid_min_x = maxX - 0.03f;
855 grid_max_x = maxX + 0.03f;
856 grid_min_y = maxY - 0.03f;
857 grid_max_y = maxY + 0.03f;
859 repeatGridCalculation =
true;
862 repeatGridCalculation =
false;
878 }
while (repeatGridCalculation);
887 if (!beacMap->size())
891 finalPose = ith_last_pose;
895 cout <<
"Drawn: " << newDrawnPosition << endl;
900 firstEstimateRobotHeading !=
901 std::numeric_limits<float>::max())
905 newDrawnPosition.
x(), newDrawnPosition.
y(),
906 newDrawnPosition.z(), firstEstimateRobotHeading, 0, 0);
917 double weightUpdate = 0;
918 partIt->log_w += PF_options.powFactor * weightUpdate;
927 "Action list does not contain any CActionRobotMovement2D " 928 "or CActionRobotMovement3D object!");
932 finalPose = ith_last_pose + incrPose;
936 partIt->d->robotPath.push_back(finalPose);
941 if (!updateStageAlreadyDone)
943 partIt->log_w += PF_options.powFactor *
944 (PF_SLAM_computeObservationLikelihoodForParticle(
945 PF_options, i, *sf, finalPose) +
960 void CMultiMetricMapPDF::prediction_and_update<mrpt::slam::StandardProposal>(
967 StandardProposal::PF_SLAM_implementation<
969 actions, sf, PF_options, options.KLD_params, *
this);
972 averageMapIsUpdated =
false;
978 void CMultiMetricMapPDF::
979 PF_SLAM_implementation_custom_update_particle_with_new_pose(
982 particleData->
robotPath.push_back(newPose);
986 bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
990 if (sf ==
nullptr)
return false;
992 return particles.begin()
994 ->mapTillNow.canComputeObservationsLikelihood(*sf);
998 bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement()
const 1000 return 0 == getNumberOfObservationsInSimplemap();
1007 double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
1009 const size_t particleIndexForMap,
const CSensoryFrame& observation,
1014 &m_poseParticles.m_particles[particleIndexForMap].d->mapTillNow);
1017 it != observation.
end(); ++it)
1022 void CMultiMetricMapPDF::executeOn(
1028 switch (PF_algorithm)
1032 *
this, action, observation, stats);
1036 *
this, action, observation, stats);
1040 *
this, action, observation, stats);
1044 *
this, action, observation, stats);
A namespace of pseudo-random numbers genrators of diferent distributions.
double x() const
Common members of all points & poses classes.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose2D mean
The mean value.
CPose3D mean
The mean value.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
ParticleData::CParticleList CParticleList
static bool cmp_Asc(const TAuxRangeMeasInfo &a, const TAuxRangeMeasInfo &b)
Auxiliary for optimal sampling in RO-SLAM.
void getMean(CPoint3D &mean_point) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
A class for storing images as grayscale or RGB bitmaps.
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
CPoint3D mean
The mean value.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
void resize(const size_t N)
Resize the number of SOG modes.
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
#define THROW_EXCEPTION(msg)
The struct for each mode:
double computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom)
Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
void uniformDistribution()
Assigns the same value to all the cells in the grid, so the sum 1.
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
void clear()
Clear all the gaussian modes.
double yaw() const
Get the YAW angle (in radians)
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Option set for KLD algorithm.
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
std::deque< CBeacon >::iterator iterator
double yaw
Yaw coordinate (rotation angle over Z axis).
A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution...
Statistics for being returned from the "execute" method.
bool isPrepared() const
Return true if samples can be generated, which only requires a previous call to setPosePDF.
void KLF_loadBinFromParticle(detail::TPathBin2D &outBin, const TKLDParams &opts, const mrpt::maps::CRBPFParticleData *currentParticleValue, const TPose3D *newPoseToBeInserted)
Fills out a "TPathBin2D" variable, given a path hypotesis and (if not set to nullptr) a new pose appe...
Declares a class for storing a collection of robot actions.
double idx2y(size_t y) const
Returns coordinates from "indexes":
T square(const T x)
Inline function for the square of a number.
void getAsMatrix(const double &phi, MATRIXLIKE &outMat)
Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for...
const T * getByIndex(size_t x, size_t y, size_t phi) const
Reads the contents of a cell.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
Represents a probabilistic 3D (6D) movement.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
This base provides a set of functions for maths stuff.
std::shared_ptr< CPosePDF > Ptr
A class for storing a map of 3D probabilistic landmarks.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
std::shared_ptr< CActionRobotMovement2D > Ptr
void setPosePDF(const CPosePDF *pdf)
This method must be called to select the PDF from which to draw samples.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
double norm() const
Returns the euclidean norm of vector: .
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
std::deque< TMeasurement > sensedData
The list of observed ranges.
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.
double KLD_binSize_XY
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...
const TGaussianMode & get(size_t i) const
Access to individual beacons.
std::shared_ptr< CActionRobotMovement3D > Ptr
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
A class used to store a 3D point.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
iterator erase(iterator i)
Auxiliary structure used in KLD-sampling in particle filters.
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#define INVALID_BEACON_ID
Used for CObservationBeaconRange, CBeacon, etc.
void executeOn(PARTICLEFILTERCAPABLE &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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
CPoint3D sensorLocationOnRobot
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
The configuration of a particle filter.
size_t size() const
Return the number of Gaussian modes.
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
std::deque< mrpt::math::TPose3D > robotPath
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...
The ICP algorithm return information.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
std::deque< CObservation::Ptr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
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.
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)...
double log_w
The log-weight.
std::vector< TPoseBin2D > bins
double idx2x(size_t x) const
Returns coordinates from "indexes":
Auxiliary for optimal sampling in RO-SLAM.
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
std::deque< TGaussianMode >::iterator iterator
void drawGaussianMultivariate(std::vector< T > &out_result, const mrpt::math::CMatrixTemplateNumeric< T > &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
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.
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
GLubyte GLubyte GLubyte a
std::shared_ptr< CBeaconMap > Ptr
void normalize()
Normalizes the PDF, such as all cells sum the unity.