34 m_particles.resize(M);
35 for (
auto& p : m_particles)
41 resetDeterministic(nullPose);
44 void CPosePDFParticles::copyFrom(
const CPosePDF& o)
48 CParticleList::iterator itDest;
49 CParticleList::const_iterator itSrc;
51 if (
this == &o)
return;
59 m_particles = pdf->m_particles;
64 size_t M = m_particles.size();
65 std::vector<CVectorDouble> parts;
66 std::vector<CVectorDouble>::iterator partsIt;
71 m_particles.resize(M);
73 for (itDest = m_particles.begin(), partsIt = parts.begin();
74 itDest != m_particles.end(); ++itDest, ++partsIt)
78 pdf->mean.x() + (*partsIt)[0], (pdf->mean.y() + (*partsIt)[1]),
79 (pdf->mean.phi() + (*partsIt)[2]));
80 itDest->d.normalizePhi();
88 void CPosePDFParticles::getMean(
CPose2D& est_)
const
91 const size_t n = m_particles.size();
95 for (
size_t i = 0; i < n; i++)
97 double w = exp(m_particles[i].log_w);
98 se_averager.
append(m_particles[i].d, w);
108 std::tuple<CMatrixDouble33, CPose2D> CPosePDFParticles::getCovarianceAndMean()
116 size_t i, n = m_particles.size();
117 double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;
118 double mean_phi =
mean.phi();
120 if (mean_phi < 0) mean_phi =
M_2PI + mean_phi;
122 double lin_w_sum = 0;
124 for (i = 0; i < n; i++) lin_w_sum += exp(m_particles[i].log_w);
125 if (lin_w_sum == 0) lin_w_sum = 1;
127 for (i = 0; i < n; i++)
129 double w = exp(m_particles[i].log_w) / lin_w_sum;
132 double err_x = m_particles[i].d.x -
mean.x();
133 double err_y = m_particles[i].d.y -
mean.y();
134 double err_phi =
math::wrapToPi(fabs(m_particles[i].d.phi - mean_phi));
136 var_x +=
square(err_x) * w;
137 var_y +=
square(err_y) * w;
138 var_p +=
square(err_phi) * w;
139 var_xy += err_x * err_y * w;
140 var_xp += err_x * err_phi * w;
141 var_yp += err_y * err_phi * w;
155 cov(1, 0) =
cov(0, 1) = var_xy;
156 cov(2, 0) =
cov(0, 2) = var_xp;
157 cov(1, 2) =
cov(2, 1) = var_yp;
162 uint8_t CPosePDFParticles::serializeGetVersion()
const {
return 1; }
165 writeParticlesToStream(
out);
167 void CPosePDFParticles::serializeFrom(
177 old.readParticlesFromStream(in);
180 old.m_particles.begin(), old.m_particles.end(),
181 std::back_inserter(m_particles),
183 return CParticleData(p.d.asTPose(), p.log_w);
189 readParticlesFromStream(in);
197 void CPosePDFParticles::resetDeterministic(
198 const TPose2D& location,
size_t particlesCount)
200 if (particlesCount > 0) m_particles.resize(particlesCount);
202 for (
auto& p : m_particles)
209 void CPosePDFParticles::resetUniform(
210 const double x_min,
const double x_max,
const double y_min,
211 const double y_max,
const double phi_min,
const double phi_max,
212 const int particlesCount)
215 if (particlesCount > 0) m_particles.resize(particlesCount);
217 for (
auto& p : m_particles)
227 void CPosePDFParticles::resetAroundSetOfPoses(
228 const std::vector<mrpt::math::TPose2D>& list_poses,
229 const size_t num_particles_per_pose,
const double spread_x,
230 const double spread_y,
const double spread_phi_rad)
234 ASSERT_(num_particles_per_pose >= 1);
236 const size_t N = list_poses.size() * num_particles_per_pose;
239 m_particles.resize(N);
241 for (i = 0, nSpot = 0; nSpot < list_poses.size(); nSpot++)
244 for (
size_t k = 0; k < num_particles_per_pose; k++, i++)
247 p.
x - spread_x * 0.5, p.
x + spread_x * 0.5);
249 p.
y - spread_y * 0.5, p.
y + spread_y * 0.5);
251 p.
phi - spread_phi_rad * 0.5, p.
phi + spread_phi_rad * 0.5);
252 m_particles[i].log_w = 0;
259 bool CPosePDFParticles::saveToTextFile(
const std::string& file)
const
262 buf +=
"%% x y yaw[rad] log_weight\n";
264 for (
const auto& p : m_particles)
265 buf +=
mrpt::format(
"%f %f %f %e\n", p.d.x, p.d.y, p.d.phi, p.log_w);
267 std::ofstream f(file);
268 if (!f.is_open())
return false;
273 TPose2D CPosePDFParticles::getParticlePose(
size_t i)
const
275 return m_particles[i].d;
278 void CPosePDFParticles::changeCoordinatesReference(
279 const CPose3D& newReferenceBase_)
282 for (
auto& p : m_particles) p.d = newReferenceBase + p.d;
285 void CPosePDFParticles::drawSingleSample(
CPose2D& outPart)
const
290 for (
auto& p : m_particles)
301 outPart =
CPose2D(m_particles.rbegin()->d);
306 for (
auto& p : m_particles) p.d = p.d + Ap;
311 for (
auto& p : o.
m_particles) m_particles.emplace_back(p);
321 out->copyFrom(*
this);
324 for (
auto& p :
out->m_particles) p.d = nullPose - p.d;
332 double max_w = -std::numeric_limits<double>::max();
333 for (
const auto& p : m_particles)
344 void CPosePDFParticles::bayesianFusion(
346 [[maybe_unused]]
const double minMahalanobisDistToDrop)
351 double CPosePDFParticles::evaluatePDF_parzen(
352 const double x,
const double y,
const double phi,
const double stdXY,
353 const double stdPhi)
const
356 for (
const auto& p : m_particles)
359 ret += exp(p.log_w) *
361 std::sqrt(
square(p.d.x - x) +
square(p.d.y - y)), 0, stdXY) *
367 void CPosePDFParticles::saveParzenPDFToTextFile(
368 const char* fileName,
const double x_min,
const double x_max,
369 const double y_min,
const double y_max,
const double phi,
370 const double stepSizeXY,
const double stdXY,
const double stdPhi)
const
374 for (
double y = y_min; y < y_max; y += stepSizeXY)
375 for (
double x = x_min; x < x_max; x += stepSizeXY)
377 "%f ", evaluatePDF_parzen(x, y, phi, stdXY, stdPhi));
380 std::ofstream f(fileName);
381 if (!f.is_open())
return;