21 #include <Eigen/Dense>
51 size_t N = m_modes.size();
52 double X = 0, Y = 0, Z = 0;
56 CListGaussianModes::const_iterator it;
59 for (it = m_modes.begin(); it != m_modes.end(); ++it)
62 sumW += w = exp(it->log_w);
63 X += it->val.mean.x() * w;
64 Y += it->val.mean.y() * w;
65 Z += it->val.mean.z() * w;
82 size_t N = m_modes.size();
95 CListGaussianModes::const_iterator it;
97 for (it = m_modes.begin(); it != m_modes.end(); ++it)
100 sumW += w = exp(it->log_w);
103 estMean_i -= estMean;
107 partCov += it->val.cov;
112 if (sumW != 0) estCov *= (1.0 / sumW);
121 uint32_t N = m_modes.size();
123 for (
const auto& m : m_modes)
141 for (
auto& m : m_modes)
146 if (version == 0) m.log_w = log(max(1e-300, m.log_w));
162 if (
this == &o)
return;
166 m_modes =
dynamic_cast<const CPointPDFSOG*
>(&o)->m_modes;
172 m_modes[0].log_w = 0;
185 if (!f)
return false;
187 for (
const auto& m_mode : m_modes)
189 f,
"%e %e %e %e %e %e %e %e %e %e\n", exp(m_mode.log_w),
190 m_mode.val.mean.x(), m_mode.val.mean.y(), m_mode.val.mean.z(),
191 m_mode.val.cov(0, 0), m_mode.val.cov(1, 1), m_mode.val.cov(2, 2),
192 m_mode.val.cov(0, 1), m_mode.val.cov(0, 2), m_mode.val.cov(1, 2));
202 for (
auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
215 vector<double> logWeights(m_modes.size());
216 vector<size_t> outIdxs;
217 vector<double>::iterator itW;
218 CListGaussianModes::const_iterator it;
219 for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
223 CParticleFilterCapable::computeResampling(
224 CParticleFilter::prMultinomial,
230 size_t selectedIdx = outIdxs[0];
231 ASSERT_(selectedIdx < m_modes.size());
239 outSample.
x(selMode->
mean.
x() + vec[0]);
240 outSample.
y(selMode->
mean.
y() + vec[1]);
241 outSample.z(selMode->
mean.z() + vec[2]);
251 const double minMahalanobisDistToDrop)
260 const auto* p1 =
dynamic_cast<const CPointPDFSOG*
>(&p1_);
261 const auto* p2 =
dynamic_cast<const CPointPDFSOG*
>(&p2_);
267 const double minMahalanobisDistToDrop2 =
square(minMahalanobisDistToDrop);
269 this->m_modes.clear();
273 for (
const auto& m : p1->m_modes)
284 ASSERT_(c(0, 0) != 0 && c(0, 0) != 0);
292 double a = -0.5 * (3 * log(
M_2PI) - log(covInv.
det()) +
293 (eta.transpose() * c.
asEigen() * eta)(0, 0));
295 for (
const auto& m2 : p2->m_modes)
297 auxSOG_Kernel_i = m2.val;
298 if (auxSOG_Kernel_i.
cov(2, 2) == 0)
300 auxSOG_Kernel_i.
cov(2, 2) = 1;
304 auxSOG_Kernel_i.
cov(0, 0) > 0 && auxSOG_Kernel_i.
cov(1, 1) > 0);
307 bool reallyComputeThisOne =
true;
308 if (minMahalanobisDistToDrop > 0)
311 double stdX2 = max(auxSOG_Kernel_i.
cov(0, 0), m.val.cov(0, 0));
313 square(auxSOG_Kernel_i.
mean.
x() - m.val.mean.x()) / stdX2;
315 double stdY2 = max(auxSOG_Kernel_i.
cov(1, 1), m.val.cov(1, 1));
317 square(auxSOG_Kernel_i.
mean.
y() - m.val.mean.y()) / stdY2;
322 max(auxSOG_Kernel_i.
cov(2, 2), m.val.cov(2, 2));
324 square(auxSOG_Kernel_i.
mean.z() - m.val.mean.z()) /
328 reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
331 if (reallyComputeThisOne)
346 newKernel.
val = auxGaussianProduct;
349 Eigen::Vector3d eta_i =
351 eta_i = covInv_i.
asEigen() * eta_i;
354 Eigen::Vector3d new_eta_i =
356 new_eta_i = new_covInv_i.
asEigen() * new_eta_i;
359 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.
det()) +
360 (eta_i.transpose() * auxSOG_Kernel_i.
cov.
asEigen() *
363 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.
det()) +
364 (new_eta_i.transpose() *
367 newKernel.
log_w = m.log_w + m2.log_w + a + a_i - new_a_i;
370 if (is2D) newKernel.
val.
cov(2, 2) = 0;
373 this->m_modes.push_back(newKernel);
392 for (
auto& m_mode : m_modes)
394 m_mode.val.cov(0, 1) = m_mode.val.cov(1, 0);
395 m_mode.val.cov(0, 2) = m_mode.val.cov(2, 0);
396 m_mode.val.cov(1, 2) = m_mode.val.cov(2, 1);
409 if (!m_modes.size())
return;
411 CListGaussianModes::iterator it;
412 double maxW = m_modes[0].log_w;
413 for (it = m_modes.begin(); it != m_modes.end(); ++it)
414 maxW = max(maxW, it->log_w);
416 for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;
427 CListGaussianModes::const_iterator it;
431 double sumLinearWeights = 0;
432 for (it = m_modes.begin(); it != m_modes.end(); ++it)
433 sumLinearWeights += exp(it->log_w);
436 for (it = m_modes.begin(); it != m_modes.end(); ++it)
437 cum +=
square(exp(it->log_w) / sumLinearWeights);
442 return 1.0 / (m_modes.size() * cum);
450 float x_min,
float x_max,
float y_min,
float y_max,
float resolutionXY,
451 float z,
CMatrixD& outMatrix,
bool sumOverAllZs)
459 const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
460 const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
463 for (
size_t i = 0; i < Ny; i++)
465 const float y = y_min + i * resolutionXY;
466 for (
size_t j = 0; j < Nx; j++)
468 float x = x_min + j * resolutionXY;
469 outMatrix(i, j) = evaluatePDF(
CPoint3D(x, y, z), sumOverAllZs);
489 for (
const auto& m_mode : m_modes)
500 CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
506 for (
const auto& m_mode : m_modes)
508 MU(0, 0) = m_mode.val.mean.x();
509 MU(1, 0) = m_mode.val.mean.y();
511 COV(0, 0) = m_mode.val.cov(0, 0);
512 COV(1, 1) = m_mode.val.cov(1, 1);
513 COV(0, 1) = COV(1, 0) = m_mode.val.cov(0, 1);
533 auto it_best = m_modes.end();
534 for (
auto it = m_modes.begin(); it != m_modes.end(); ++it)
535 if (it_best == m_modes.end() || it->log_w > it_best->log_w)
538 outVal = it_best->val;