/
CPosePDFParticlesExtended.h
321 lines (272 loc) · 10 KB
/
CPosePDFParticlesExtended.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
/* +------------------------------------------------------------------------+
| Mobile Robot Programming Toolkit (MRPT) |
| http://www.mrpt.org/ |
| |
| Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
| See: http://www.mrpt.org/Authors - All rights reserved. |
| Released under BSD License. See details in http://www.mrpt.org/License |
+------------------------------------------------------------------------+ */
#ifndef CPosePDFParticlesExtended_H
#define CPosePDFParticlesExtended_H
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/bayes/CProbabilityParticle.h>
#include <mrpt/bayes/CParticleFilterCapable.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CSensoryFrame.h>
namespace mrpt
{
namespace poses
{
class TExtendedCPose2D
{
public:
CPose2D pose;
mrpt::math::CVectorDouble state;
};
#define DUMMY_LINKAGE
/** Declares a class that represents a Probability Distribution
* function (PDF) of a 2D pose (x,y,phi).
* This class implements that PDF using a set of particles
* (for using particle filter methods), where M weighted
* particles represent the PDF.
*
* \sa CPose2D, CPosePDF, CPoseGaussianPDF
*/
class CPosePDFParticlesExtended
: public CPosePDF,
public mrpt::bayes::CParticleFilterData<TExtendedCPose2D>,
public mrpt::bayes::CParticleFilterDataImpl<
CPosePDFParticlesExtended,
mrpt::bayes::CParticleFilterData<TExtendedCPose2D>::CParticleList>
{
DEFINE_SERIALIZABLE(CPosePDFParticlesExtended)
public:
/** Free all the memory associated to particles, and set the number of parts
* = 0 */
void clear();
public:
/** Constructor
* \param M The number of particles.
*/
CPosePDFParticlesExtended(size_t M = 1);
/** Copy constructor:
*/
CPosePDFParticlesExtended(const CPosePDFParticlesExtended& obj)
{
copyFrom(obj);
}
/** Destructor
*/
virtual ~CPosePDFParticlesExtended();
/** Copy operator, translating if necesary (for example, between particles
* and gaussian representations)
*/
void copyFrom(const CPosePDF& o) override;
/** Reset the PDF to a single point: All particles will be set exactly to
* the supplied pose.
* \param location The location to set all the particles.
* \param particlesCount If this is set to -1 the number of particles
* remains unchanged.
* \sa resetUniform, resetUniformFreeSpace
*/
void resetDeterministic(
TExtendedCPose2D& location, int particlesCount = -1);
/** Reset the PDF to an uniformly distributed one, inside of the defined
* cube.
* If particlesCount is set to -1 the number of particles remains
* unchanged.
* \sa resetDeterministic, resetUniformFreeSpace
*/
void resetUniform(
float x_min, float x_max, float y_min, float y_max,
mrpt::math::CVectorFloat state_min, mrpt::math::CVectorFloat state_max,
float phi_min = -M_PI, float phi_max = M_PI, int particlesCount = -1);
/** Returns an estimate of the pose, i.e. a "mean value", computed
* as a weighted average over all particles.
*/
void getMean(CPose2D& p) const override;
/** Returns an estimate of the pose-state, i.e. a "mean value", computed as
* a weighted average over all particles.
*/
TExtendedCPose2D getEstimatedPoseState() const;
/** Returns an estimate of the pose covariance matrix (3x3 cov.matrix for
* x,y,phi variables)
*/
void getCovarianceAndMean(
mrpt::math::CMatrixDouble33& C, CPose2D& p) const override;
/** Returns the pose of the i'th particle.
*/
CPose2D getParticlePose(int i) const;
/** Save PDF's particles to a text file. In each line it will go: "x y phi
* weight"
*/
void saveToTextFile(const std::string& file) const override;
/** Get the particles count (equivalent to "particlesCount")
*/
size_t size() const { return m_particles.size(); }
/** This can be used to convert a PDF from local coordinates to global,
* providing the point (newReferenceBase) from which
* "to proyect" the current pdf. Result PDF substituted the currently
* stored one in the object.
*/
void changeCoordinatesReference(
const mrpt::poses::CPose3D& newReferenceBase) override;
/** Draws a single sample from the distribution (WARNING: weights are
* assumed to be normalized!)
*/
void drawSingleSample(mrpt::poses::CPose2D& outPart) const override;
/** Draws a number of samples from the distribution, and saves as a list of
* 1x3 vectors, where each row contains a (x,y,phi) datum.
*/
void drawManySamples(
size_t N,
std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
/** Appends (pose-composition) a given pose "p" to each particle
*/
void operator+=(const mrpt::poses::CPose2D& Ap);
/** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
*/
void inverse(mrpt::poses::CPosePDF& o) const override;
/** Returns the particle with the highest weight.
*/
CPose2D getMostLikelyParticle() const;
/** Bayesian fusion.
*/
void bayesianFusion(
const mrpt::poses::CPosePDF& p1, const mrpt::poses::CPosePDF& p2,
const double& minMahalanobisDistToDrop = 0) override;
/** Evaluates the PDF at a given arbitrary point as reconstructed by a
* Parzen window.
* \sa saveParzenPDFToTextFile
*/
double evaluatePDF_parzen(
float x, float y, float phi, float stdXY, float stdPhi) const;
/** Save a text file (compatible with matlab) representing the 2D evaluation
* of the PDF as reconstructed by a Parzen window.
* \sa evaluatePDF_parzen
*/
void saveParzenPDFToTextFile(
const char* fileName, float x_min, float x_max, float y_min,
float y_max, float phi, float stepSizeXY, float stdXY,
float stdPhi) const;
void bayesianFusion(
mrpt::poses::CPosePDF& p1, mrpt::poses::CPosePDF& p2,
const double& minMahalanobisDistToDrop = 0)
{
THROW_EXCEPTION("Not implemented");
}
}; // End of class def.
class CPosePDFParticlesExtendedPF
{
public:
using CParticleList = CPosePDFParticlesExtended::CParticleList;
CPosePDFParticlesExtended m_poseParticles;
CPosePDFParticlesExtendedPF(size_t M = 1) : m_poseParticles(M) {}
/** The struct for passing extra simulation parameters to the prediction
* stage
* when running a particle filter.
* \sa prediction
*/
struct TPredictionParams
{
/** Default settings method.
*/
TPredictionParams();
/** [update stage] Must be set to a metric map used to estimate the
* likelihood of observations
*/
mrpt::maps::CMetricMap* metricMap;
/** [update stage] Alternative way (if metricMap==nullptr): A metric map
* is supplied for each particle: There must be the same maps here as
* pose particles.
*/
mrpt::maps::TMetricMapList metricMaps;
/** Parameters for the KLD adaptive sample size algorithm (see Dieter
* Fox's papers), which is used only if the CParticleFilter is created
* with the "adaptiveSampleSize" flag set to true.
*/
float KLD_binSize_XY, KLD_binSize_PHI, KLD_delta, KLD_epsilon;
/** Parameters for the KLD adaptive sample size algorithm (see Dieter
* Fox's papers), which is used only if the CParticleFilter is created
* with the "adaptiveSampleSize" flag set to true.
*/
unsigned int KLD_minSampleSize, KLD_maxSampleSize;
/** In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", the number
* of samples for searching the maximum likelihood value (see papers!)
*/
unsigned int pfAuxFilterOptimal_MaximumSearchSamples;
/** The probability [0,1] of changing the UWB bias (individual for each
* beacon's bias).
*/
float probabilityChangingBias;
/** The bias of each beacon will be added a uniform random variable
* [-changingBiasUnifRange/2,changingBiasUnifRange/2] with a probability
* "probabilityChangingBias".
*/
float changingBiasUnifRange;
/** A number between 0 and 1 (0=standard proposal).
*/
float mixtureProposalRatio;
} options;
/** Update the particles, predicting the posterior of robot pose and map
* after a movement command.
* This method has additional configuration parameters in "options".
* Performs the update stage of the RBPF, using the sensed sensory Frame:
*
* \param action This is a pointer to CActionCollection, containing the
* pose change the robot has been commanded.
* \param observation This must be a pointer to a CsensoryFrame object,
* with robot sensed observations.
*
* \sa options
*/
template <typename PF_ALGORITHM>
void prediction_and_update(
const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
void executeOn(
mrpt::bayes::CParticleFilter& pf, const mrpt::obs::CActionCollection* action,
const mrpt::obs::CSensoryFrame* observation,
mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithms);
private:
void offsetTransitionModel(double& val);
/** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
*/
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb;
/** Auxiliary function that evaluates the likelihood of an observation,
* given a robot pose, and according to the options in
* "CPosePDFParticlesExtended::options".
*/
double auxiliarComputeObservationLikelihood(
const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
size_t particleIndexForMap, const mrpt::obs::CSensoryFrame* observation,
const TExtendedCPose2D* x);
/** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
*/
double particlesEvaluator_AuxPFOptimal(
const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
size_t index, const CPose2D* action,
const mrpt::obs::CSensoryFrame* observation);
};
/** Auxiliary structure
*/
struct TPoseBin
{
int x, y, phi;
};
/** Auxiliary structure
*/
struct lt_TPoseBin
{
bool operator()(const TPoseBin& s1, const TPoseBin& s2) const
{
return s1.x < s2.x;
}
};
} // namespace poses
} // namespace mrpt
#endif