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>
18 #include <mrpt/utils/TEnumType.h>
19 #include <mrpt/maps/CMetricMap.h>
21 
22 #include <mrpt/maps/link_pragmas.h>
23 #if EIGEN_VERSION_AT_LEAST(3,1,0) // eigen 3.1+
24  #include <Eigen/SparseCore>
25  #include <Eigen/SparseCholesky>
26 #endif
27 
28 namespace mrpt
29 {
30 namespace 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 */
148  TMapRepresentation mapType = mrKernelDM,
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 
274  gimBilinear
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 */
394  double computeMeanCellValue_DM_DMV (const TRandomFieldCell *cell ) const;
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  {
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.
virtual CRandomFieldGridMap2D::TInsertionOptionsCommon * getCommonInsertOptions()=0
Get the part of the options common to all CRandomFieldGridMap2D classes.
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.
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
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:43
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
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.1 for MRPT 1.4.0 SVN: at Mon Apr 18 04:07:33 UTC 2022