46 const bayes::CParticleFilter::TParticleFilterOptions& opts,
49 : averageMap(mapsInitializers),
50 averageMapIsUpdated(false),
56 m_particles.resize(opts.sampleSize);
58 it != m_particles.end(); ++it)
65 const CPose3D nullPose(0, 0, 0);
69 if (predictionOptions !=
nullptr) options = *predictionOptions;
86 const size_t M = m_particles.size();
87 for (
size_t i = 0; i < M; i++)
89 m_particles[i].log_w = 0;
91 m_particles[i].d->mapTillNow.clear();
93 m_particles[i].d->robotPath.resize(1);
94 m_particles[i].d->robotPath[0] = initialPose.
asTPose();
100 averageMapIsUpdated =
false;
107 const size_t nParts = m_particles.size(), nOldKeyframes = prevMap.
size();
108 if (nOldKeyframes == 0)
114 for (
size_t idxPart = 0; idxPart < nParts; idxPart++)
116 auto&
p = m_particles[idxPart];
119 p.d->mapTillNow.clear();
121 p.d->robotPath.resize(nOldKeyframes);
122 for (
size_t i = 0; i < nOldKeyframes; i++)
126 prevMap.
get(i, keyframe_pose, sfkeyframe_sf);
133 bool kf_pose_set =
false;
137 keyframe_pose.get());
139 if (pdf_parts->particlesCount() == nParts)
141 kf_pose =
CPose3D(pdf_parts->m_particles[idxPart].d);
147 kf_pose = keyframe_pose->getMeanVal();
149 p.d->robotPath[i] = kf_pose.
asTPose();
150 for (
const auto& obs : *sfkeyframe_sf)
152 p.d->mapTillNow.insertObservation(&(*obs), &kf_pose);
158 SF2robotPath.
clear();
159 SF2robotPath.reserve(nOldKeyframes);
160 for (
size_t i = 0; i < nOldKeyframes; i++) SF2robotPath.push_back(i);
162 averageMapIsUpdated =
false;
171 void CMultiMetricMapPDF::getEstimatedPosePDF(
174 ASSERT_(m_particles[0].d->robotPath.size() > 0);
175 getEstimatedPosePDFAtTime(
176 m_particles[0].d->robotPath.size() - 1, out_estimation);
182 void CMultiMetricMapPDF::getEstimatedPosePDFAtTime(
186 size_t i,
n = m_particles.size();
193 for (i = 0; i <
n; i++)
195 out_estimation.
m_particles[i].d = m_particles[i].d->robotPath[timeStep];
196 out_estimation.
m_particles[i].log_w = m_particles[i].log_w;
200 uint8_t CRBPFParticleData::serializeGetVersion()
const {
return 0; }
210 uint8_t CMultiMetricMapPDF::serializeGetVersion()
const {
return 0; }
214 for (
const auto& part : m_particles)
216 out << part.log_w << part.d->mapTillNow;
218 for (
const auto&
p : part.d->robotPath) out <<
p;
220 out << SFs << SF2robotPath;
223 void CMultiMetricMapPDF::serializeFrom(
236 SF2robotPath.clear();
238 averageMapIsUpdated =
false;
244 m_particles.resize(
n);
245 for (i = 0; i <
n; i++)
250 in >> m_particles[i].log_w >> m_particles[i].d->mapTillNow;
253 m_particles[i].d->robotPath.resize(m);
254 for (j = 0; j < m; j++) in >> m_particles[i].d->robotPath[j];
257 in >> SFs >> SF2robotPath;
266 const size_t i,
bool& is_valid_pose)
const
268 if (i >= m_particles.size())
271 if (m_particles[i].d->robotPath.empty())
273 is_valid_pose =
false;
274 return TPose3D(0, 0, 0, 0, 0, 0);
278 return *m_particles[i].d->robotPath.rbegin();
291 void CMultiMetricMapPDF::rebuildAverageMap()
293 float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
296 if (averageMapIsUpdated)
return;
301 for (part = m_particles.begin(); part != m_particles.end(); ++part)
303 ASSERT_(part->d->mapTillNow.m_gridMaps.size() > 0);
305 min_x =
min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
306 max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
307 min_y =
min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
308 max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
312 for (part = m_particles.begin(); part != m_particles.end(); ++part)
313 part->d->mapTillNow.m_gridMaps[0]->resizeGrid(
314 min_x, max_x, min_y, max_y, 0.5f,
false);
316 for (part = m_particles.begin(); part != m_particles.end(); ++part)
318 min_x =
min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
319 max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
320 min_y =
min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
321 max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
325 ASSERT_(averageMap.m_gridMaps.size() > 0);
326 averageMap.m_gridMaps[0]->setSize(
327 min_x, max_x, min_y, max_y,
328 m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution(), 0);
331 double sumLinearWeights = 0;
332 for (part = m_particles.begin(); part != m_particles.end(); ++part)
333 sumLinearWeights += exp(part->log_w);
336 for (part = m_particles.begin(); part != m_particles.end(); ++part)
339 part->d->mapTillNow.m_gridMaps[0]->getSizeX() ==
340 averageMap.m_gridMaps[0]->getSizeX());
342 part->d->mapTillNow.m_gridMaps[0]->getSizeY() ==
343 averageMap.m_gridMaps[0]->getSizeY());
354 std::vector<float> floatMap;
355 floatMap.resize(averageMap.m_gridMaps[0]->map.size(), 0);
359 for (part = m_particles.begin(); part != m_particles.end(); ++part)
360 sumW += exp(part->log_w);
362 if (sumW == 0) sumW = 1;
364 for (part = m_particles.begin(); part != m_particles.end(); ++part)
369 part->d->mapTillNow.m_gridMaps[0]->map.begin();
371 part->d->mapTillNow.m_gridMaps[0]->map.end();
375 float w = exp(part->log_w) / sumW;
378 part->d->mapTillNow.m_gridMaps[0]->map.size() ==
382 for (srcCell = firstSrcCell, destCell = floatMap.begin();
383 srcCell != lastSrcCell; srcCell++, destCell++)
384 (*destCell) +=
w * (*srcCell);
390 averageMap.m_gridMaps[0]->map.begin();
392 ASSERT_(averageMap.m_gridMaps[0]->map.size() == floatMap.size());
394 for (srcCell = floatMap.begin(); srcCell != floatMap.end();
395 srcCell++, destCell++)
402 averageMapIsUpdated =
true;
410 const size_t M = particlesCount();
414 mrpt::make_aligned_shared<CPose3DPDFParticles>();
415 getEstimatedPosePDF(*posePDF);
418 const uint32_t new_sf_id = SFs.size();
420 SF2robotPath.resize(new_sf_id + 1);
421 SF2robotPath[new_sf_id] = m_particles[0].d->robotPath.size() - 1;
424 for (
size_t i = 0; i < M; i++)
430 &m_particles[i].d->mapTillNow, &robotPose);
431 anymap = anymap || map_modified;
434 averageMapIsUpdated =
false;
441 void CMultiMetricMapPDF::getPath(
442 size_t i, std::deque<math::TPose3D>& out_path)
const
445 out_path = m_particles[i].d->robotPath;
451 double CMultiMetricMapPDF::getCurrentEntropyOfPaths()
455 m_particles[0].d->robotPath.size();
464 for (i = 0; i < N; i++)
468 getEstimatedPosePDFAtTime(i, posePDFParts);
481 double CMultiMetricMapPDF::getCurrentJointEntropy()
483 double H_joint, H_paths, H_maps;
484 size_t i, M = m_particles.size();
488 H_paths = getCurrentEntropyOfPaths();
490 float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
496 for (part = m_particles.begin(); part != m_particles.end(); ++part)
498 ASSERT_(part->d->mapTillNow.m_gridMaps.size() > 0);
500 min_x =
min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
501 max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
502 min_y =
min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
503 max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
507 for (part = m_particles.begin(); part != m_particles.end(); ++part)
508 part->d->mapTillNow.m_gridMaps[0]->resizeGrid(
509 min_x, max_x, min_y, max_y, 0.5f,
false);
512 double sumLinearWeights = 0;
513 for (i = 0; i < M; i++) sumLinearWeights += exp(m_particles[i].log_w);
518 for (i = 0; i < M; i++)
520 ASSERT_(m_particles[i].d->mapTillNow.m_gridMaps.size() > 0);
522 m_particles[i].d->mapTillNow.m_gridMaps[0]->computeEntropy(entropy);
523 H_maps += exp(m_particles[i].log_w) * entropy.
H / sumLinearWeights;
526 printf(
"H_paths=%e\n", H_paths);
527 printf(
"H_maps=%e\n", H_maps);
529 H_joint = H_paths + H_maps;
535 size_t i, max_i = 0,
n = m_particles.size();
536 double max_w = m_particles[0].log_w;
538 for (i = 0; i <
n; i++)
540 if (m_particles[i].log_w > max_w)
542 max_w = m_particles[i].log_w;
548 return &m_particles[max_i].d->mapTillNow;
554 void CMultiMetricMapPDF::updateSensoryFrameSequence()
561 for (
size_t i = 0; i < SFs.size(); i++)
564 SFs.get(i, previousPosePDF, dummy);
567 getEstimatedPosePDFAtTime(SF2robotPath[i], posePartsPDF);
570 previousPosePDF->copyFrom(posePartsPDF);
579 void CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(
586 it != m_particles.end(); ++it)
588 for (
size_t i = 0; i < it->d->robotPath.size(); i++)
593 f,
"%.04f %.04f %.04f %.04f %.04f %.04f ",
p.x,
p.y,
p.z,
p.yaw,
605 CMultiMetricMapPDF::TPredictionParams::TPredictionParams()
606 : pfOptimalProposal_mapSelection(0),
607 ICPGlobalAlign_MinQuality(0.70f),
608 update_gridMapLikelihoodOptions(),
618 std::ostream& out)
const
621 "\n----------- [CMultiMetricMapPDF::TPredictionParams] ------------ "
625 "pfOptimalProposal_mapSelection = %i\n",
626 pfOptimalProposal_mapSelection);
628 "ICPGlobalAlign_MinQuality = %f\n",
629 ICPGlobalAlign_MinQuality);
631 KLD_params.dumpToTextStream(out);
632 icp_params.dumpToTextStream(out);
642 pfOptimalProposal_mapSelection =
iniFile.read_int(
643 section,
"pfOptimalProposal_mapSelection",
644 pfOptimalProposal_mapSelection,
true);
648 KLD_params.loadFromConfigFile(
iniFile, section);
649 icp_params.loadFromConfigFile(
iniFile, section);