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;
309 map_to_align_to = grid.get();
312 else if (options.pfOptimalProposal_mapSelection == 3)
318 if (!built_map_points)
320 built_map_points =
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);
390 finalPose.
x() + rndSamples[0], finalPose.
y() + rndSamples[1],
391 finalPose.z(), finalPose.
yaw() + rndSamples[2],
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)