54 insertionOpts.loadFromConfigFile(
55 source, sectionNamePrefix +
string(
"_insertOpts"));
56 likelihoodOpts.loadFromConfigFile(
57 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
65 this->insertionOpts.dumpToTextStream(out);
66 this->likelihoodOpts.dumpToTextStream(out);
113 out << genericMapParams;
135 if (version >= 1)
in >> genericMapParams;
146 for (i = 0; i < n; i++) in >> m_beacons[i];
192 beac = getBeaconByID(it_obs->beaconID);
194 if (beac !=
nullptr && it_obs->sensedDistance > 0 &&
195 !std::isnan(it_obs->sensedDistance))
197 float sensedRange = it_obs->sensedDistance;
202 sensor3D = robotPose3D + it_obs->sensorLocationOnRobot;
220 itLW = logWeights.begin(), itLL = logLiks.begin();
222 ++it, ++itLW, ++itLL)
225 it->d->x, it->d->y, it->d->z);
232 (sensedRange - expectedRange) /
233 likelihoodOptions.rangeStd);
239 if (logWeights.size())
241 logWeights, logLiks);
253 float varZ, varR =
square(likelihoodOptions.rangeStd);
263 float expectedRange =
265 H *= 1.0 / expectedRange;
280 -0.5 *
square(sensedRange - expectedRange) / varZ;
295 itLW = logWeights.begin(), itLL = logLiks.begin();
297 ++it, ++itLW, ++itLL)
301 varR =
square(likelihoodOptions.rangeStd);
302 double Ax = it->val.mean.x() - sensor3D.
x();
303 double Ay = it->val.mean.y() - sensor3D.
y();
304 double Az = it->val.mean.z() - sensor3D.z();
308 double expectedRange =
313 varZ = H.multiply_HCHt_scalar(it->val.cov);
323 *itLL = -0.5 *
square(sensedRange - expectedRange) /
328 if (logWeights.size())
376 robotPose2D =
CPose2D(*robotPose);
377 robotPose3D = (*robotPose);
409 CPoint3D sensorPnt(robotPose3D + it->sensorLocationOnRobot);
410 float sensedRange = it->sensedDistance;
411 unsigned int sensedID = it->beaconID;
413 CBeacon* beac = getBeaconByID(sensedID);
423 newBeac.
m_ID = sensedID;
425 if (insertionOptions.insertAsMonteCarlo)
432 size_t numParts =
round(
433 insertionOptions.MC_numSamplesPerMeter *
436 insertionOptions.minElevation_deg <=
437 insertionOptions.maxElevation_deg)
439 DEG2RAD(insertionOptions.minElevation_deg);
441 DEG2RAD(insertionOptions.maxElevation_deg);
452 sensedRange, likelihoodOptions.rangeStd);
453 itP->d->x = sensorPnt.
x() +
R * cos(th) * cos(el);
454 itP->d->y = sensorPnt.
y() +
R * sin(th) * cos(el);
455 itP->d->z = sensorPnt.z() +
R * sin(el);
472 m_beacons.push_back(newBeac);
487 double maxW = -1e308, sumW = 0;
497 it->d->x, it->d->y, it->d->z);
503 (sensedRange - expectedRange) /
504 likelihoodOptions.rangeStd);
505 maxW = max(it->log_w, maxW);
506 sumW += exp(it->log_w);
512 if (insertionOptions.MC_performResampling)
520 vector<double> log_ws;
521 vector<size_t> indxs;
525 CParticleFilterCapable::computeResampling(
526 CParticleFilter::prSystematic, log_ws,
535 (insertionOptions.minElevation_deg ==
536 insertionOptions.maxElevation_deg);
539 .MC_afterResamplingNoise;
579 .MC_thresholdNegligible))
608 double D1 = sqrt(COV(0, 0));
609 double D2 = sqrt(COV(1, 1));
610 double D3 = sqrt(COV(2, 2));
612 double mxVar =
max3(D1, D2, D3);
614 if (mxVar < insertionOptions.MC_maxStdToGauss)
627 if (insertionOptions.minElevation_deg ==
628 insertionOptions.maxElevation_deg)
648 float varR =
square(likelihoodOptions.rangeStd);
660 float y = sensedRange - expectedRange;
679 varZ = H.multiply_HCHt_scalar(
693 (Eigen::Matrix<double, 3, 3>::Identity() -
706 float varR =
square(likelihoodOptions.rangeStd);
717 double expectedRange =
721 double y = sensedRange - expectedRange;
726 double Ax = (it->val.mean.x() - sensorPnt.
x());
727 double Ay = (it->val.mean.y() - sensorPnt.
y());
728 double Az = (it->val.mean.z() - sensorPnt.z());
734 varZ = H.multiply_HCHt_scalar(it->val.cov);
737 K.multiply(it->val.cov, H.transpose());
741 it->val.mean.x_incr(K(0, 0) *
y);
742 it->val.mean.y_incr(K(1, 0) *
y);
743 it->val.mean.z_incr(K(2, 0) *
y);
746 (Eigen::Matrix<double, 3, 3>::Identity() -
753 it->log_w += -0.5 *
square(
y) / varZ;
765 if (max_w - it->log_w >
766 insertionOptions.SOG_thresholdNegligible)
788 double D1 = sqrt(curCov(0, 0));
789 double D2 = sqrt(curCov(1, 1));
790 double D3 = sqrt(curCov(2, 2));
791 float maxDiag =
max3(D1, D2, D3);
836 CPose3D otherMapPose3D(otherMapPose);
841 vector<bool> otherCorrespondences;
847 computeMatchingWith3DLandmarks(
848 otherMap2, correspondences, extraResults.correspondencesRatio,
849 otherCorrespondences);
860 for (
iterator lm = m_beacons.begin(); lm != m_beacons.end(); ++lm)
861 lm->changeCoordinatesReference(newOrg);
873 changeCoordinatesReference(newOrg);
882 vector<bool>& otherCorrespondences)
const 887 size_t nThis, nOther;
891 vector<bool> thisLandmarkAssigned;
894 nThis = m_beacons.size();
898 thisLandmarkAssigned.resize(nThis,
false);
901 correspondences.clear();
902 otherCorrespondences.clear();
903 otherCorrespondences.resize(nOther,
false);
904 correspondencesRatio = 0;
906 for (k = 0, otherIt = anotherMap->
m_beacons.begin();
907 otherIt != anotherMap->
m_beacons.end(); ++otherIt, ++k)
909 for (j = 0, thisIt = m_beacons.begin(); thisIt != m_beacons.end();
913 if ((otherIt)->m_ID == (thisIt)->m_ID)
917 if (!thisLandmarkAssigned[j])
919 thisLandmarkAssigned[j] =
true;
922 otherCorrespondences[k] =
true;
926 CPoint3D mean_j = m_beacons[j].getMeanVal();
930 match.
this_z = mean_j.z();
938 correspondences.push_back(match);
947 correspondencesRatio =
948 2.0f * correspondences.size() /
static_cast<float>(nThis + nOther);
957 const string& file,
const char* style,
float confInterval)
const 963 if (!f)
return false;
967 f,
"%%-------------------------------------------------------\n");
968 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
969 os::fprintf(f,
"%% 'CBeaconMap::saveToMATLABScript3D'\n");
973 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
976 f,
"%%-------------------------------------------------------\n\n");
983 for (
const_iterator it = m_beacons.begin(); it != m_beacons.end(); ++it)
985 it->getAsMatlabDrawCommands(strs);
1007 "\n----------- [CBeaconMap::TLikelihoodOptions] ------------ \n\n");
1009 out.
printf(
"rangeStd = %f\n", rangeStd);
1020 rangeStd = iniFile.
read_float(section.c_str(),
"rangeStd", rangeStd);
1027 : insertAsMonteCarlo(true),
1028 maxElevation_deg(0),
1029 minElevation_deg(0),
1030 MC_numSamplesPerMeter(1000),
1031 MC_maxStdToGauss(0.4f),
1032 MC_thresholdNegligible(5),
1033 MC_performResampling(false),
1034 MC_afterResamplingNoise(0.01f),
1035 SOG_thresholdNegligible(20.0f),
1036 SOG_maxDistBetweenGaussians(1.0f),
1037 SOG_separationConstant(3.0f)
1048 "\n----------- [CBeaconMap::TInsertionOptions] ------------ \n\n");
1051 "insertAsMonteCarlo = %c\n",
1052 insertAsMonteCarlo ?
'Y' :
'N');
1054 "minElevation_deg = %.03f\n", minElevation_deg);
1056 "maxElevation_deg = %.03f\n", maxElevation_deg);
1058 "MC_numSamplesPerMeter = %d\n",
1059 MC_numSamplesPerMeter);
1061 "MC_maxStdToGauss = %.03f\n", MC_maxStdToGauss);
1063 "MC_thresholdNegligible = %.03f\n",
1064 MC_thresholdNegligible);
1066 "MC_performResampling = %c\n",
1067 MC_performResampling ?
'Y' :
'N');
1069 "MC_afterResamplingNoise = %.03f\n",
1070 MC_afterResamplingNoise);
1072 "SOG_thresholdNegligible = %.03f\n",
1073 SOG_thresholdNegligible);
1075 "SOG_maxDistBetweenGaussians = %.03f\n",
1076 SOG_maxDistBetweenGaussians);
1078 "SOG_separationConstant = %.03f\n",
1079 SOG_separationConstant);
1096 MC_thresholdNegligible,
float, iniFile, section.c_str());
1099 MC_afterResamplingNoise,
float, iniFile, section.c_str());
1101 SOG_thresholdNegligible,
float, iniFile, section.c_str());
1103 SOG_maxDistBetweenGaussians,
float, iniFile, section.c_str());
1105 SOG_separationConstant,
float, iniFile, section.c_str());
1116 const CPose3D& in_robotPose,
const CPoint3D& in_sensorLocationOnRobot,
1125 point3D = in_robotPose + in_sensorLocationOnRobot;
1133 it->getMean(beacon3D);
1150 out_Observations.
sensedData.push_back(newMeas);
1160 const string& filNamePrefix)
const 1165 string fil1(filNamePrefix +
string(
"_3D.m"));
1171 mrpt::make_aligned_shared<opengl::CSetOfObjects>();
1175 mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
1176 -100, 100, -100, 100, 0, 1);
1181 string fil2(filNamePrefix +
string(
"_3D.3Dscene"));
1186 string fil3(filNamePrefix +
string(
"_covs.txt"));
1190 string fil4(filNamePrefix +
string(
"_population.txt"));
1192 FILE* f =
os::fopen(fil4.c_str(),
"wt");
1195 size_t nParts = 0, nGaussians = 0;
1200 switch (it->m_typePDF)
1203 nParts += it->m_locationMC.size();
1206 nGaussians += it->m_locationSOG.size();
1215 f,
"%u %u", static_cast<unsigned>(nParts),
1216 static_cast<unsigned>(nGaussians));
1240 it->getAs3DObject(outObj);
1273 otherMap = static_cast<const CBeaconMap*>(otherMap2);
1275 if (!otherMap)
return 0;
1278 vector<bool> otherCorrespondences;
1279 float out_corrsRatio;
1286 &modMap, matchList, out_corrsRatio, otherCorrespondences);
1288 return out_corrsRatio;
1299 if (it->m_ID ==
id)
return &(*it);
1309 if (it->m_ID ==
id)
return &(*it);
1330 it->getCovarianceAndMean(C,
p);
1333 float D2 = C(0, 0) * C(1, 1) -
square(C(0, 1));
1335 f,
"%i %f %f %f %e %e %e %e %e %e %e %e\n",
1336 static_cast<int>(it->m_ID),
p.x(),
p.y(),
p.z(), C(0, 0), C(1, 1),
1337 C(2, 2), D2, D3, C(0, 1), C(1, 2), C(1, 2));
A namespace of pseudo-random numbers genrators of diferent distributions.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
double x() const
Common members of all points & poses classes.
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Parameters for CMetricMap::compute3DMatchingRatio()
mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts
Observations insertion options.
mrpt::poses::CPointPDFParticles m_locationMC
The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accesible for ease of use, but the CPointPDF interface is also implemented in CBeacon).
int64_t TBeaconID
The type for the IDs of landmarks.
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...
void y_incr(const double v)
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
int void fclose(FILE *f)
An OS-independent version of fclose.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPoint3D mean
The mean value.
#define MRPT_CHECK_NORMAL_NUMBER(val)
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
void computeMatchingWith3DLandmarks(const mrpt::maps::CBeaconMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: Firsly, the landmarks' descriptor is used to find correspondences, then inconsistent ones removed by looking at their 3D poses.
#define THROW_EXCEPTION(msg)
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
EIGEN_STRONG_INLINE iterator begin()
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string §ionNamePrefix) override
Load all map-specific params.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
TMapGenericParams genericMapParams
Common params to all maps.
std::deque< CBeacon >::iterator iterator
const Scalar * const_iterator
TInsertionOptions()
Initilization of default parameters.
virtual void internal_clear() override
Internal method called by clear()
GLsizei GLsizei GLuint * obj
void clear()
Clear the contents of this container.
mrpt::poses::CPointPDFSOG m_locationSOG
The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of use, but the CPointPDF interf...
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
TSequenceBeacons m_beacons
The individual beacons.
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.
CParticleList m_particles
The array of particles.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
This CStream derived class allow using a file as a write-only, binary stream.
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
#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).
std::deque< TGaussianMode >::const_iterator const_iterator
std::deque< TMeasurement > sensedData
The list of observed ranges.
void x_incr(const double v)
void setSize(size_t numberParticles, const CPoint3D &defaultValue=CPoint3D(0, 0, 0))
Erase all the previous particles and change the number of particles, with a given initial value...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string 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).
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void saveToTextFile(const std::string &fil) const
Save a text file with a row per beacon, containing this 11 elements:
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
TTypePDF m_typePDF
Which one of the different 3D point PDF is currently used in this object: montecarlo, gaussian, or a sum of gaussians.
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...
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void performSubstitution(const std::vector< size_t > &indx) override
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
iterator erase(iterator i)
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
float minSensorDistance
Info about sensor.
float sensedDistance
The sensed range itself (in meters).
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const override
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
size_t size() const
Return the number of Gaussian modes.
const CBeacon * getBeaconByID(CBeacon::TBeaconID id) const
Returns a pointer to the beacon with the given ID, or nullptr if it does not exist.
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams ¶ms, TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
GLsizei GLsizei GLchar * source
virtual bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
const T max3(const T &A, const T &B, const T &C)
int round(const T value)
Returns the closer integer (int) to x.
void resize(const size_t N)
Resize the number of SOG modes.
mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Each one of the measurements.
void clear()
Clear all the particles (free memory)
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
std::deque< CBeacon >::const_iterator const_iterator
TBeaconID m_ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
bool saveToMATLABScript3D(const std::string &file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
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::deque< TGaussianMode >::iterator iterator
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Parameters for the determination of matchings between point clouds, etc.
unsigned __int32 uint32_t
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a reading toward each of the beacons in the landmarks map, if any.
GLenum const GLfloat * params
void getText(std::string &outText) const
Returns the whole string list as a single string with ' ' characters for newlines.
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
std::shared_ptr< CGridPlaneXY > Ptr
A gaussian distribution for 3D points.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
size_t size() const
Returns the stored landmarks count.
double distance3DTo(double ax, double ay, double az) const
Returns the 3D distance from this pose/point to a 3D point.
TLikelihoodOptions()
Initilization of default parameters.
mrpt::poses::CPointPDFGaussian m_locationGauss
The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease of use, but the CPointPDF inte...