Main MRPT website > C++ reference for MRPT 1.4.0
maps/CRandomFieldGridMap2D.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9
10#ifndef CRandomFieldGridMap2D_H
11#define CRandomFieldGridMap2D_H
12
14#include <mrpt/utils/CImage.h>
16#include <mrpt/math/CMatrixD.h>
21
23#if EIGEN_VERSION_AT_LEAST(3,1,0) // eigen 3.1+
24 #include <Eigen/SparseCore>
25 #include <Eigen/SparseCholesky>
26#endif
27
28namespace mrpt
29{
30namespace maps
31{
33
34 // Pragma defined to ensure no structure packing: since we'll serialize TRandomFieldCell to streams, we want it not to depend on compiler options, etc.
35#if defined(MRPT_IS_X86_AMD64)
36#pragma pack(push,1)
37#endif
38
39 /** The contents of each cell in a CRandomFieldGridMap2D map.
40 * \ingroup mrpt_maps_grp
41 **/
43 {
44 /** Constructor */
45 TRandomFieldCell(double kfmean_dm_mean = 1e-20, double kfstd_dmmeanw = 0) :
46 kf_mean (kfmean_dm_mean),
47 kf_std (kfstd_dmmeanw),
48 dmv_var_mean (0),
49 last_updated(mrpt::system::now()),
50 updated_std (kfstd_dmmeanw)
51 { }
52
53 // *Note*: Use unions to share memory between data fields, since only a set
54 // of the variables will be used for each mapping strategy.
55 // You can access to a "TRandomFieldCell *cell" like: cell->kf_mean, cell->kf_std, etc..
56 // but accessing cell->kf_mean would also modify (i.e. ARE the same memory slot) cell->dm_mean, for example.
57
58 // Note 2: If the number of type of fields are changed in the future,
59 // *PLEASE* also update the writeToStream() and readFromStream() methods!!
60
61 union
62 {
63 double kf_mean; //!< [KF-methods only] The mean value of this cell
64 double dm_mean; //!< [Kernel-methods only] The cumulative weighted readings of this cell
65 double gmrf_mean; //!< [GMRF only] The mean value of this cell
66 };
67
68 union
69 {
70 double kf_std; //!< [KF-methods only] The standard deviation value of this cell
71 double dm_mean_w; //!< [Kernel-methods only] The cumulative weights (concentration = alpha * dm_mean / dm_mean_w + (1-alpha)*r0 )
72 double gmrf_std;
73 };
74
75 double dmv_var_mean; //!< [Kernel DM-V only] The cumulative weighted variance of this cell
76
77 mrpt::system::TTimeStamp last_updated; //!< [Dynamic maps only] The timestamp of the last time the cell was updated
78 double updated_std; //!< [Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve
79 };
80
81#if defined(MRPT_IS_X86_AMD64)
82#pragma pack(pop)
83#endif
84
85 /** CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property which is estimated by this map, either
86 * as a simple value or as a probility distribution (for each cell).
87 *
88 * There are a number of methods available to build the MRF grid-map, depending on the value of
89 * `TMapRepresentation maptype` passed in the constructor.
90 *
91 * The following papers describe the mapping alternatives implemented here:
92 * - `mrKernelDM`: A Gaussian kernel-based method. See:
93 * - "Building gas concentration gridmaps with a mobile robot", Lilienthal, A. and Duckett, T., Robotics and Autonomous Systems, v.48, 2004.
94 * - `mrKernelDMV`: A kernel-based method. See:
95 * - "A Statistical Approach to Gas Distribution Modelling with Mobile Robots--The Kernel DM+ V Algorithm", Lilienthal, A.J. and Reggente, M. and Trincavelli, M. and Blanco, J.L. and Gonzalez, J., IROS 2009.
96 * - `mrKalmanFilter`: A "brute-force" approach to estimate the entire map with a dense (linear) Kalman filter. Will be very slow for mid or large maps. It's provided just for comparison purposes, not useful in practice.
97 * - `mrKalmanApproximate`: A compressed/sparse Kalman filter approach. See:
98 * - "A Kalman Filter Based Approach to Probabilistic Gas Distribution Mapping", JL Blanco, JG Monroy, J Gonzalez-Jimenez, A Lilienthal, 28th Symposium On Applied Computing (SAC), 2013.
99 * - `mrGMRF_G` and `mrGMRF_SD`: A Gaussian Markov Random Field (GMRF) estimator, with these constraints:
100 * - `mrGMRF_G`: Each cell connected to a square area of neighbors cells.
101 * - `mrGMRF_SD`: Each cell only connected to its 4 immediate neighbors (Up, down, left, right).
102 * - See papers:
103 * - "Time-variant gas distribution mapping with obstacle information", Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots, 40(1), 1-16, 2016.
104 *
105 * Note that this class is virtual, since derived classes still have to implement:
106 * - mrpt::maps::CMetricMap::internal_computeObservationLikelihood()
107 * - mrpt::maps::CMetricMap::internal_insertObservation()
108 * - Serialization methods: writeToStream() and readFromStream()
109 *
110 * \sa mrpt::maps::CGasConcentrationGridMap2D, mrpt::maps::CWirelessPowerGridMap2D, mrpt::maps::CMetricMap, mrpt::utils::CDynamicGrid, The application icp-slam, mrpt::maps::CMultiMetricMap
111 * \ingroup mrpt_maps_grp
112 */
113 class CRandomFieldGridMap2D : public mrpt::maps::CMetricMap, public utils::CDynamicGrid<TRandomFieldCell>
114 {
116
117 // This must be added to any CSerializable derived class:
119 public:
120 static bool ENABLE_GMRF_PROFILER; //!< [default:false] Enables a profiler to show a performance report at application end.
121
122 /** Calls the base CMetricMap::clear
123 * Declared here to avoid ambiguity between the two clear() in both base classes.
124 */
125 inline void clear() { CMetricMap::clear(); }
126
127 // This method is just used for the ::saveToTextFile() method in base class.
129 {
130 return c.kf_mean;
131 }
132
133 /** The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
134 */
136 {
137 mrKernelDM = 0, //!< Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D)
138 mrAchim = 0, //!< Another alias for "mrKernelDM", for backwards compatibility (see discussion in mrpt::maps::CRandomFieldGridMap2D)
139 mrKalmanFilter, //!< "Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D)
140 mrKalmanApproximate, //!< (see discussion in mrpt::maps::CRandomFieldGridMap2D)
141 mrKernelDMV, //!< Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D)
142 mrGMRF_G, //!< Gaussian Markov Random Field, Gaussian prior weights between neighboring cells up to a certain distance (see discussion in mrpt::maps::CRandomFieldGridMap2D)
143 mrGMRF_SD //!< Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see discussion in mrpt::maps::CRandomFieldGridMap2D)
144 };
145
146 /** Constructor */
149 double x_min = -2, double x_max = 2,
150 double y_min = -2, double y_max = 2,
151 double resolution = 0.1
152 );
153
154 /** Destructor */
156
157 /** Returns true if the map is empty/no observation has been inserted (in this class it always return false,
158 * unless redefined otherwise in base classes)
159 */
160 virtual bool isEmpty() const MRPT_OVERRIDE;
161
162 /** Save the current map as a graphical file (BMP,PNG,...).
163 * The file format will be derived from the file extension (see CImage::saveToFile )
164 * It depends on the map representation model:
165 * mrAchim: Each pixel is the ratio \f$ \sum{\frac{wR}{w}} \f$
166 * mrKalmanFilter: Each pixel is the mean value of the Gaussian that represents each cell.
167 *
168 * \sa \a getAsBitmapFile()
169 */
170 virtual void saveAsBitmapFile(const std::string &filName) const;
171
172 /** Returns an image just as described in \a saveAsBitmapFile */
173 virtual void getAsBitmapFile(mrpt::utils::CImage &out_img) const;
174
175 /** Like saveAsBitmapFile(), but returns the data in matrix form (first row in the matrix is the upper (y_max) part of the map) */
176 virtual void getAsMatrix( mrpt::math::CMatrixDouble &out_mat) const;
177
178 /** Parameters common to any derived class.
179 * Derived classes should derive a new struct from this one, plus "public utils::CLoadableOptions",
180 * and call the internal_* methods where appropiate to deal with the variables declared here.
181 * Derived classes instantions of their "TInsertionOptions" MUST set the pointer "m_insertOptions_common" upon construction.
182 */
184 {
185 TInsertionOptionsCommon(); //!< Default values loader
186
187 /** See utils::CLoadableOptions */
189 const mrpt::utils::CConfigFileBase &source,
190 const std::string &section);
191
192 void internal_dumpToTextStream_common(mrpt::utils::CStream &out) const; //!< See utils::CLoadableOptions
193
194 /** @name Kernel methods (mrKernelDM, mrKernelDMV)
195 @{ */
196 float sigma; //!< The sigma of the "Parzen"-kernel Gaussian
197 float cutoffRadius; //!< The cutoff radius for updating cells.
198 float R_min,R_max; //!< Limits for normalization of sensor readings.
199 double dm_sigma_omega; //!< [DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; see CRandomFieldGridMap2D) */
200 /** @} */
201
202 /** @name Kalman-filter methods (mrKalmanFilter, mrKalmanApproximate)
203 @{ */
204 float KF_covSigma; //!< The "sigma" for the initial covariance value between cells (in meters).
205 float KF_initialCellStd; //!< The initial standard deviation of each cell's concentration (will be stored both at each cell's structure and in the covariance matrix as variances in the diagonal) (in normalized concentration units).
206 float KF_observationModelNoise; //!< The sensor model noise (in normalized concentration units).
207 float KF_defaultCellMeanValue; //!< The default value for the mean of cells' concentration.
208 uint16_t KF_W_size; //!< [mrKalmanApproximate] The size of the window of neighbor cells.
209 /** @} */
210
211 /** @name Gaussian Markov Random Fields methods (mrGMRF_G & mrGMRF_SD)
212 @{ */
213 double GMRF_lambdaPrior; //!< The information (Lambda) of fixed map constraints
214 double GMRF_lambdaObs; //!< The initial information (Lambda) of each observation (this information will decrease with time)
215 double GMRF_lambdaObsLoss; //!< The loss of information of the observations with each iteration
216
217 bool GMRF_use_occupancy_information; //!< whether to use information of an occupancy_gridmap map for buidling the GMRF
218 std::string GMRF_simplemap_file; //!< simplemap_file name of the occupancy_gridmap
219 std::string GMRF_gridmap_image_file; //!< image name of the occupancy_gridmap
220 double GMRF_gridmap_image_res; //!< occupancy_gridmap resolution: size of each pixel (m)
221 size_t GMRF_gridmap_image_cx; //!< Pixel coordinates of the origin for the occupancy_gridmap
222 size_t GMRF_gridmap_image_cy; //!< Pixel coordinates of the origin for the occupancy_gridmap
223
224 uint16_t GMRF_constraintsSize; //!< [mrGMRF_G only] The size of the Gaussian window to impose fixed restrictions between cells.
225 double GMRF_constraintsSigma; //!< [mrGMRF_G only] The sigma of the Gaussian window to impose fixed restrictions between cells.
226 double GMRF_saturate_min, GMRF_saturate_max; //!< (Default:-inf,+inf) Saturate the estimated mean in these limits
227 bool GMRF_skip_variance; //!< (Default:false) Skip the computation of the variance, just compute the mean
228 /** @} */
229 };
230
231 /** Changes the size of the grid, maintaining previous contents. \sa setSize */
232 virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const TRandomFieldCell& defaultValueNewCells, double additionalMarginMeters = 1.0f ) MRPT_OVERRIDE;
233
234 /** Changes the size of the grid, erasing previous contents. \sa resize */
235 virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const TRandomFieldCell * fill_value = NULL);
236
237 /** See docs in base class: in this class this always returns 0 */
238 float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
239
240 /** The implementation in this class just calls all the corresponding method of the contained metric maps */
241 virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
242
243 /** Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the concentration of each cell.
244 * This method can only be called in a KF map model.
245 * \sa getAsMatlab3DGraphScript */
246 virtual void saveAsMatlab3DGraph(const std::string &filName) const;
247
248 /** Return a large text block with a MATLAB script to plot the contents of this map \sa saveAsMatlab3DGraph
249 * This method can only be called in a KF map model */
250 void getAsMatlab3DGraphScript(std::string &out_script) const;
251
252 /** Returns a 3D object representing the map (mean) */
253 virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const MRPT_OVERRIDE;
254
255 /** Returns two 3D objects representing the mean and variance maps */
256 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &meanObj, mrpt::opengl::CSetOfObjectsPtr &varObj ) const;
257
258 TMapRepresentation getMapType(); //!< Return the type of the random-field grid map, according to parameters passed on construction.
259
260 /** Direct update of the map with a reading in a given position of the map, using
261 * the appropriate method according to mapType passed in the constructor.
262 *
263 * This is a direct way to update the map, an alternative to the generic insertObservation() method which works with mrpt::obs::CObservation objects.
264 */
266 const double sensorReading, //!< [in] The value observed in the (x,y) position
267 const mrpt::math::TPoint2D & point, //!< [in] The (x,y) location
268 const bool update_map = true, //!< [in] Run a global map update after inserting this observatin (algorithm-dependant)
269 const bool time_invariant = true //!< [in] Whether the observation "vanishes" with time (false) or not (true) [Only for GMRF methods]
270 );
271
275 };
276
277 /** Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form of the expected variance). */
278 virtual void predictMeasurement(
279 const double x, //!< [in] Query X coordinate
280 const double y, //!< [in] Query Y coordinate
281 double &out_predict_response, //!< [out] The output value
282 double &out_predict_response_variance, //!< [out] The output variance
283 bool do_sensor_normalization, //!< [in] Whether to renormalize the prediction to a predefined interval (`R` values in insertionOptions)
284 const TGridInterpolationMethod interp_method = gimNearest //!< [in] Interpolation method
285 );
286
287 /** Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based methods). */
289
290 /** Return the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods). */
292
293 /** Load the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods). */
295
296 void updateMapEstimation(); //!< Run the method-specific procedure required to ensure that the mean & variances are up-to-date with all inserted observations.
297
298 void enableVerbose(bool enable_verbose) { m_rfgm_verbose = enable_verbose; }
299 bool isEnabledVerbose() const { return m_rfgm_verbose; }
300
301 protected:
302 bool m_rfgm_verbose; //!< Enable verbose debug output for Random Field grid map operations (Default: false)
304
305 /** Common options to all random-field grid maps: pointer that is set to the derived-class instance of "insertOptions" upon construction of this class. */
307
308 /** Get the part of the options common to all CRandomFieldGridMap2D classes */
310
311 TMapRepresentation m_mapType; //!< The map representation type of this map, as passed in the constructor
312
313 mrpt::math::CMatrixD m_cov; //!< The whole covariance matrix, used for the Kalman Filter map representation.
314
315 /** The compressed band diagonal matrix for the KF2 implementation.
316 * The format is a Nx(W^2+2W+1) matrix, one row per cell in the grid map with the
317 * cross-covariances between each cell and half of the window around it in the grid.
318 */
320 mutable bool m_hasToRecoverMeanAndCov; //!< Only for the KF2 implementation.
321
322 /** @name Auxiliary vars for DM & DM+V methods
323 @{ */
325 std::vector<float> m_DM_gaussWindow;
328 /** @} */
329
330 /** @name Auxiliary vars for GMRF method
331 @{ */
332#if EIGEN_VERSION_AT_LEAST(3,1,0)
333 std::vector<Eigen::Triplet<double> > H_prior; // the prior part of H
334#endif
335 Eigen::VectorXd g; // Gradient vector
336 size_t nPriorFactors; // L
337 size_t nObsFactors; // M
338 size_t nFactors; // L+M
339 std::multimap<size_t,size_t> cell_interconnections; //Store the interconnections (relations) of each cell with its neighbourds
340
341 std::vector<float> gauss_val; // For factor Weigths (only for mrGMRF_G)
342
344 {
345 double obsValue;
346 double Lambda;
347 bool time_invariant; //if the observation will lose weight (lambda) as time goes on (default false)
348 };
349
350 std::vector<std::vector<TobservationGMRF> > activeObs; //Vector with the active observations and their respective Information
351
352
353 /** @} */
354
355 /** The implementation of "insertObservation" for Achim Lilienthal's map models DM & DM+V.
356 * \param normReading Is a [0,1] normalized concentration reading.
357 * \param point Is the sensor location on the map
358 * \param is_DMV = false -> map type is Kernel DM; true -> map type is DM+V
359 */
361 double normReading,
362 const mrpt::math::TPoint2D &point,
363 bool is_DMV );
364
365 /** The implementation of "insertObservation" for the (whole) Kalman Filter map model.
366 * \param normReading Is a [0,1] normalized concentration reading.
367 * \param point Is the sensor location on the map
368 */
370 double normReading,
371 const mrpt::math::TPoint2D &point );
372
373 /** The implementation of "insertObservation" for the Efficient Kalman Filter map model.
374 * \param normReading Is a [0,1] normalized concentration reading.
375 * \param point Is the sensor location on the map
376 */
378 double normReading,
379 const mrpt::math::TPoint2D &point );
380
381 /** The implementation of "insertObservation" for the Gaussian Markov Random Field map model.
382 * \param normReading Is a [0,1] normalized concentration reading.
383 * \param point Is the sensor location on the map
384 */
385 void insertObservation_GMRF(double normReading,const mrpt::math::TPoint2D &point, const bool update_map,const bool time_invariant);
386
387 /** solves the minimum quadratic system to determine the new concentration of each cell */
389
390 /** Computes the confidence of the cell concentration (alpha) */
392
393 /** Computes the average cell concentration, or the overall average value if it has never been observed */
395
396 /** Computes the estimated variance of the cell concentration, or the overall average variance if it has never been observed */
397 double computeVarCellValue_DM_DMV (const TRandomFieldCell *cell ) const;
398
399 /** In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std values.
400 * \sa m_hasToRecoverMeanAndCov
401 */
402 void recoverMeanAndCov() const;
403
404 /** Erase all the contents of the map */
406
407 /** Check if two cells of the gridmap (m_map) are connected, based on the provided occupancy gridmap*/
409 const mrpt::maps::COccupancyGridMap2D *m_Ocgridmap,
410 size_t cxo_min,
411 size_t cxo_max,
412 size_t cyo_min,
413 size_t cyo_max,
414 const size_t seed_cxo,
415 const size_t seed_cyo,
416 const size_t objective_cxo,
417 const size_t objective_cyo);
418 };
420
421
422 } // End of namespace
423
424
425 // Specializations MUST occur at the same namespace:
426 namespace utils
427 {
428 template <>
430 {
432 static void fill(bimap<enum_t,std::string> &m_map)
433 {
434 m_map.insert(maps::CRandomFieldGridMap2D::mrKernelDM, "mrKernelDM");
435 m_map.insert(maps::CRandomFieldGridMap2D::mrKalmanFilter, "mrKalmanFilter");
436 m_map.insert(maps::CRandomFieldGridMap2D::mrKalmanApproximate, "mrKalmanApproximate");
437 m_map.insert(maps::CRandomFieldGridMap2D::mrKernelDMV, "mrKernelDMV");
438 m_map.insert(maps::CRandomFieldGridMap2D::mrGMRF_G, "mrGMRF_G");
439 m_map.insert(maps::CRandomFieldGridMap2D::mrGMRF_SD, "mrGMRF_SD");
440 }
441 };
442 } // End of namespace
443} // End of namespace
444
445#endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Declares a virtual base class for all metric maps storage classes.
void clear()
Erase all the contents of the map.
A class for storing an occupancy grid map.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const TRandomFieldCell &defaultValueNewCells, double additionalMarginMeters=1.0f) MRPT_OVERRIDE
Changes the size of the grid, maintaining previous contents.
virtual void getAsMatrix(mrpt::math::CMatrixDouble &out_mat) const
Like saveAsBitmapFile(), but returns the data in matrix form (first row in the matrix is the upper (y...
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
bool m_rfgm_verbose
Enable verbose debug output for Random Field grid map operations (Default: false)
std::vector< std::vector< TobservationGMRF > > activeObs
void getMeanAndCov(mrpt::math::CVectorDouble &out_means, mrpt::math::CMatrixDouble &out_cov) const
Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based meth...
double computeMeanCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the average cell concentration, or the overall average value if it has never been observed
void insertObservation_KernelDM_DMV(double normReading, const mrpt::math::TPoint2D &point, bool is_DMV)
The implementation of "insertObservation" for Achim Lilienthal's map models DM & DM+V.
void insertIndividualReading(const double sensorReading, const mrpt::math::TPoint2D &point, const bool update_map=true, const bool time_invariant=true)
Direct update of the map with a reading in a given position of the map, using the appropriate method ...
std::multimap< size_t, size_t > cell_interconnections
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
float cell2float(const TRandomFieldCell &c) const MRPT_OVERRIDE
virtual ~CRandomFieldGridMap2D()
Destructor.
bool exist_relation_between2cells(const mrpt::maps::COccupancyGridMap2D *m_Ocgridmap, size_t cxo_min, size_t cxo_max, size_t cyo_min, size_t cyo_max, const size_t seed_cxo, const size_t seed_cyo, const size_t objective_cxo, const size_t objective_cyo)
Check if two cells of the gridmap (m_map) are connected, based on the provided occupancy gridmap.
virtual CRandomFieldGridMap2D::TInsertionOptionsCommon * getCommonInsertOptions()=0
Get the part of the options common to all CRandomFieldGridMap2D classes.
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
@ mrKalmanApproximate
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
@ mrGMRF_SD
Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see disc...
@ mrKernelDMV
Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGri...
@ mrAchim
Another alias for "mrKernelDM", for backwards compatibility (see discussion in mrpt::maps::CRandomFie...
@ mrGMRF_G
Gaussian Markov Random Field, Gaussian prior weights between neighboring cells up to a certain distan...
@ mrKalmanFilter
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D)
@ mrKernelDM
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D)
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map (mean)
void clear()
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both bas...
virtual void getAsBitmapFile(mrpt::utils::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
void getAsMatlab3DGraphScript(std::string &out_script) const
Return a large text block with a MATLAB script to plot the contents of this map.
virtual void predictMeasurement(const double x, const double y, double &out_predict_response, double &out_predict_response_variance, bool do_sensor_normalization, const TGridInterpolationMethod interp_method=gimNearest)
Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form o...
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const TRandomFieldCell *fill_value=NULL)
Changes the size of the grid, erasing previous contents.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
The implementation in this class just calls all the corresponding method of the contained metric maps...
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std ...
void insertObservation_GMRF(double normReading, const mrpt::math::TPoint2D &point, const bool update_map, const bool time_invariant)
The implementation of "insertObservation" for the Gaussian Markov Random Field map model.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
See docs in base class: in this class this always returns 0.
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
CRandomFieldGridMap2D(TMapRepresentation mapType=mrKernelDM, double x_min=-2, double x_max=2, double y_min=-2, double y_max=2, double resolution=0.1)
Constructor.
void getMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD) const
Return the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods).
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
virtual void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.
TMapRepresentation getMapType()
Return the type of the random-field grid map, according to parameters passed on construction.
static bool ENABLE_GMRF_PROFILER
[default:false] Enables a profiler to show a performance report at application end.
virtual void saveAsMatlab3DGraph(const std::string &filName) const
Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the...
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
utils::CDynamicGrid< TRandomFieldCell > BASE
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
void setMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD)
Load the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods).
void updateMapEstimation_GMRF()
solves the minimum quadratic system to determine the new concentration of each cell
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:31
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
This class allows loading and storing values and vectors of different types from a configuration text...
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:41
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
unsigned int uint16_t
Definition: pstdint.h:170
uint16_t GMRF_constraintsSize
[mrGMRF_G only] The size of the Gaussian window to impose fixed restrictions between cells.
size_t GMRF_gridmap_image_cy
Pixel coordinates of the origin for the occupancy_gridmap.
void internal_dumpToTextStream_common(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
double GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
float sigma
The sigma of the "Parzen"-kernel Gaussian.
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
size_t GMRF_gridmap_image_cx
Pixel coordinates of the origin for the occupancy_gridmap.
double GMRF_saturate_max
(Default:-inf,+inf) Saturate the estimated mean in these limits
double dm_sigma_omega
[DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; s...
float KF_covSigma
The "sigma" for the initial covariance value between cells (in meters).
std::string GMRF_gridmap_image_file
image name of the occupancy_gridmap
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
bool GMRF_use_occupancy_information
whether to use information of an occupancy_gridmap map for buidling the GMRF
float KF_initialCellStd
The initial standard deviation of each cell's concentration (will be stored both at each cell's struc...
double GMRF_gridmap_image_res
occupancy_gridmap resolution: size of each pixel (m)
double GMRF_lambdaObs
The initial information (Lambda) of each observation (this information will decrease with time)
float R_max
Limits for normalization of sensor readings.
std::string GMRF_simplemap_file
simplemap_file name of the occupancy_gridmap
float KF_defaultCellMeanValue
The default value for the mean of cells' concentration.
double GMRF_constraintsSigma
[mrGMRF_G only] The sigma of the Gaussian window to impose fixed restrictions between cells.
void internal_loadFromConfigFile_common(const mrpt::utils::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
Parameters for CMetricMap::compute3DMatchingRatio()
The contents of each cell in a CRandomFieldGridMap2D map.
TRandomFieldCell(double kfmean_dm_mean=1e-20, double kfstd_dmmeanw=0)
Constructor.
double kf_mean
[KF-methods only] The mean value of this cell
double dmv_var_mean
[Kernel DM-V only] The cumulative weighted variance of this cell
double kf_std
[KF-methods only] The standard deviation value of this cell
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha * dm_mean / dm_mean_w + (1-alpha)...
double updated_std
[Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve
double gmrf_mean
[GMRF only] The mean value of this cell
mrpt::system::TTimeStamp last_updated
[Dynamic maps only] The timestamp of the last time the cell was updated
double dm_mean
[Kernel-methods only] The cumulative weighted readings of this cell
Lightweight 2D point.
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 04:35:51 UTC 2023