32 m_particles.resize(M);
34 for (
auto& it : m_particles)
41 resetDeterministic(nullPose);
51 if (
this == &o)
return;
81 if (m_particles.empty())
return;
87 it != m_particles.end(); ++it)
89 const double w = exp(it->log_w);
113 double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
114 double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
115 double std_zya = 0, std_zp = 0, std_zr = 0;
116 double std_yap = 0, std_yar = 0;
120 double mean_yaw =
mean.yaw();
121 double mean_pitch =
mean.pitch();
122 double mean_roll =
mean.roll();
123 if (mean_yaw < 0) mean_yaw +=
M_2PI;
124 if (mean_pitch < 0) mean_pitch +=
M_2PI;
125 if (mean_roll < 0) mean_roll +=
M_2PI;
128 if (m_particles.size() < 2)
return;
134 it != m_particles.end(); ++it)
142 it != m_particles.end(); ++it)
144 double w = exp(it->log_w) / W;
147 double err_yaw =
wrapToPi(fabs(it->d->yaw() - mean_yaw));
148 double err_pitch =
wrapToPi(fabs(it->d->pitch() - mean_pitch));
149 double err_roll =
wrapToPi(fabs(it->d->roll() - mean_roll));
151 double err_x = it->d->x() -
mean.x();
152 double err_y = it->d->y() -
mean.y();
153 double err_z = it->d->z() -
mean.z();
158 vars[3] +=
square(err_yaw) *
w;
159 vars[4] +=
square(err_pitch) *
w;
160 vars[5] +=
square(err_roll) *
w;
162 std_xy += err_x * err_y *
w;
163 std_xz += err_x * err_z *
w;
164 std_xya += err_x * err_yaw *
w;
165 std_xp += err_x * err_pitch *
w;
166 std_xr += err_x * err_roll *
w;
168 std_yz += err_y * err_z *
w;
169 std_yya += err_y * err_yaw *
w;
170 std_yp += err_y * err_pitch *
w;
171 std_yr += err_y * err_roll *
w;
173 std_zya += err_z * err_yaw *
w;
174 std_zp += err_z * err_pitch *
w;
175 std_zr += err_z * err_roll *
w;
177 std_yap += err_yaw * err_pitch *
w;
178 std_yar += err_yaw * err_roll *
w;
180 std_pr += err_pitch * err_roll *
w;
191 cov(1, 0) =
cov(0, 1) = std_xy;
192 cov(2, 0) =
cov(0, 2) = std_xz;
193 cov(3, 0) =
cov(0, 3) = std_xya;
194 cov(4, 0) =
cov(0, 4) = std_xp;
195 cov(5, 0) =
cov(0, 5) = std_xr;
197 cov(2, 1) =
cov(1, 2) = std_yz;
198 cov(3, 1) =
cov(1, 3) = std_yya;
199 cov(4, 1) =
cov(1, 4) = std_yp;
200 cov(5, 1) =
cov(1, 5) = std_yr;
202 cov(3, 2) =
cov(2, 3) = std_zya;
203 cov(4, 2) =
cov(2, 4) = std_zp;
204 cov(5, 2) =
cov(2, 5) = std_zr;
206 cov(4, 3) =
cov(3, 4) = std_yap;
207 cov(5, 3) =
cov(3, 5) = std_yar;
209 cov(5, 4) =
cov(4, 5) = std_pr;
224 writeParticlesToStream(out);
237 readParticlesFromStream(
in);
257 os::fprintf(f,
"%% x y z yaw[rad] pitch[rad] roll[rad] log_weight\n");
259 for (
const auto&
p : m_particles)
261 f,
"%f %f %f %f %f %f %e\n",
p.d->x(),
p.d->y(),
p.d->z(),
262 p.d->yaw(),
p.d->pitch(),
p.d->roll(),
p.log_w);
272 return *m_particles[i].d;
279 const CPose3D& newReferenceBase)
282 it != m_particles.end(); ++it)
283 it->d->composeFrom(newReferenceBase, *it->d);
299 size_t N, std::vector<CVectorDouble>& outSamples)
const 339 for (it = out->m_particles.begin(); it != out->m_particles.end(); ++it)
340 *it->d = zero - *it->d;
351 itMax = m_particles.begin();
352 double max_w = -1e300;
354 for (it = m_particles.begin(); it != m_particles.end(); ++it)
356 if (it->log_w > max_w)
386 if (particlesCount > 0)
389 m_particles.resize(particlesCount);
390 for (it = m_particles.begin(); it != m_particles.end(); ++it)
394 for (it = m_particles.begin(); it != m_particles.end(); ++it)
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
void getCovarianceAndMean(mrpt::math::CMatrixDouble66 &cov, CPose3D &mean_point) const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
int void fclose(FILE *f)
An OS-independent version of fclose.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
const Scalar * const_iterator
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.
GLubyte GLubyte GLubyte GLubyte w
T square(const T x)
Inline function for the square of a number.
void append(CPose3DPDFParticles &o)
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights...
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
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.
#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.
void operator+=(const CPose3D &Ap)
Appends (pose-composition) a given pose "p" to each particle.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CPose3D getParticlePose(int i) const
Returns the pose of the i'th particle.
CPose3D getMostLikelyParticle() const
Returns the particle with the highest weight.
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Computes weighted and un-weighted averages of SE(3) poses.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...
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...
void get_average(mrpt::poses::CPose3D &out_mean) const
Returns the average pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void resetDeterministic(const CPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
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...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void saveToTextFile(const std::string &file) const override
Save PDF's m_particles to a text file.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.