33 double xMin,
double xMax,
double yMin,
double yMax,
double resolutionXY,
34 double resolutionPhi,
double phiMin,
double phiMax)
36 xMin, xMax, yMin, yMax, resolutionXY, resolutionPhi, phiMin, phiMax)
38 uniformDistribution();
43 if (
this == &o)
return;
51 CPosePDFGrid::~CPosePDFGrid() {}
62 for (
size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
63 for (
size_t y = 0;
y < m_sizeY;
y++)
64 for (
size_t x = 0;
x < m_sizeX;
x++)
66 const double w = *getByIndex(
x,
y, phiInd);
68 CPose2D(idx2x(
x), idx2y(
y), idx2phi(phiInd)),
w);
80 TPose2D(0, 0, 0), m_sizePhi * m_sizeY * m_sizeX);
82 for (
size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
84 for (
size_t y = 0;
y < m_sizeY;
y++)
85 for (
size_t x = 0;
x < m_sizeX;
x++)
88 log(*getByIndex(
x,
y, phiInd));
90 TPose2D(idx2x(
x), idx2y(
y), idx2phi(phiInd));
96 uint8_t CPosePDFGrid::serializeGetVersion()
const {
return 0; }
100 out << m_xMin << m_xMax << m_yMin << m_yMax << m_phiMin << m_phiMax
101 << m_resolutionXY << m_resolutionPhi << static_cast<int32_t>(m_sizeX)
102 << static_cast<int32_t>(m_sizeY) <<
static_cast<int32_t>(m_sizePhi)
103 << static_cast<int32_t>(m_sizeXY) <<
static_cast<int32_t>(m_idxLeftX)
104 << static_cast<int32_t>(m_idxLeftY)
105 <<
static_cast<int32_t>(m_idxLeftPhi);
110 void CPosePDFGrid::serializeFrom(
118 in >> m_xMin >> m_xMax >> m_yMin >> m_yMax >> m_phiMin >>
119 m_phiMax >> m_resolutionXY >> m_resolutionPhi;
121 int32_t sizeX, sizeY, sizePhi, sizeXY, idxLeftX, idxLeftY,
124 in >> sizeX >> sizeY >> sizePhi >> sizeXY >> idxLeftX >> idxLeftY >>
131 m_idxLeftX = idxLeftX;
132 m_idxLeftY = idxLeftY;
133 m_idxLeftPhi = idxLeftPhi;
146 const auto dimsFile = dataFile +
std::string(
"_dims.txt");
148 std::ofstream f_d(dataFile), f_s(dimsFile);
149 if (!f_d.is_open() || !f_s.is_open())
return false;
153 "%u %u %u %f %f %f %f %f %f\n", (
unsigned)m_sizeX, (
unsigned)m_sizeY,
154 (
unsigned)m_sizePhi, m_xMin, m_xMax, m_yMin, m_yMax, m_phiMin,
158 for (
unsigned int phiInd = 0; phiInd < m_sizePhi; phiInd++)
160 for (
unsigned int y = 0;
y < m_sizeY;
y++)
162 for (
unsigned int x = 0;
x < m_sizeX;
x++)
174 void CPosePDFGrid::changeCoordinatesReference(
const CPose3D& newReferenceBase)
183 void CPosePDFGrid::bayesianFusion(
185 const double minMahalanobisDistToDrop)
205 void CPosePDFGrid::drawSingleSample(
CPose2D& outPart)
const 214 void CPosePDFGrid::drawManySamples(
215 size_t N, std::vector<CVectorDouble>& outSamples)
const 247 void CPosePDFGrid::uniformDistribution()
249 double val = 1.0f / m_data.size();
Computes weighted and un-weighted averages of SE(2) poses.
#define THROW_EXCEPTION(msg)
CParticleList m_particles
The array of particles.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const override
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
void resetDeterministic(const mrpt::math::TPose2D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
GLubyte GLubyte GLubyte GLubyte w
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
This base provides a set of functions for maths stuff.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLsizei const GLchar ** string
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void normalize(Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
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).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
const Scalar * const_iterator
This is a template class for storing a 3D (2D+heading) grid containing any kind of data...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.