49 const TPose3D* newPoseToBeInserted)
52 if (newPoseToBeInserted)
61 currentParticleValue && !currentParticleValue->
robotPath.empty());
76 const TPose3D* newPoseToBeInserted)
78 const size_t lenBinPath = (currentParticleValue !=
nullptr)
83 outBin.
bins.resize(lenBinPath + (newPoseToBeInserted !=
nullptr ? 1 : 0));
86 if (currentParticleValue !=
nullptr)
87 for (
size_t i = 0; i < lenBinPath; ++i)
98 if (newPoseToBeInserted !=
nullptr)
101 outBin.
bins[lenBinPath].x =
103 outBin.
bins[lenBinPath].y =
105 outBin.
bins[lenBinPath].phi =
118 : sensorLocationOnRobot(),
126 float sensedDistance{0};
128 size_t nGaussiansInMap{
143 void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFOptimal(
150 PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin2D>(
151 actions, sf, PF_options, options.KLD_params);
159 void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFStandard(
166 PF_SLAM_implementation_pfAuxiliaryPFStandard<
168 actions, sf, PF_options, options.KLD_params);
188 void CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal(
199 bool updateStageAlreadyDone =
false;
200 CPose3D initialPose, incrPose, finalPose;
207 CParticleList::iterator partIt;
220 robotActionSampler.
setPosePDF(*robotMovement2D->poseChange);
221 motionModelMeanIncr =
230 robotActionSampler.
setPosePDF(robotMovement3D->poseChange);
231 robotMovement3D->poseChange.getMean(motionModelMeanIncr);
241 averageMapIsUpdated =
false;
250 const size_t M = m_particles.size();
253 size_t particleWithHighestW = 0;
254 for (
size_t i = 0; i < M; i++)
255 if (getW(i) > getW(particleWithHighestW)) particleWithHighestW = i;
258 ASSERT_(!m_particles[0].d->robotPath.empty());
262 bool built_map_points =
false;
263 bool built_map_lms =
false;
267 for (i = 0, partIt = m_particles.begin(); partIt != m_particles.end();
270 double extra_log_lik = 0;
274 *partIt->d->robotPath.rbegin());
276 CPose3D initialPoseEstimation = ith_last_pose + motionModelMeanIncr;
279 if (options.pfOptimalProposal_mapSelection == 0 ||
280 options.pfOptimalProposal_mapSelection == 1 ||
281 options.pfOptimalProposal_mapSelection == 3)
286 auto partMap = partIt->d->mapTillNow;
289 ASSERT_(numPtMaps == 0 || numPtMaps == 1);
293 if (options.pfOptimalProposal_mapSelection == 0)
299 if (!built_map_points)
301 built_map_points =
true;
303 localMapPoints.insertionOptions.minDistBetweenLaserPoints =
305 localMapPoints.insertionOptions.isPlanarMap =
true;
309 map_to_align_to = grid.get();
312 else if (options.pfOptimalProposal_mapSelection == 3)
318 if (!built_map_points)
320 built_map_points =
true;
322 localMapPoints.insertionOptions.minDistBetweenLaserPoints =
324 localMapPoints.insertionOptions.isPlanarMap =
true;
328 map_to_align_to = ptsMap.get();
338 built_map_lms =
true;
342 map_to_align_to = lmMap.get();
345 ASSERT_(map_to_align_to !=
nullptr);
350 map_to_align_to, &localMapPoints,
351 CPose2D(initialPoseEstimation), icpInfo);
355 if (i == particleWithHighestW)
357 newInfoIndex = 1 - icpInfo.
goodness;
365 "gridICP[particle %u]: %.02f%%", static_cast<unsigned int>(i),
367 if (icpInfo.
goodness < options.ICPGlobalAlign_MinQuality &&
371 "gridICP[particle %u]: %.02f%% -> Using odometry instead!",
372 (
unsigned int)i, 100 * icpInfo.
goodness);
373 icpEstimation.
mean =
CPose2D(initialPoseEstimation);
385 finalEstimatedPoseGauss.
mean;
387 rndSamples, finalEstimatedPoseGauss.
cov);
389 finalPose.setFromValues(
390 finalPose.x() + rndSamples[0], finalPose.y() + rndSamples[1],
391 finalPose.z(), finalPose.yaw() + rndSamples[2],
392 finalPose.pitch(), finalPose.roll());
394 else if (options.pfOptimalProposal_mapSelection == 2)
399 auto beacMap = partIt->d->mapTillNow.mapByClass<
CBeaconMap>();
402 updateStageAlreadyDone =
true;
413 bool methodSOGorGrid =
false;
415 float firstEstimateRobotHeading = std::numeric_limits<float>::max();
418 CPoint3D centerPositionPrior(ith_last_pose);
419 float centerPositionPriorRadius = 2.0f;
423 firstEstimateRobotHeading = ith_last_pose.
yaw();
428 if (!beacMap->size())
432 "[RO-SLAM] Optimal filtering without map & " 433 "odometry...this message should appear only the " 434 "first iteration!!");
441 if (beacMap->get(0).m_typePDF == CBeacon::pdfSOG)
444 "[RO-SLAM] Optimal filtering without map & " 445 "odometry->FIXING ONE BEACON!");
446 ASSERT_(beacMap->get(0).m_locationSOG.size() > 0);
448 beacMap->get(0).m_locationSOG[0].val.mean);
451 beacMap->get(0).m_typePDF = CBeacon::pdfGauss;
452 beacMap->get(0).m_locationSOG.clear();
453 beacMap->get(0).m_locationGauss.mean = fixedBeacon;
454 beacMap->get(0).m_locationGauss.cov.setDiagonal(
462 deque<TAuxRangeMeasInfo> lstObservedRanges;
464 for (
const auto& itObs : *sf)
471 deque<CObservationBeaconRanges::TMeasurement>::
472 const_iterator itRanges;
473 for (itRanges = obs->sensedData.begin();
474 itRanges != obs->sensedData.end(); itRanges++)
478 for (
auto itBeacs = beacMap->begin();
479 itBeacs != beacMap->end(); ++itBeacs)
481 if ((itBeacs)->m_ID == itRanges->beaconID)
484 newMeas.
beaconID = itRanges->beaconID;
486 itRanges->sensedDistance;
488 itRanges->sensorLocationOnRobot;
491 (itBeacs)->m_typePDF == CBeacon::pdfGauss ||
492 (itBeacs)->m_typePDF == CBeacon::pdfSOG);
494 (itBeacs)->m_typePDF == CBeacon::pdfSOG
495 ? (itBeacs)->m_locationSOG.size()
498 lstObservedRanges.push_back(newMeas);
511 lstObservedRanges.begin(), lstObservedRanges.end(),
518 fusedObsModels.
clear();
535 firstEstimateRobotHeading = auxPose.
yaw();
547 newMode.
val.
cov = poseCOV;
553 for (
auto itBeacs = beacMap->begin(); itBeacs != beacMap->end();
558 for (
auto& lstObservedRange : lstObservedRanges)
560 if ((itBeacs)->m_ID == lstObservedRange.beaconID)
563 float sensedRange = lstObservedRange.sensedDistance;
566 (itBeacs)->generateObservationModelDistribution(
567 sensedRange, newObsModel,
570 .sensorLocationOnRobot,
573 centerPositionPrior, centerPositionPriorRadius);
575 if (!fusedObsModels.
size())
579 fusedObsModels = newObsModel;
586 fusedObsModels, newObsModel,
589 fusedObsModels = tempFused;
596 cout <<
"#modes bef: " << fusedObsModels.
size()
597 <<
" ESS: " << fusedObsModels.
ESS()
599 double max_w = -1e100;
604 for (it = fusedObsModels.
begin();
605 it != fusedObsModels.
end(); it++)
607 max(max_w, (it)->log_w);
611 for (it = fusedObsModels.
begin();
612 it != fusedObsModels.
end();)
614 if (max_w - (it)->log_w >
616 it = fusedObsModels.
erase(it);
622 <<
"#modes after: " << fusedObsModels.
size()
631 currentCov(0, 0) > 0 && currentCov(1, 1) > 0);
632 if (sqrt(currentCov(0, 0)) < 0.10f &&
633 sqrt(currentCov(1, 1)) < 0.10f &&
634 sqrt(currentCov(2, 2)) < 0.10f)
638 fusedObsModels.
getMean(currentMean);
639 fusedObsModels[0].log_w = 0;
640 fusedObsModels[0].val.mean = currentMean;
641 fusedObsModels[0].val.cov = currentCov;
669 float grid_min_x = ith_last_pose.
x() - 0.5f;
670 float grid_max_x = ith_last_pose.
x() + 0.5f;
671 float grid_min_y = ith_last_pose.
y() - 0.5f;
672 float grid_max_y = ith_last_pose.
y() + 0.5f;
673 float grid_resXY = 0.02f;
675 bool repeatGridCalculation =
false;
679 auto pdfGrid = std::make_unique<CPosePDFGrid>(
680 grid_min_x, grid_max_x, grid_min_y, grid_max_y,
681 grid_resXY, 180.0_deg, 0, 0);
683 pdfGrid->uniformDistribution();
687 for (
auto itBeacs = beacMap->begin();
688 itBeacs != beacMap->end(); ++itBeacs)
692 for (
auto& lstObservedRange : lstObservedRanges)
694 if ((itBeacs)->m_ID == lstObservedRange.beaconID)
698 lstObservedRange.sensedDistance;
714 for (
size_t idxX = 0;
715 idxX < pdfGrid->getSizeX(); idxX++)
717 float grid_x = pdfGrid->idx2x(idxX);
718 for (
size_t idxY = 0;
719 idxY < pdfGrid->getSizeY(); idxY++)
721 double grid_y = pdfGrid->idx2y(idxY);
725 pdfGrid->getByIndex(idxX, idxY, 0);
730 switch ((itBeacs)->m_typePDF)
732 case CBeacon::pdfSOG:
735 &(itBeacs)->m_locationSOG;
736 double sensorSTD2 =
square(
737 beacMap->likelihoodOptions
741 for (it = sog->
begin();
742 it != sog->
end(); it++)
748 (it)->
val.mean.distance2DTo(
751 .sensorLocationOnRobot
755 .sensorLocationOnRobot
769 pdfGrid->normalize();
779 float maxX = 0, maxY = 0;
780 for (
size_t idxX = 0; idxX < pdfGrid->getSizeX();
784 for (
size_t idxY = 0; idxY < pdfGrid->getSizeY();
790 float c = *pdfGrid->getByIndex(idxX, idxY, 0);
794 maxX = pdfGrid->idx2x(idxX);
795 maxY = pdfGrid->idx2y(idxY);
799 newDrawnPosition.
x(maxX);
800 newDrawnPosition.
y(maxY);
802 if (grid_resXY > 0.01f)
804 grid_min_x = maxX - 0.03f;
805 grid_max_x = maxX + 0.03f;
806 grid_min_y = maxY - 0.03f;
807 grid_max_y = maxY + 0.03f;
809 repeatGridCalculation =
true;
812 repeatGridCalculation =
false;
816 }
while (repeatGridCalculation);
825 if (!beacMap->size())
829 finalPose = ith_last_pose;
833 cout <<
"Drawn: " << newDrawnPosition << endl;
839 firstEstimateRobotHeading !=
840 std::numeric_limits<float>::max());
843 newDrawnPosition.
x(), newDrawnPosition.
y(),
844 newDrawnPosition.z(), firstEstimateRobotHeading, 0, 0);
855 double weightUpdate = 0;
856 partIt->log_w += PF_options.
powFactor * weightUpdate;
865 "Action list does not contain any CActionRobotMovement2D " 866 "or CActionRobotMovement3D object!");
870 finalPose = ith_last_pose + incrPose;
874 partIt->d->robotPath.push_back(finalPose.
asTPose());
879 if (!updateStageAlreadyDone)
882 (PF_SLAM_computeObservationLikelihoodForParticle(
883 PF_options, i, *sf, finalPose) +
897 void CMultiMetricMapPDF::prediction_and_update_pfStandardProposal(
904 PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
905 actions, sf, PF_options, options.KLD_params);
908 averageMapIsUpdated =
false;
914 void CMultiMetricMapPDF::
915 PF_SLAM_implementation_custom_update_particle_with_new_pose(
918 particleData->
robotPath.push_back(newPose);
922 bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
923 const CMultiMetricMapPDF::CParticleList& particles,
926 if (sf ==
nullptr)
return false;
928 return particles.begin()
930 ->mapTillNow.canComputeObservationsLikelihood(*sf);
934 bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement()
const 936 return 0 == getNumberOfObservationsInSimplemap();
943 double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
945 const size_t particleIndexForMap,
const CSensoryFrame& observation,
949 &m_particles[particleIndexForMap].d->mapTillNow);
951 for (
const auto& it : observation)
A namespace of pseudo-random numbers generators of diferent distributions.
mrpt::math::TPose3D asTPose() const
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
CPose2D mean
The mean value.
CPose3D mean
The mean value.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
static bool cmp_Asc(const TAuxRangeMeasInfo &a, const TAuxRangeMeasInfo &b)
Auxiliary for optimal sampling in RO-SLAM.
#define THROW_EXCEPTION(msg)
void getMean(CPoint3D &mean_point) const override
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 ...
The struct for each mode:
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
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) ...
double yaw
Yaw coordinate (rotation angle over Z axis).
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
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.
#define ASSERT_(f)
Defines an assertion mechanism.
Represents a probabilistic 3D (6D) movement.
This base provides a set of functions for maths stuff.
A class for storing a map of 3D probabilistic landmarks.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
void setPosePDF(const CPosePDF &pdf)
This method must be called to select the PDF from which to draw samples.
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
This namespace contains representation of robot actions and observations.
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
double KLD_binSize_XY
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
const TGaussianMode & get(size_t i) const
Access to individual beacons.
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
double x() const
Common members of all points & poses classes.
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
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 ...
iterator erase(iterator i)
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...
Auxiliary structure used in KLD-sampling in particle filters.
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
return_t square(const num_t x)
Inline function for the square of a number.
#define INVALID_BEACON_ID
Used for CObservationBeaconRange, CBeacon, etc.
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void drawGaussianMultivariate(std::vector< T > &out_result, const MATRIX &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
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).
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
CPoint3D sensorLocationOnRobot
The configuration of a particle filter.
size_t size() const
Return the number of Gaussian modes.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
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).
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
#define MRPT_LOG_WARN(_STRING)
double log_w
The log-weight.
std::vector< TPoseBin2D > bins
Auxiliary for optimal sampling in RO-SLAM.
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
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...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=nullptr) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
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 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< TGaussianMode >::iterator iterator
int round(const T value)
Returns the closer integer (int) to x.