39 : m_typePDF(pdfGauss),
64 out << i << j << m_locationMC << m_locationGauss << m_locationSOG;
80 in >> i >> j >> m_locationMC >> m_locationGauss >> m_locationSOG;
82 m_typePDF =
static_cast<TTypePDF>(j);
99 m_locationMC.getMean(
p);
102 m_locationGauss.getMean(
p);
105 m_locationSOG.getMean(
p);
122 m_locationMC.getCovarianceAndMean(COV,
p);
125 m_locationGauss.getCovarianceAndMean(COV,
p);
128 m_locationSOG.getCovarianceAndMean(COV,
p);
141 const double& minMahalanobisDistToDrop)
147 m_locationMC.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
150 m_locationGauss.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
153 m_locationSOG.bayesianFusion(p1, p2, minMahalanobisDistToDrop);
170 m_locationMC.drawSingleSample(outSample);
173 m_locationGauss.drawSingleSample(outSample);
176 m_locationSOG.drawSingleSample(outSample);
193 m_locationMC.copyFrom(o);
196 m_locationGauss.copyFrom(o);
199 m_locationSOG.copyFrom(o);
216 m_locationMC.saveToTextFile(file);
219 m_locationGauss.saveToTextFile(file);
222 m_locationSOG.saveToTextFile(file);
239 m_locationMC.changeCoordinatesReference(newReferenceBase);
242 m_locationGauss.changeCoordinatesReference(newReferenceBase);
245 m_locationSOG.changeCoordinatesReference(newReferenceBase);
265 mrpt::make_aligned_shared<opengl::CPointCloud>();
266 obj->setColor(1, 0, 0);
268 obj->setPointSize(2.5);
270 const size_t N = m_locationMC.m_particles.size();
273 for (
size_t i = 0; i < N; i++)
275 i, m_locationMC.m_particles[i].d->x,
276 m_locationMC.m_particles[i].d->y,
277 m_locationMC.m_particles[i].d->z);
285 mrpt::make_aligned_shared<opengl::CEllipsoid>();
287 obj->setPose(m_locationGauss.mean);
288 obj->setLineWidth(3);
291 if (C(2, 2) == 0) C.setSize(2, 2);
292 obj->setCovMatrix(C);
294 obj->setQuantiles(3);
295 obj->enableDrawSolid3D(
false);
297 obj->setColor(1, 0, 0, 0.85);
303 m_locationSOG.getAs3DObject(outObj);
311 obj2->setString(
format(
"#%d", static_cast<int>(m_ID)));
314 this->getMean(meanP);
315 obj2->setLocation(meanP.
x() + 0.10, meanP.
y() + 0.10, meanP.z());
316 outObj->insert(obj2);
338 size_t i, N = m_locationMC.m_particles.size();
343 for (i = 0; i < N; i++)
346 auxStr,
sizeof(auxStr),
"%.3f%c",
347 m_locationMC.m_particles[i].d->x, (i == N - 1) ?
' ' :
',');
350 auxStr,
sizeof(auxStr),
"%.3f%c",
351 m_locationMC.m_particles[i].d->y, (i == N - 1) ?
' ' :
',');
368 auxStr,
sizeof(auxStr),
"m=[%.3f %.3f];",
369 m_locationGauss.mean.x(), m_locationGauss.mean.y());
372 auxStr,
sizeof(auxStr),
"C=[%e %e;%e %e];",
373 m_locationGauss.cov(0, 0), m_locationGauss.cov(0, 1),
374 m_locationGauss.cov(1, 0), m_locationGauss.cov(1, 1));
378 std::string(
"error_ellipse(C,m,'conf',0.997,'style','k');"));
384 it != m_locationSOG.end(); ++it)
387 auxStr,
sizeof(auxStr),
"m=[%.3f %.3f];",
388 (it)->
val.mean.x(), (it)->
val.mean.y());
391 auxStr,
sizeof(auxStr),
"C=[%e %e;%e %e];",
392 (it)->
val.cov(0, 0), (it)->
val.cov(0, 1),
393 (it)->
val.cov(1, 0), (it)->
val.cov(1, 1));
397 "error_ellipse(C,m,'conf',0.997,'style','k');"));
410 auxStr,
sizeof(auxStr),
"text(%f,%f,'#%i');", meanP.
x(), meanP.
y(),
411 static_cast<int>(m_ID));
437 const CPoint3D& centerPoint,
const float& maxDistanceFromCenter)
const 443 if (m_typePDF == pdfGauss)
449 new_beaconPos->
get(0).
val = m_locationGauss;
450 beaconPos = new_beaconPos;
455 beaconPos =
static_cast<const CPointPDFSOG*
>(&m_locationSOG);
461 it != beaconPos->end(); ++it)
465 (it)->
val.mean.x() - sensorPntOnRobot.
x(),
466 (it)->
val.mean.y() - sensorPntOnRobot.
y(),
467 (it)->
val.mean.z() - sensorPntOnRobot.z());
469 size_t startIdx = outPDF.
size();
480 maxDistanceFromCenter
484 for (
size_t k = startIdx; k < outPDF.
size(); k++)
488 if (m_typePDF == pdfGauss)
delete beaconPos;
500 bool clearPreviousContentsOutPDF,
const CPoint3D& centerPoint,
501 const float& maxDistanceFromCenter)
514 double el, th, A_ang;
515 const float maxDistBetweenGaussians =
519 size_t B = (size_t)(
M_2PIf *
R / maxDistBetweenGaussians) + 1;
522 B = max(B, (
size_t)30);
534 S(1, 1) = S(2, 2) =
square(
543 if (clearPreviousContentsOutPDF)
552 modeIdx = outPDF.
size();
558 for (idxEl = 0; idxEl <= (1 + B / 2); idxEl++)
560 el = minEl + idxEl * A_ang;
561 if (el > (maxEl + 0.5 * A_ang))
continue;
565 if (fabs(cos(el)) < 1e-4)
570 for (idxTh = 0; idxTh < nThSteps; idxTh++)
575 dir.
x((sensorPnt.
x() +
R * cos(th) * cos(el)));
576 dir.
y((sensorPnt.
y() +
R * sin(th) * cos(el)));
577 dir.z((sensorPnt.z() +
R * sin(el)));
581 bool reallyCreateIt =
true;
582 if (maxDistanceFromCenter > 0)
584 dir.
distanceTo(centerPoint) < maxDistanceFromCenter;
595 dir = dir - sensorPnt;
608 C33.get_unsafe(0, 2) = C33.get_unsafe(2, 0) =
609 C33.get_unsafe(1, 2) = C33.get_unsafe(2, 1) =
610 C33.get_unsafe(2, 2) = 0;
614 if (covarianceCompositionToAdd)
615 outPDF.
get(modeIdx).
val.
cov += *covarianceCompositionToAdd;
double x() const
Common members of all points & poses classes.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
void copyFrom(const mrpt::poses::CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPoint3D mean
The mean value.
void resize(const size_t N)
Resize the number of SOG modes.
void changeCoordinatesReference(const mrpt::poses::CPose3D &newReferenceBase) override
this = p (+) this.
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
#define THROW_EXCEPTION(msg)
The struct for each mode:
void clear()
Clear all the gaussian modes.
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
GLsizei GLsizei GLuint * obj
T square(const T x)
Inline function for the square of a number.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A class for storing a list of text lines.
This base provides a set of functions for maths stuff.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
float SOG_maxDistBetweenGaussians
A parameter for initializing 2D/3D SOGs.
std::deque< TGaussianMode >::const_iterator const_iterator
void getAsMatlabDrawCommands(utils::CStringList &out_Str) const
Gets a set of MATLAB commands which draw the current state of the beacon:
float rangeStd
The standard deviation used for Beacon ranges likelihood (default=0.08m).
void clear()
Clear the whole list.
std::shared_ptr< CSetOfObjects > Ptr
This namespace contains representation of robot actions and observations.
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
float SOG_separationConstant
Constant used to compute the std.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, mrpt::poses::CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
const TGaussianMode & get(size_t i) const
Access to individual beacons.
float maxElevation_deg
Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation:...
GLsizei const GLchar ** string
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define INVALID_BEACON_ID
Used for CObservationBeaconRange, CBeacon, etc.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CMatrixTemplateNumeric< T > generateAxisBaseFromDirection(T dx, T dy, T dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
OPENGL_SETOFOBJECTSPTR getAs3DObject() const
Returns a 3D representation of this PDF.
size_t size() const
Return the number of Gaussian modes.
void saveToTextFile(const std::string &file) const override
Save PDF's particles to a text file.
mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOptions
double log_w
The log-weight.
void drawSingleSample(mrpt::poses::CPoint3D &outSample) const override
Draw a sample from the pdf.
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
static void generateRingSOG(const float &sensedRange, mrpt::poses::CPointPDFSOG &outPDF, const CBeaconMap *myBeaconMap, const mrpt::poses::CPoint3D &sensorPnt, const mrpt::math::CMatrixDouble33 *covarianceCompositionToAdd=nullptr, bool clearPreviousContentsOutPDF=true, const mrpt::poses::CPoint3D ¢erPoint=mrpt::poses::CPoint3D(0, 0, 0), const float &maxDistanceFromCenter=0)
This static method returns a SOG with ring-shape (or as a 3D sphere) that can be used to initialize a...
std::shared_ptr< CText > Ptr
mrpt::maps::CBeaconMap::TInsertionOptions insertionOptions
std::shared_ptr< CPointCloud > Ptr
unsigned __int32 uint32_t
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions...
void generateObservationModelDistribution(const float &sensedRange, mrpt::poses::CPointPDFSOG &outPDF, const CBeaconMap *myBeaconMap, const mrpt::poses::CPoint3D &sensorPntOnRobot, const mrpt::poses::CPoint3D ¢erPoint=mrpt::poses::CPoint3D(0, 0, 0), const float &maxDistanceFromCenter=0) const
Compute the observation model p(z_t|x_t) for a given observation (range value), and return it as an a...
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
void getMean(mrpt::poses::CPoint3D &mean_point) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF).
void add(const std::string &str)
Appends a new string at the end of the string list.
std::shared_ptr< CEllipsoid > Ptr
virtual ~CBeacon()
Virtual destructor.