48 const bayes::CParticleFilter::TParticleFilterOptions& opts,
51 : averageMap(mapsInitializers), SFs(), SF2robotPath(), options()
53 m_particles.resize(opts.sampleSize);
54 for (
auto& m_particle : m_particles)
61 const CPose3D nullPose(0, 0, 0);
65 options = predictionOptions;
82 const size_t M = m_particles.size();
83 for (
size_t i = 0; i < M; i++)
85 m_particles[i].log_w = 0;
87 m_particles[i].d->mapTillNow.clear();
89 m_particles[i].d->robotPath.resize(1);
90 m_particles[i].d->robotPath[0] = initialPose.
asTPose();
96 averageMapIsUpdated =
false;
103 const size_t nParts = m_particles.size(), nOldKeyframes = prevMap.
size();
104 if (nOldKeyframes == 0)
110 for (
size_t idxPart = 0; idxPart < nParts; idxPart++)
112 auto& p = m_particles[idxPart];
115 p.d->mapTillNow.clear();
117 p.d->robotPath.resize(nOldKeyframes);
118 for (
size_t i = 0; i < nOldKeyframes; i++)
122 prevMap.
get(i, keyframe_pose, sfkeyframe_sf);
129 bool kf_pose_set =
false;
133 keyframe_pose.get());
135 if (pdf_parts->particlesCount() == nParts)
137 kf_pose =
CPose3D(pdf_parts->m_particles[idxPart].d);
143 kf_pose = keyframe_pose->getMeanVal();
145 p.d->robotPath[i] = kf_pose.
asTPose();
146 for (
const auto& obs : *sfkeyframe_sf)
148 p.d->mapTillNow.insertObservation(*obs, &kf_pose);
154 SF2robotPath.
clear();
155 SF2robotPath.reserve(nOldKeyframes);
156 for (
size_t i = 0; i < nOldKeyframes; i++) SF2robotPath.push_back(i);
158 averageMapIsUpdated =
false;
167 void CMultiMetricMapPDF::getEstimatedPosePDF(
170 ASSERT_(m_particles[0].d->robotPath.size() > 0);
171 getEstimatedPosePDFAtTime(
172 m_particles[0].d->robotPath.size() - 1, out_estimation);
178 void CMultiMetricMapPDF::getEstimatedPosePDFAtTime(
182 size_t i, n = m_particles.size();
189 for (i = 0; i < n; i++)
191 out_estimation.
m_particles[i].d = m_particles[i].d->robotPath[timeStep];
192 out_estimation.
m_particles[i].log_w = m_particles[i].log_w;
196 uint8_t CRBPFParticleData::serializeGetVersion()
const {
return 0; }
206 uint8_t CMultiMetricMapPDF::serializeGetVersion()
const {
return 0; }
211 out.WriteAs<uint32_t>(m_particles.size());
212 for (
const auto& part : m_particles)
215 out << part.d->mapTillNow;
216 out.WriteAs<uint32_t>(part.d->robotPath.size());
217 for (
const auto& p : part.d->robotPath)
out << p;
219 out << SFs << SF2robotPath;
222 void CMultiMetricMapPDF::serializeFrom(
235 SF2robotPath.clear();
237 averageMapIsUpdated =
false;
243 m_particles.resize(n);
244 for (i = 0; i < n; i++)
249 in >> m_particles[i].log_w >> m_particles[i].d->mapTillNow;
252 m_particles[i].d->robotPath.resize(m);
253 for (j = 0; j < m; j++) in >> m_particles[i].d->robotPath[j];
256 in >> SFs >> SF2robotPath;
265 const size_t i,
bool& is_valid_pose)
const
267 if (i >= m_particles.size())
270 if (m_particles[i].d->robotPath.empty())
272 is_valid_pose =
false;
273 return TPose3D(0, 0, 0, 0, 0, 0);
277 return *m_particles[i].d->robotPath.rbegin();
287 void CMultiMetricMapPDF::rebuildAverageMap()
289 float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
291 if (averageMapIsUpdated)
return;
296 for (
auto& p : m_particles)
302 min_x = min(min_x, grid->
getXMin());
303 max_x = max(max_x, grid->getXMax());
304 min_y = min(min_y, grid->getYMin());
305 max_y = max(max_y, grid->getYMax());
309 for (
auto& p : m_particles)
312 grid->
resizeGrid(min_x, max_x, min_y, max_y, 0.5f,
false);
315 float grid_resolution = 0.1f;
316 for (
auto& p : m_particles)
321 min_x = min(min_x, grid->getXMin());
322 max_x = max(max_x, grid->getXMax());
323 min_y = min(min_y, grid->getYMin());
324 max_y = max(max_y, grid->getYMax());
330 avrg_grid->
setSize(min_x, max_x, min_y, max_y, grid_resolution, 0);
333 double sumLinearWeights = 0;
334 for (
auto& p : m_particles) sumLinearWeights += exp(p.log_w);
337 for (
auto& p : m_particles)
340 ASSERT_(grid->getSizeX() == avrg_grid->getSizeX());
341 ASSERT_(grid->getSizeY() == avrg_grid->getSizeY());
352 std::vector<float> floatMap;
353 floatMap.resize(avrg_grid->map.size(), 0);
357 for (
auto& p : m_particles) sumW += exp(p.log_w);
359 if (sumW == 0) sumW = 1;
361 for (
auto& p : m_particles)
365 std::vector<COccupancyGridMap2D::cellType>::iterator srcCell;
366 auto firstSrcCell = grid->
map.begin();
367 auto lastSrcCell = grid->map.end();
368 std::vector<float>::iterator destCell;
371 float w = exp(p.log_w) / sumW;
373 ASSERT_(grid->map.size() == floatMap.size());
376 for (srcCell = firstSrcCell, destCell = floatMap.begin();
377 srcCell != lastSrcCell; srcCell++, destCell++)
378 (*destCell) += w * (*srcCell);
382 std::vector<float>::iterator srcCell;
383 auto destCell = avrg_grid->map.begin();
385 ASSERT_(avrg_grid->map.size() == floatMap.size());
387 for (srcCell = floatMap.begin(); srcCell != floatMap.end();
388 srcCell++, destCell++)
395 averageMapIsUpdated =
true;
403 const size_t M = particlesCount();
407 getEstimatedPosePDF(*posePDF);
410 const uint32_t new_sf_id = SFs.size();
411 SFs.insert(posePDF, CSensoryFrame::Create(sf));
412 SF2robotPath.resize(new_sf_id + 1);
413 SF2robotPath[new_sf_id] = m_particles[0].d->robotPath.size() - 1;
416 for (
size_t i = 0; i < M; i++)
422 &m_particles[i].d->mapTillNow, &robotPose);
423 anymap = anymap || map_modified;
426 averageMapIsUpdated =
false;
433 void CMultiMetricMapPDF::getPath(
434 size_t i, std::deque<math::TPose3D>& out_path)
const
437 out_path = m_particles[i].d->robotPath;
443 double CMultiMetricMapPDF::getCurrentEntropyOfPaths()
447 m_particles[0].d->robotPath.size();
456 for (i = 0; i < N; i++)
460 getEstimatedPosePDFAtTime(i, posePDFParts);
473 double CMultiMetricMapPDF::getCurrentJointEntropy()
475 double H_joint, H_paths, H_maps;
479 H_paths = getCurrentEntropyOfPaths();
481 float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
484 for (
auto& p : m_particles)
489 min_x = min(min_x, grid->getXMin());
490 max_x = max(max_x, grid->getXMax());
491 min_y = min(min_y, grid->getYMin());
492 max_y = max(max_y, grid->getYMax());
496 for (
auto& p : m_particles)
499 grid->
resizeGrid(min_x, max_x, min_y, max_y, 0.5f,
false);
503 double sumLinearWeights = 0;
504 for (
auto& p : m_particles) sumLinearWeights += exp(p.log_w);
509 for (
auto& p : m_particles)
514 H_maps += exp(p.log_w) * entropy.
H / sumLinearWeights;
517 printf(
"H_paths=%e\n", H_paths);
518 printf(
"H_maps=%e\n", H_maps);
520 H_joint = H_paths + H_maps;
526 size_t i, max_i = 0, n = m_particles.size();
527 double max_w = m_particles[0].log_w;
529 for (i = 0; i < n; i++)
531 if (m_particles[i].log_w > max_w)
533 max_w = m_particles[i].log_w;
539 return &m_particles[max_i].d->mapTillNow;
545 void CMultiMetricMapPDF::updateSensoryFrameSequence()
552 for (
size_t i = 0; i < SFs.size(); i++)
555 SFs.get(i, previousPosePDF, dummy);
558 getEstimatedPosePDFAtTime(SF2robotPath[i], posePartsPDF);
561 previousPosePDF->copyFrom(posePartsPDF);
570 void CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(
571 const std::string& fil)
576 for (
auto& m_particle : m_particles)
578 for (
size_t i = 0; i < m_particle.d->robotPath.size(); i++)
583 f,
"%.04f %.04f %.04f %.04f %.04f %.04f ", p.
x, p.
y, p.
z, p.
yaw,
595 void CMultiMetricMapPDF::TPredictionParams::dumpToTextStream(
596 std::ostream&
out)
const
598 out <<
"\n----------- [CMultiMetricMapPDF::TPredictionParams] ------------ "
602 "pfOptimalProposal_mapSelection = %i\n",
603 pfOptimalProposal_mapSelection);
605 "ICPGlobalAlign_MinQuality = %f\n",
606 ICPGlobalAlign_MinQuality);
608 KLD_params.dumpToTextStream(
out);
609 icp_params.dumpToTextStream(
out);
616 void CMultiMetricMapPDF::TPredictionParams::loadFromConfigFile(
619 pfOptimalProposal_mapSelection =
iniFile.read_int(
620 section,
"pfOptimalProposal_mapSelection",
621 pfOptimalProposal_mapSelection,
true);
625 KLD_params.loadFromConfigFile(
iniFile, section);
626 icp_params.loadFromConfigFile(
iniFile, section);