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{
134 return a.nGaussiansInMap <
b.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 size_t M = m_particles.
size();
200 bool updateStageAlreadyDone =
false;
201 CPose3D initialPose, incrPose, finalPose;
208 CParticleList::iterator partIt;
221 robotActionSampler.
setPosePDF(*robotMovement2D->poseChange);
222 motionModelMeanIncr =
231 robotActionSampler.
setPosePDF(robotMovement3D->poseChange);
232 robotMovement3D->poseChange.getMean(motionModelMeanIncr);
242 averageMapIsUpdated =
false;
250 printf(
" 1) Prediction...");
251 M = m_particles.size();
254 size_t particleWithHighestW = 0;
255 for (
size_t i = 0; i < M; i++)
256 if (getW(i) > getW(particleWithHighestW)) particleWithHighestW = i;
259 ASSERT_(!m_particles[0].d->robotPath.empty());
263 bool built_map_points =
false;
264 bool built_map_lms =
false;
268 for (i = 0, partIt = m_particles.begin(); partIt != m_particles.end();
271 double extra_log_lik = 0;
275 *partIt->d->robotPath.rbegin());
277 CPose3D initialPoseEstimation = ith_last_pose + motionModelMeanIncr;
280 if (options.pfOptimalProposal_mapSelection == 0 ||
281 options.pfOptimalProposal_mapSelection == 1 ||
282 options.pfOptimalProposal_mapSelection == 3)
287 auto partMap = partIt->d->mapTillNow;
290 ASSERT_(numPtMaps == 0 || numPtMaps == 1);
294 if (options.pfOptimalProposal_mapSelection == 0)
300 if (!built_map_points)
302 built_map_points =
true;
304 localMapPoints.insertionOptions.minDistBetweenLaserPoints =
306 localMapPoints.insertionOptions.isPlanarMap =
true;
310 map_to_align_to = grid.get();
313 else if (options.pfOptimalProposal_mapSelection == 3)
319 if (!built_map_points)
321 built_map_points =
true;
323 localMapPoints.insertionOptions.minDistBetweenLaserPoints =
325 localMapPoints.insertionOptions.isPlanarMap =
true;
329 map_to_align_to = ptsMap.get();
339 built_map_lms =
true;
343 map_to_align_to = lmMap.get();
346 ASSERT_(map_to_align_to !=
nullptr);
351 map_to_align_to, &localMapPoints,
352 CPose2D(initialPoseEstimation),
nullptr, &icpInfo);
356 if (i == particleWithHighestW)
358 newInfoIndex = 1 - icpInfo.
goodness;
367 if (icpInfo.
goodness < options.ICPGlobalAlign_MinQuality &&
371 "[rbpf-slam] Warning: gridICP[%u]: %.02f%% -> Using " 372 "odometry instead!\n",
373 (
unsigned int)i, 100 * icpInfo.
goodness);
374 icpEstimation.
mean =
CPose2D(initialPoseEstimation);
394 #if 0 // Use hacked ICP covariance: 395 CPose3D Ap = finalEstimatedPoseGauss.
mean - ith_last_pose;
396 const double Ap_dist = Ap.
norm();
399 finalEstimatedPoseGauss.
cov(0,0) =
square( fabs(Ap_dist)*0.01 );
400 finalEstimatedPoseGauss.
cov(1,1) =
square( fabs(Ap_dist)*0.01 );
401 finalEstimatedPoseGauss.
cov(2,2) =
square( fabs(Ap.
yaw())*0.02 );
414 finalEstimatedPoseGauss.
mean;
416 rndSamples, finalEstimatedPoseGauss.
cov);
418 finalPose.setFromValues(
419 finalPose.x() + rndSamples[0], finalPose.y() + rndSamples[1],
420 finalPose.z(), finalPose.yaw() + rndSamples[2],
421 finalPose.pitch(), finalPose.roll());
423 else if (options.pfOptimalProposal_mapSelection == 2)
428 auto beacMap = partIt->d->mapTillNow.mapByClass<
CBeaconMap>();
431 updateStageAlreadyDone =
true;
443 bool methodSOGorGrid =
false;
445 float firstEstimateRobotHeading = std::numeric_limits<float>::max();
448 CPoint3D centerPositionPrior(ith_last_pose);
449 float centerPositionPriorRadius = 2.0f;
453 firstEstimateRobotHeading = ith_last_pose.
yaw();
458 if (!beacMap->size())
461 cerr <<
"[RO-SLAM] Optimal filtering without map & " 462 "odometry...this message should appear only the " 471 if (beacMap->get(0).m_typePDF == CBeacon::pdfSOG)
473 cerr <<
"[RO-SLAM] Optimal filtering without map & " 474 "odometry->FIXING ONE BEACON!" 476 ASSERT_(beacMap->get(0).m_locationSOG.size() > 0);
478 beacMap->get(0).m_locationSOG[0].val.mean);
481 beacMap->get(0).m_typePDF = CBeacon::pdfGauss;
482 beacMap->get(0).m_locationSOG.clear();
483 beacMap->get(0).m_locationGauss.mean = fixedBeacon;
484 beacMap->get(0).m_locationGauss.cov.setDiagonal(
492 deque<TAuxRangeMeasInfo> lstObservedRanges;
494 for (
const auto& itObs : *sf)
501 deque<CObservationBeaconRanges::TMeasurement>::
502 const_iterator itRanges;
503 for (itRanges = obs->sensedData.begin();
504 itRanges != obs->sensedData.end(); itRanges++)
508 for (
auto itBeacs = beacMap->begin();
509 itBeacs != beacMap->end(); ++itBeacs)
511 if ((itBeacs)->m_ID == itRanges->beaconID)
514 newMeas.
beaconID = itRanges->beaconID;
516 itRanges->sensedDistance;
518 itRanges->sensorLocationOnRobot;
521 (itBeacs)->m_typePDF == CBeacon::pdfGauss ||
522 (itBeacs)->m_typePDF == CBeacon::pdfSOG);
524 (itBeacs)->m_typePDF == CBeacon::pdfSOG
525 ? (itBeacs)->m_locationSOG.size()
528 lstObservedRanges.push_back(newMeas);
541 lstObservedRanges.begin(), lstObservedRanges.end(),
548 fusedObsModels.
clear();
565 firstEstimateRobotHeading = auxPose.
yaw();
577 newMode.
val.
cov = poseCOV;
583 for (
auto itBeacs = beacMap->begin(); itBeacs != beacMap->end();
588 for (
auto& lstObservedRange : lstObservedRanges)
590 if ((itBeacs)->m_ID == lstObservedRange.beaconID)
593 float sensedRange = lstObservedRange.sensedDistance;
596 (itBeacs)->generateObservationModelDistribution(
597 sensedRange, newObsModel,
600 .sensorLocationOnRobot,
603 centerPositionPrior, centerPositionPriorRadius);
605 if (!fusedObsModels.
size())
609 fusedObsModels = newObsModel;
616 fusedObsModels, newObsModel,
619 fusedObsModels = tempFused;
626 cout <<
"#modes bef: " << fusedObsModels.
size()
627 <<
" ESS: " << fusedObsModels.
ESS()
629 double max_w = -1e100;
634 for (it = fusedObsModels.
begin();
635 it != fusedObsModels.
end(); it++)
637 max(max_w, (it)->log_w);
641 for (it = fusedObsModels.
begin();
642 it != fusedObsModels.
end();)
644 if (max_w - (it)->log_w >
646 it = fusedObsModels.
erase(it);
652 <<
"#modes after: " << fusedObsModels.
size()
661 currentCov(0, 0) > 0 && currentCov(1, 1) > 0);
662 if (sqrt(currentCov(0, 0)) < 0.10f &&
663 sqrt(currentCov(1, 1)) < 0.10f &&
664 sqrt(currentCov(2, 2)) < 0.10f)
668 fusedObsModels.
getMean(currentMean);
669 fusedObsModels[0].log_w = 0;
670 fusedObsModels[0].val.mean = currentMean;
671 fusedObsModels[0].val.cov = currentCov;
711 float grid_min_x = ith_last_pose.
x() - 0.5f;
712 float grid_max_x = ith_last_pose.
x() + 0.5f;
713 float grid_min_y = ith_last_pose.
y() - 0.5f;
714 float grid_max_y = ith_last_pose.
y() + 0.5f;
715 float grid_resXY = 0.02f;
717 bool repeatGridCalculation =
false;
722 grid_min_x, grid_max_x, grid_min_y, grid_max_y,
723 grid_resXY,
DEG2RAD(180), 0, 0);
725 pdfGrid->uniformDistribution();
729 for (
auto itBeacs = beacMap->begin();
730 itBeacs != beacMap->end(); ++itBeacs)
734 for (
auto& lstObservedRange : lstObservedRanges)
736 if ((itBeacs)->m_ID == lstObservedRange.beaconID)
740 lstObservedRange.sensedDistance;
756 for (
size_t idxX = 0;
757 idxX < pdfGrid->getSizeX(); idxX++)
759 float grid_x = pdfGrid->idx2x(idxX);
760 for (
size_t idxY = 0;
761 idxY < pdfGrid->getSizeY(); idxY++)
763 double grid_y = pdfGrid->idx2y(idxY);
767 pdfGrid->getByIndex(idxX, idxY, 0);
772 switch ((itBeacs)->m_typePDF)
774 case CBeacon::pdfSOG:
777 &(itBeacs)->m_locationSOG;
778 double sensorSTD2 =
square(
779 beacMap->likelihoodOptions
783 for (it = sog->
begin();
784 it != sog->
end(); it++)
790 (it)->
val.mean.distance2DTo(
793 .sensorLocationOnRobot
797 .sensorLocationOnRobot
811 pdfGrid->normalize();
821 float maxX = 0, maxY = 0;
822 for (
size_t idxX = 0; idxX < pdfGrid->getSizeX();
826 for (
size_t idxY = 0; idxY < pdfGrid->getSizeY();
832 float c = *pdfGrid->getByIndex(idxX, idxY, 0);
836 maxX = pdfGrid->idx2x(idxX);
837 maxY = pdfGrid->idx2y(idxY);
841 newDrawnPosition.
x(maxX);
842 newDrawnPosition.
y(maxY);
849 pdfGrid->getAsMatrix(0, outMat );
851 CImage imgF(outMat,
true);
852 static int autocount=0;
853 imgF.saveToFile(
format(
"debug_grid_%f_%05i.png",grid_resXY,autocount++));
854 printf(
"grid res: %f MAX: %f,%f\n",grid_resXY,maxX,maxY);
859 if (grid_resXY > 0.01f)
861 grid_min_x = maxX - 0.03f;
862 grid_max_x = maxX + 0.03f;
863 grid_min_y = maxY - 0.03f;
864 grid_max_y = maxY + 0.03f;
866 repeatGridCalculation =
true;
869 repeatGridCalculation =
false;
885 }
while (repeatGridCalculation);
894 if (!beacMap->size())
898 finalPose = ith_last_pose;
902 cout <<
"Drawn: " << newDrawnPosition << endl;
908 firstEstimateRobotHeading !=
909 std::numeric_limits<float>::max());
912 newDrawnPosition.
x(), newDrawnPosition.
y(),
913 newDrawnPosition.z(), firstEstimateRobotHeading, 0, 0);
924 double weightUpdate = 0;
925 partIt->log_w += PF_options.
powFactor * weightUpdate;
934 "Action list does not contain any CActionRobotMovement2D " 935 "or CActionRobotMovement3D object!");
939 finalPose = ith_last_pose + incrPose;
943 partIt->d->robotPath.push_back(finalPose.
asTPose());
948 if (!updateStageAlreadyDone)
951 (PF_SLAM_computeObservationLikelihoodForParticle(
952 PF_options, i, *sf, finalPose) +
966 void CMultiMetricMapPDF::prediction_and_update_pfStandardProposal(
973 PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
974 actions, sf, PF_options, options.KLD_params);
977 averageMapIsUpdated =
false;
983 void CMultiMetricMapPDF::
984 PF_SLAM_implementation_custom_update_particle_with_new_pose(
987 particleData->
robotPath.push_back(newPose);
991 bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
992 const CMultiMetricMapPDF::CParticleList& particles,
995 if (sf ==
nullptr)
return false;
997 return particles.begin()
999 ->mapTillNow.canComputeObservationsLikelihood(*sf);
1003 bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement()
const 1005 return 0 == getNumberOfObservationsInSimplemap();
1012 double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
1014 const size_t particleIndexForMap,
const CSensoryFrame& observation,
1019 &m_particles[particleIndexForMap].d->mapTillNow);
1021 for (
const auto& it : observation)
Scalar maxCoeff() const
Maximum value in the matrix/vector.
A namespace of pseudo-random numbers generators of diferent distributions.
mrpt::math::TPose3D asTPose() const
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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
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 ...
double DEG2RAD(const double x)
Degrees to radians.
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...
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
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).
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.
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
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.
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...
A class for storing a map of 3D probabilistic landmarks.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
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 x() const
Common members of all points & poses classes.
double norm() const
Returns the euclidean norm of vector: .
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.
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.
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 ...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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 ...
double log_w
The log-weight.
std::vector< TPoseBin2D > bins
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 ...
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...
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.
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).
GLubyte GLubyte GLubyte a
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
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
int round(const T value)
Returns the closer integer (int) to x.