23 #include <Eigen/Dense>
35 uint8_t
CBeacon::serializeGetVersion()
const {
return 0; }
39 uint32_t j = m_typePDF;
40 out << i << j << m_locationMC << m_locationGauss << m_locationSOG;
50 in >> i >> j >> m_locationMC >> m_locationGauss >> m_locationSOG;
52 m_typePDF =
static_cast<TTypePDF>(j);
69 m_locationMC.getMean(p);
72 m_locationGauss.getMean(p);
75 m_locationSOG.getMean(p);
89 return m_locationMC.getCovarianceAndMean();
91 return m_locationGauss.getCovarianceAndMean();
93 return m_locationSOG.getCovarianceAndMean();
105 const double minMahalanobisDistToDrop)
111 m_locationMC.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
114 m_locationGauss.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
117 m_locationSOG.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
134 m_locationMC.drawSingleSample(outSample);
137 m_locationGauss.drawSingleSample(outSample);
140 m_locationSOG.drawSingleSample(outSample);
157 m_locationMC.copyFrom(o);
160 m_locationGauss.copyFrom(o);
163 m_locationSOG.copyFrom(o);
180 return m_locationMC.saveToTextFile(file);
183 return m_locationGauss.saveToTextFile(file);
186 return m_locationSOG.saveToTextFile(file);
203 m_locationMC.changeCoordinatesReference(newReferenceBase);
206 m_locationGauss.changeCoordinatesReference(newReferenceBase);
209 m_locationSOG.changeCoordinatesReference(newReferenceBase);
229 std::make_shared<opengl::CPointCloud>();
230 obj->setColor(1, 0, 0);
232 obj->setPointSize(2.5);
234 const size_t N = m_locationMC.m_particles.size();
237 for (
size_t i = 0; i < N; i++)
239 i, m_locationMC.m_particles[i].d->x,
240 m_locationMC.m_particles[i].d->y,
241 m_locationMC.m_particles[i].d->z);
249 std::make_shared<opengl::CEllipsoid3D>();
251 obj->setPose(m_locationGauss.mean);
252 obj->setLineWidth(3);
255 obj->setCovMatrix(C);
257 obj->setQuantiles(3);
258 obj->enableDrawSolid3D(
false);
260 obj->setColor(1, 0, 0, 0.85f);
266 m_locationSOG.getAs3DObject(outObj);
274 obj2->setString(
format(
"#%d",
static_cast<int>(m_ID)));
277 this->getMean(meanP);
278 obj2->setLocation(meanP.
x() + 0.10, meanP.
y() + 0.10, meanP.z());
279 outObj->insert(obj2);
301 size_t i, N = m_locationMC.m_particles.size();
306 for (i = 0; i < N; i++)
309 auxStr,
sizeof(auxStr),
"%.3f%c",
310 m_locationMC.m_particles[i].d->x, (i == N - 1) ?
' ' :
',');
311 sx = sx + std::string(auxStr);
313 auxStr,
sizeof(auxStr),
"%.3f%c",
314 m_locationMC.m_particles[i].d->y, (i == N - 1) ?
' ' :
',');
315 sy = sy + std::string(auxStr);
319 out_Str.emplace_back(sx);
320 out_Str.emplace_back(sy);
321 out_Str.emplace_back(
"plot(xs,ys,'k.','MarkerSize',4);");
331 auxStr,
sizeof(auxStr),
"m=[%.3f %.3f];",
332 m_locationGauss.mean.x(), m_locationGauss.mean.y());
333 out_Str.emplace_back(auxStr);
335 auxStr,
sizeof(auxStr),
"C=[%e %e;%e %e];",
336 m_locationGauss.cov(0, 0), m_locationGauss.cov(0, 1),
337 m_locationGauss.cov(1, 0), m_locationGauss.cov(1, 1));
338 out_Str.emplace_back(auxStr);
340 out_Str.emplace_back(
341 "error_ellipse(C,m,'conf',0.997,'style','k');");
346 for (
const auto& it : m_locationSOG)
349 auxStr,
sizeof(auxStr),
"m=[%.3f %.3f];", it.val.mean.x(),
351 out_Str.emplace_back(auxStr);
353 auxStr,
sizeof(auxStr),
"C=[%e %e;%e %e];",
354 it.val.cov(0, 0), it.val.cov(0, 1), it.val.cov(1, 0),
356 out_Str.emplace_back(auxStr);
357 out_Str.emplace_back(
358 "error_ellipse(C,m,'conf',0.997,'style','k');");
371 auxStr,
sizeof(auxStr),
"text(%f,%f,'#%i');", meanP.
x(), meanP.
y(),
372 static_cast<int>(m_ID));
373 out_Str.emplace_back(auxStr);
398 float maxDistanceFromCenter)
const
402 std::unique_ptr<CPointPDFSOG> beaconPos;
405 if (m_typePDF == pdfGauss)
408 auto new_beaconPos = std::make_unique<CPointPDFSOG>(1);
410 new_beaconPos->get(0).log_w = 0;
411 new_beaconPos->get(0).val = m_locationGauss;
412 beaconPos = std::move(new_beaconPos);
413 beaconPosToUse = beaconPos.
get();
418 beaconPosToUse =
static_cast<const CPointPDFSOG*
>(&m_locationSOG);
423 for (
const auto& beaconPo : *beaconPosToUse)
427 beaconPo.val.mean.x() - sensorPntOnRobot.
x(),
428 beaconPo.val.mean.y() - sensorPntOnRobot.
y(),
429 beaconPo.val.mean.z() - sensorPntOnRobot.z());
431 size_t startIdx = outPDF.
size();
443 maxDistanceFromCenter
447 for (
size_t k = startIdx; k < outPDF.
size(); k++)
448 outPDF.
get(k).
log_w = beaconPo.log_w;
461 bool clearPreviousContentsOutPDF,
const CPoint3D& centerPoint,
462 float maxDistanceFromCenter)
477 double el, th, A_ang;
478 const float maxDistBetweenGaussians =
482 size_t B = (size_t)(
M_2PIf *
R / maxDistBetweenGaussians) + 1;
485 B = max(B, (
size_t)30);
497 S(1, 1) = S(2, 2) =
square(
506 if (clearPreviousContentsOutPDF)
515 modeIdx = outPDF.
size();
521 for (idxEl = 0; idxEl <= (1 + B / 2); idxEl++)
523 el = minEl + idxEl * A_ang;
524 if (el > (maxEl + 0.5 * A_ang))
continue;
528 if (fabs(cos(el)) < 1e-4)
533 for (idxTh = 0; idxTh < nThSteps; idxTh++)
538 dir.x((sensorPnt.
x() +
R * cos(th) * cos(el)));
539 dir.y((sensorPnt.
y() +
R * sin(th) * cos(el)));
540 dir.z((sensorPnt.z() +
R * sin(el)));
544 bool reallyCreateIt =
true;
545 if (maxDistanceFromCenter > 0)
547 dir.distanceTo(centerPoint) < maxDistanceFromCenter;
566 if (std::abs(minEl - maxEl) < 1e-6)
570 C33(0, 2) = C33(2, 0) = C33(1, 2) = C33(2, 1) = C33(2, 2) =
575 if (covarianceCompositionToAdd)
576 outPDF.
get(modeIdx).
val.
cov += *covarianceCompositionToAdd;