Go to the documentation of this file.
30 m_particles.resize(M);
31 TPose3D nullPose(0, 0, 0, 0, 0, 0);
32 resetDeterministic(nullPose);
38 if (
this == &o)
return;
43 m_particles = pdf->m_particles;
57 if (m_particles.empty())
return;
61 for (
const auto& part : m_particles)
63 const double w = exp(part.log_w);
64 se_averager.
append(part.d, w);
86 double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
87 double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
88 double std_zya = 0, std_zp = 0, std_zr = 0;
89 double std_yap = 0, std_yar = 0;
93 double mean_yaw =
mean.yaw();
94 double mean_pitch =
mean.pitch();
95 double mean_roll =
mean.roll();
96 if (mean_yaw < 0) mean_yaw +=
M_2PI;
97 if (mean_pitch < 0) mean_pitch +=
M_2PI;
98 if (mean_roll < 0) mean_roll +=
M_2PI;
101 if (m_particles.size() < 2)
return {
cov,
mean};
105 for (
const auto& p : m_particles) W += exp(p.log_w);
110 for (
const auto& p : m_particles)
112 double w = exp(p.log_w) / W;
115 double err_yaw =
wrapToPi(std::abs(p.d.yaw - mean_yaw));
116 double err_pitch =
wrapToPi(std::abs(p.d.pitch - mean_pitch));
117 double err_roll =
wrapToPi(std::abs(p.d.roll - mean_roll));
119 double err_x = p.d.x -
mean.x();
120 double err_y = p.d.y -
mean.y();
121 double err_z = p.d.z -
mean.z();
123 vars[0] +=
square(err_x) * w;
124 vars[1] +=
square(err_y) * w;
125 vars[2] +=
square(err_z) * w;
126 vars[3] +=
square(err_yaw) * w;
127 vars[4] +=
square(err_pitch) * w;
128 vars[5] +=
square(err_roll) * w;
130 std_xy += err_x * err_y * w;
131 std_xz += err_x * err_z * w;
132 std_xya += err_x * err_yaw * w;
133 std_xp += err_x * err_pitch * w;
134 std_xr += err_x * err_roll * w;
136 std_yz += err_y * err_z * w;
137 std_yya += err_y * err_yaw * w;
138 std_yp += err_y * err_pitch * w;
139 std_yr += err_y * err_roll * w;
141 std_zya += err_z * err_yaw * w;
142 std_zp += err_z * err_pitch * w;
143 std_zr += err_z * err_roll * w;
145 std_yap += err_yaw * err_pitch * w;
146 std_yar += err_yaw * err_roll * w;
148 std_pr += err_pitch * err_roll * w;
159 cov(1, 0) =
cov(0, 1) = std_xy;
160 cov(2, 0) =
cov(0, 2) = std_xz;
161 cov(3, 0) =
cov(0, 3) = std_xya;
162 cov(4, 0) =
cov(0, 4) = std_xp;
163 cov(5, 0) =
cov(0, 5) = std_xr;
165 cov(2, 1) =
cov(1, 2) = std_yz;
166 cov(3, 1) =
cov(1, 3) = std_yya;
167 cov(4, 1) =
cov(1, 4) = std_yp;
168 cov(5, 1) =
cov(1, 5) = std_yr;
170 cov(3, 2) =
cov(2, 3) = std_zya;
171 cov(4, 2) =
cov(2, 4) = std_zp;
172 cov(5, 2) =
cov(2, 5) = std_zr;
174 cov(4, 3) =
cov(3, 4) = std_yap;
175 cov(5, 3) =
cov(3, 5) = std_yar;
177 cov(5, 4) =
cov(4, 5) = std_pr;
186 writeParticlesToStream(
out);
198 old.readParticlesFromStream(in);
201 old.m_particles.begin(), old.m_particles.end(),
202 std::back_inserter(m_particles),
204 return CParticleData(p.d.asTPose(), p.log_w);
210 readParticlesFromStream(in);
228 if (!f)
return false;
230 os::fprintf(f,
"%% x y z yaw[rad] pitch[rad] roll[rad] log_weight\n");
232 for (
const auto& p : m_particles)
234 f,
"%f %f %f %f %f %f %e\n", p.d.x, p.d.y, p.d.z, p.d.yaw,
235 p.d.pitch, p.d.roll, p.log_w);
243 return m_particles[i].d;
247 const CPose3D& newReferenceBase)
249 for (
auto& p : m_particles)
250 p.d = (newReferenceBase +
CPose3D(p.d)).asTPose();
254 [maybe_unused]]
CPose3D& outPart)
const
260 [[maybe_unused]]
size_t N,
261 [[maybe_unused]] std::vector<CVectorDouble>& outSamples)
const
283 out->copyFrom(*
this);
285 for (
auto& p :
out->m_particles) p.d = (zero -
CPose3D(p.d)).asTPose();
292 double max_w = -std::numeric_limits<double>::max();
293 for (
const auto& p : m_particles)
312 const TPose3D& location,
size_t particlesCount)
314 if (particlesCount > 0) m_particles.resize(particlesCount);
316 for (
auto& p : m_particles)
325 const int particlesCount)
328 if (particlesCount > 0) m_particles.resize(particlesCount);
332 for (
auto& p : m_particles)
334 p.d.x = rnd.drawUniform(cmin.
x, cmax.
x);
335 p.d.y = rnd.drawUniform(cmin.
y, cmax.
y);
336 p.d.z = rnd.drawUniform(cmin.
z, cmax.
z);
337 p.d.yaw = rnd.drawUniform(cmin.
yaw, cmax.
yaw);
338 p.d.pitch = rnd.drawUniform(cmin.
pitch, cmax.
pitch);
339 p.d.roll = rnd.drawUniform(cmin.
roll, cmax.
roll);
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
bool saveToTextFile(const std::string &file) const override
Save PDF's m_particles to a text file.
void operator+=(const CPose3D &Ap)
Appends (pose-composition) a given pose "p" to each particle.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
int void fclose(FILE *f)
An OS-independent version of fclose.
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void get_average(mrpt::poses::CPose3D &out_mean) const
Returns the average pose.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
mrpt::vision::TStereoCalibResults out
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion.
mrpt::math::TPose3D getMostLikelyParticle() const
Returns the particle with the highest weight.
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Virtual base class for "archives": classes abstracting I/O streams.
A template class for holding a the data and the weight of a particle.
void resetUniform(const mrpt::math::TPose3D &corner_min, const mrpt::math::TPose3D &corner_max, const int particlesCount=-1)
Reset the PDF to an uniformly distributed one, inside of the defined "cube".
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
A compile-time fixed-size numeric matrix container.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
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,...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!...
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
This template class declares the array of particles and its internal data, managing some memory-relat...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
return_t square(const num_t x)
Inline function for the square of a number.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
double pitch
Pitch coordinate (rotation angle over Y axis).
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
double yaw
Yaw coordinate (rotation angle over Z axis).
double roll
Roll coordinate (rotation angle over X coordinate).
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Computes weighted and un-weighted averages of SE(3) poses.
mrpt::math::TPose3D getParticlePose(int i) const
Returns the pose of the i'th particle.
This base provides a set of functions for maths stuff.
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF),...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
void append(CPose3DPDFParticles &o)
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020 | |