Main MRPT website > C++ reference for MRPT 1.9.9
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-2017, 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 <list>
23 
24 namespace mrpt
25 {
26 namespace maps
27 {
28 class COccupancyGridMap2D;
29 
30 // Pragma defined to ensure no structure packing: since we'll serialize
31 // TRandomFieldCell to streams, we want it not to depend on compiler options,
32 // etc.
33 #if defined(MRPT_IS_X86_AMD64)
34 #pragma pack(push, 1)
35 #endif
36 
37 /** The contents of each cell in a CRandomFieldGridMap2D map.
38  * \ingroup mrpt_maps_grp
39  **/
41 {
42  /** Constructor */
43  TRandomFieldCell(double kfmean_dm_mean = 1e-20, double kfstd_dmmeanw = 0)
44  : kf_mean(kfmean_dm_mean),
45  kf_std(kfstd_dmmeanw),
46  dmv_var_mean(0),
47  last_updated(mrpt::system::now()),
48  updated_std(kfstd_dmmeanw)
49  {
50  }
51 
52  // *Note*: Use unions to share memory between data fields, since only a set
53  // of the variables will be used for each mapping strategy.
54  // You can access to a "TRandomFieldCell *cell" like: cell->kf_mean,
55  // cell->kf_std, etc..
56  // but accessing cell->kf_mean would also modify (i.e. ARE the same memory
57  // slot) cell->dm_mean, for example.
58 
59  // Note 2: If the number of type of fields are changed in the future,
60  // *PLEASE* also update the writeToStream() and readFromStream() methods!!
61 
62  union {
63  /** [KF-methods only] The mean value of this cell */
64  double kf_mean;
65  /** [Kernel-methods only] The cumulative weighted readings of this cell
66  */
67  double dm_mean;
68  /** [GMRF only] The mean value of this cell */
69  double gmrf_mean;
70  };
71 
72  union {
73  /** [KF-methods only] The standard deviation value of this cell */
74  double kf_std;
75  /** [Kernel-methods only] The cumulative weights (concentration = alpha
76  * * dm_mean / dm_mean_w + (1-alpha)*r0 ) */
77  double dm_mean_w;
78  double gmrf_std;
79  };
80 
81  /** [Kernel DM-V only] The cumulative weighted variance of this cell */
82  double dmv_var_mean;
83 
84  /** [Dynamic maps only] The timestamp of the last time the cell was updated
85  */
87  /** [Dynamic maps only] The std cell value that was updated (to be used in
88  * the Forgetting_curve */
89  double updated_std;
90 };
91 
92 #if defined(MRPT_IS_X86_AMD64)
93 #pragma pack(pop)
94 #endif
95 
96 /** CRandomFieldGridMap2D represents a 2D grid map where each cell is associated
97  *one real-valued property which is estimated by this map, either
98  * as a simple value or as a probility distribution (for each cell).
99  *
100  * There are a number of methods available to build the MRF grid-map,
101  *depending on the value of
102  * `TMapRepresentation maptype` passed in the constructor.
103  *
104  * The following papers describe the mapping alternatives implemented here:
105  * - `mrKernelDM`: A Gaussian kernel-based method. See:
106  * - "Building gas concentration gridmaps with a mobile robot",
107  *Lilienthal,
108  *A. and Duckett, T., Robotics and Autonomous Systems, v.48, 2004.
109  * - `mrKernelDMV`: A kernel-based method. See:
110  * - "A Statistical Approach to Gas Distribution Modelling with Mobile
111  *Robots--The Kernel DM+ V Algorithm", Lilienthal, A.J. and Reggente, M. and
112  *Trincavelli, M. and Blanco, J.L. and Gonzalez, J., IROS 2009.
113  * - `mrKalmanFilter`: A "brute-force" approach to estimate the entire map
114  *with a dense (linear) Kalman filter. Will be very slow for mid or large maps.
115  *It's provided just for comparison purposes, not useful in practice.
116  * - `mrKalmanApproximate`: A compressed/sparse Kalman filter approach.
117  *See:
118  * - "A Kalman Filter Based Approach to Probabilistic Gas Distribution
119  *Mapping", JL Blanco, JG Monroy, J Gonzalez-Jimenez, A Lilienthal, 28th
120  *Symposium On Applied Computing (SAC), 2013.
121  * - `mrGMRF_SD`: A Gaussian Markov Random Field (GMRF) estimator, with
122  *these
123  *constraints:
124  * - `mrGMRF_SD`: Each cell only connected to its 4 immediate neighbors
125  *(Up,
126  *down, left, right).
127  * - (Removed in MRPT 1.5.0: `mrGMRF_G`: Each cell connected to a
128  *square
129  *area
130  *of neighbors cells)
131  * - See papers:
132  * - "Time-variant gas distribution mapping with obstacle
133  *information",
134  *Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots,
135  *40(1), 1-16, 2016.
136  *
137  * Note that this class is virtual, since derived classes still have to
138  *implement:
139  * - mrpt::maps::CMetricMap::internal_computeObservationLikelihood()
140  * - mrpt::maps::CMetricMap::internal_insertObservation()
141  * - Serialization methods: writeToStream() and readFromStream()
142  *
143  * [GMRF only] A custom connectivity pattern between cells can be defined by
144  *calling setCellsConnectivity().
145  *
146  * \sa mrpt::maps::CGasConcentrationGridMap2D,
147  *mrpt::maps::CWirelessPowerGridMap2D, mrpt::maps::CMetricMap,
148  *mrpt::utils::CDynamicGrid, The application icp-slam,
149  *mrpt::maps::CMultiMetricMap
150  * \ingroup mrpt_maps_grp
151  */
153  : public mrpt::maps::CMetricMap,
154  public mrpt::utils::CDynamicGrid<TRandomFieldCell>,
155  public mrpt::utils::COutputLogger
156 {
158 
160  public:
161  /** Calls the base CMetricMap::clear
162  * Declared here to avoid ambiguity between the two clear() in both base
163  * classes.
164  */
165  inline void clear() { CMetricMap::clear(); }
166  // This method is just used for the ::saveToTextFile() method in base class.
167  float cell2float(const TRandomFieldCell& c) const override
168  {
169  return c.kf_mean;
170  }
171 
172  /** The type of map representation to be used, see CRandomFieldGridMap2D for
173  * a discussion.
174  */
176  {
177  /** Gaussian kernel-based estimator (see discussion in
178  mrpt::maps::CRandomFieldGridMap2D) */
180  /** Another alias for "mrKernelDM", for backwards compatibility (see
181  discussion in mrpt::maps::CRandomFieldGridMap2D) */
182  mrAchim = 0,
183  /** "Brute-force" Kalman filter (see discussion in
184  mrpt::maps::CRandomFieldGridMap2D) */
186  /** (see discussion in mrpt::maps::CRandomFieldGridMap2D) */
188  /** Double mean + variance Gaussian kernel-based estimator (see
189  discussion in mrpt::maps::CRandomFieldGridMap2D) */
191  // Removed in MRPT 1.5.0: mrGMRF_G, //!< Gaussian Markov Random Field,
192  // Gaussian prior weights between neighboring cells up to a certain
193  // distance (see discussion in mrpt::maps::CRandomFieldGridMap2D)
194  /** Gaussian Markov Random Field, squared differences prior weights
195  between 4 neighboring cells (see discussion in
196  mrpt::maps::CRandomFieldGridMap2D) */
198  };
199 
200  /** Constructor */
202  TMapRepresentation mapType = mrKernelDM, double x_min = -2,
203  double x_max = 2, double y_min = -2, double y_max = 2,
204  double resolution = 0.1);
205 
206  /** Destructor */
207  virtual ~CRandomFieldGridMap2D();
208 
209  /** Returns true if the map is empty/no observation has been inserted (in
210  * this class it always return false,
211  * unless redefined otherwise in base classes)
212  */
213  virtual bool isEmpty() const override;
214 
215  /** Save the current map as a graphical file (BMP,PNG,...).
216  * The file format will be derived from the file extension (see
217  *CImage::saveToFile )
218  * It depends on the map representation model:
219  * mrAchim: Each pixel is the ratio \f$ \sum{\frac{wR}{w}} \f$
220  * mrKalmanFilter: Each pixel is the mean value of the Gaussian that
221  *represents each cell.
222  *
223  * \sa \a getAsBitmapFile()
224  */
225  virtual void saveAsBitmapFile(const std::string& filName) const;
226 
227  /** Returns an image just as described in \a saveAsBitmapFile */
228  virtual void getAsBitmapFile(mrpt::utils::CImage& out_img) const;
229 
230  /** Like saveAsBitmapFile(), but returns the data in matrix form (first row
231  * in the matrix is the upper (y_max) part of the map) */
232  virtual void getAsMatrix(mrpt::math::CMatrixDouble& out_mat) const;
233 
234  /** Parameters common to any derived class.
235  * Derived classes should derive a new struct from this one, plus "public
236  * utils::CLoadableOptions",
237  * and call the internal_* methods where appropiate to deal with the
238  * variables declared here.
239  * Derived classes instantions of their "TInsertionOptions" MUST set the
240  * pointer "m_insertOptions_common" upon construction.
241  */
243  {
244  /** Default values loader */
246 
247  /** See utils::CLoadableOptions */
250  const std::string& section);
251 
252  /** See utils::CLoadableOptions */
254 
255  /** @name Kernel methods (mrKernelDM, mrKernelDMV)
256  @{ */
257  /** The sigma of the "Parzen"-kernel Gaussian */
258  float sigma;
259  /** The cutoff radius for updating cells. */
261  /** Limits for normalization of sensor readings. */
262  float R_min, R_max;
263  /** [DM/DM+V methods] The scaling parameter for the confidence "alpha"
264  * values (see the IROS 2009 paper; see CRandomFieldGridMap2D) */
266  /** @} */
267 
268  /** @name Kalman-filter methods (mrKalmanFilter, mrKalmanApproximate)
269  @{ */
270  /** The "sigma" for the initial covariance value between cells (in
271  * meters). */
272  float KF_covSigma;
273  /** The initial standard deviation of each cell's concentration (will be
274  * stored both at each cell's structure and in the covariance matrix as
275  * variances in the diagonal) (in normalized concentration units). */
277  /** The sensor model noise (in normalized concentration units). */
279  /** The default value for the mean of cells' concentration. */
281  /** [mrKalmanApproximate] The size of the window of neighbor cells. */
283  /** @} */
284 
285  /** @name Gaussian Markov Random Fields methods (mrGMRF_SD)
286  @{ */
287  /** The information (Lambda) of fixed map constraints */
289  /** The initial information (Lambda) of each observation (this
290  * information will decrease with time) */
292  /** The loss of information of the observations with each iteration */
294 
295  /** whether to use information of an occupancy_gridmap map for building
296  * the GMRF */
298  /** simplemap_file name of the occupancy_gridmap */
300  /** image name of the occupancy_gridmap */
302  /** occupancy_gridmap resolution: size of each pixel (m) */
304  /** Pixel coordinates of the origin for the occupancy_gridmap */
306  /** Pixel coordinates of the origin for the occupancy_gridmap */
308 
309  /** (Default:-inf,+inf) Saturate the estimated mean in these limits */
311  /** (Default:false) Skip the computation of the variance, just compute
312  * the mean */
314  /** @} */
315  };
316 
317  /** Changes the size of the grid, maintaining previous contents. \sa setSize
318  */
319  virtual void resize(
320  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
321  const TRandomFieldCell& defaultValueNewCells,
322  double additionalMarginMeters = 1.0f) override;
323 
324  /** Changes the size of the grid, erasing previous contents.
325  * \param[in] connectivity_descriptor Optional user-supplied object that
326  * will visit all grid cells to define their connectivity with neighbors and
327  * the strength of existing edges. If present, it overrides all options in
328  * insertionOptions
329  * \sa resize
330  */
331  virtual void setSize(
332  const double x_min, const double x_max, const double y_min,
333  const double y_max, const double resolution,
334  const TRandomFieldCell* fill_value = nullptr);
335 
336  /** Base class for user-supplied objects capable of describing cells
337  * connectivity, used to build prior factors of the MRF graph. \sa
338  * setCellsConnectivity() */
340  {
341  using Ptr = std::shared_ptr<ConnectivityDescriptor>;
343  virtual ~ConnectivityDescriptor();
344 
345  /** Implement the check of whether node i=(icx,icy) is connected with
346  * node j=(jcx,jcy).
347  * This visitor method will be called only for immediate neighbors.
348  * \return true if connected (and the "information" value should be
349  * also updated in out_edge_information), false otherwise.
350  */
351  virtual bool getEdgeInformation(
352  /** The parent map on which we are running */
353  const CRandomFieldGridMap2D* parent,
354  /** (cx,cy) for node "i" */
355  size_t icx, size_t icy,
356  /** (cx,cy) for node "j" */
357  size_t jcx, size_t jcy,
358  /** Must output here the inverse of the variance of the constraint
359  edge. */
360  double& out_edge_information) = 0;
361  };
362 
363  /** Sets a custom object to define the connectivity between cells. Must call
364  * clear() or setSize() afterwards for the changes to take place. */
366  const ConnectivityDescriptor::Ptr& new_connectivity_descriptor);
367 
368  /** See docs in base class: in this class this always returns 0 */
370  const mrpt::maps::CMetricMap* otherMap,
371  const mrpt::poses::CPose3D& otherMapPose,
372  const TMatchingRatioParams& params) const override;
373 
374  /** The implementation in this class just calls all the corresponding method
375  * of the contained metric maps */
377  const std::string& filNamePrefix) const override;
378 
379  /** Save a matlab ".m" file which represents as 3D surfaces the mean and a
380  * given confidence level for the concentration of each cell.
381  * This method can only be called in a KF map model.
382  * \sa getAsMatlab3DGraphScript */
383  virtual void saveAsMatlab3DGraph(const std::string& filName) const;
384 
385  /** Return a large text block with a MATLAB script to plot the contents of
386  * this map \sa saveAsMatlab3DGraph
387  * This method can only be called in a KF map model */
388  void getAsMatlab3DGraphScript(std::string& out_script) const;
389 
390  /** Returns a 3D object representing the map (mean) */
391  virtual void getAs3DObject(
392  mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
393 
394  /** Returns two 3D objects representing the mean and variance maps */
395  virtual void getAs3DObject(
397  mrpt::opengl::CSetOfObjects::Ptr& varObj) const;
398 
399  /** Return the type of the random-field grid map, according to parameters
400  * passed on construction. */
402 
403  /** Direct update of the map with a reading in a given position of the map,
404  * using
405  * the appropriate method according to mapType passed in the constructor.
406  *
407  * This is a direct way to update the map, an alternative to the generic
408  * insertObservation() method which works with mrpt::obs::CObservation
409  * objects.
410  */
412  /** [in] The value observed in the (x,y) position */
413  const double sensorReading,
414  /** [in] The (x,y) location */
415  const mrpt::math::TPoint2D& point,
416  /** [in] Run a global map update after inserting this observatin
417  (algorithm-dependant) */
418  const bool update_map = true,
419  /** [in] Whether the observation "vanishes" with time (false) or not
420  (true) [Only for GMRF methods] */
421  const bool time_invariant = true,
422  /** [in] The uncertainty (standard deviation) of the reading.
423  Default="0.0" means use the default settings per map-wide parameters.
424  */
425  const double reading_stddev = .0);
426 
428  {
431  };
432 
433  /** Returns the prediction of the measurement at some (x,y) coordinates, and
434  * its certainty (in the form of the expected variance). */
435  virtual void predictMeasurement(
436  /** [in] Query X coordinate */
437  const double x,
438  /** [in] Query Y coordinate */
439  const double y,
440  /** [out] The output value */
441  double& out_predict_response,
442  /** [out] The output variance */
443  double& out_predict_response_variance,
444  /** [in] Whether to renormalize the prediction to a predefined
445  interval (`R` values in insertionOptions) */
446  bool do_sensor_normalization,
447  /** [in] Interpolation method */
448  const TGridInterpolationMethod interp_method = gimNearest);
449 
450  /** Return the mean and covariance vector of the full Kalman filter estimate
451  * (works for all KF-based methods). */
452  void getMeanAndCov(
453  mrpt::math::CVectorDouble& out_means,
454  mrpt::math::CMatrixDouble& out_cov) const;
455 
456  /** Return the mean and STD vectors of the full Kalman filter estimate
457  * (works for all KF-based methods). */
458  void getMeanAndSTD(
459  mrpt::math::CVectorDouble& out_means,
460  mrpt::math::CVectorDouble& out_STD) const;
461 
462  /** Load the mean and STD vectors of the full Kalman filter estimate (works
463  * for all KF-based methods). */
464  void setMeanAndSTD(
465  mrpt::math::CVectorDouble& out_means,
466  mrpt::math::CVectorDouble& out_STD);
467 
468  /** Run the method-specific procedure required to ensure that the mean &
469  * variances are up-to-date with all inserted observations. */
470  void updateMapEstimation();
471 
472  void enableVerbose(bool enable_verbose)
473  {
474  this->setMinLoggingLevel(mrpt::utils::LVL_DEBUG);
475  }
476  bool isEnabledVerbose() const
477  {
478  return this->getMinLoggingLevel() == mrpt::utils::LVL_DEBUG;
479  }
480 
481  void enableProfiler(bool enable = true)
482  {
483  this->m_gmrf.enableProfiler(enable);
484  }
485  bool isProfilerEnabled() const { return this->m_gmrf.isProfilerEnabled(); }
486  protected:
488 
489  /** Common options to all random-field grid maps: pointer that is set to the
490  * derived-class instance of "insertOptions" upon construction of this
491  * class. */
493 
494  /** Get the part of the options common to all CRandomFieldGridMap2D classes
495  */
498 
499  /** The map representation type of this map, as passed in the constructor */
501 
502  /** The whole covariance matrix, used for the Kalman Filter map
503  * representation. */
505 
506  /** The compressed band diagonal matrix for the KF2 implementation.
507  * The format is a Nx(W^2+2W+1) matrix, one row per cell in the grid map
508  * with the
509  * cross-covariances between each cell and half of the window around it
510  * in the grid.
511  */
513  /** Only for the KF2 implementation. */
515 
516  /** @name Auxiliary vars for DM & DM+V methods
517  @{ */
519  std::vector<float> m_DM_gaussWindow;
522  /** @} */
523 
524  /** Empty: default */
526 
528 
531  {
532  /** Observation value */
533  double obsValue;
534  /** "Information" of the observation (=inverse of the variance) */
535  double Lambda;
536  /** whether the observation will lose weight (lambda) as time goes on
537  * (default false) */
539 
540  double evaluateResidual() const override;
541  double getInformation() const override;
542  void evalJacobian(double& dr_dx) const override;
543 
545  : obsValue(.0), Lambda(.0), time_invariant(false), m_parent(&parent)
546  {
547  }
548 
549  private:
551  };
552 
555  {
556  /** "Information" of the observation (=inverse of the variance) */
557  double Lambda;
558 
559  double evaluateResidual() const override;
560  double getInformation() const override;
561  void evalJacobian(double& dr_dx_i, double& dr_dx_j) const override;
562 
564  : Lambda(.0), m_parent(&parent)
565  {
566  }
567 
568  private:
570  };
571 
572  // Important: converted to a std::list<> so pointers are NOT invalidated
573  // upon deletion.
574  /** Vector with the active observations and their respective Information */
575  std::vector<std::list<TObservationGMRF>> m_mrf_factors_activeObs;
576  /** Vector with the precomputed priors for each GMRF model */
577  std::deque<TPriorFactorGMRF> m_mrf_factors_priors;
578 
579  /** The implementation of "insertObservation" for Achim Lilienthal's map
580  * models DM & DM+V.
581  * \param normReading Is a [0,1] normalized concentration reading.
582  * \param point Is the sensor location on the map
583  * \param is_DMV = false -> map type is Kernel DM; true -> map type is DM+V
584  */
586  double normReading, const mrpt::math::TPoint2D& point, bool is_DMV);
587 
588  /** The implementation of "insertObservation" for the (whole) Kalman Filter
589  * map model.
590  * \param normReading Is a [0,1] normalized concentration reading.
591  * \param point Is the sensor location on the map
592  */
594  double normReading, const mrpt::math::TPoint2D& point);
595 
596  /** The implementation of "insertObservation" for the Efficient Kalman
597  * Filter map model.
598  * \param normReading Is a [0,1] normalized concentration reading.
599  * \param point Is the sensor location on the map
600  */
602  double normReading, const mrpt::math::TPoint2D& point);
603 
604  /** The implementation of "insertObservation" for the Gaussian Markov Random
605  * Field map model.
606  * \param normReading Is a [0,1] normalized concentration reading.
607  * \param point Is the sensor location on the map
608  */
610  double normReading, const mrpt::math::TPoint2D& point,
611  const bool update_map, const bool time_invariant,
612  const double reading_information);
613 
614  /** solves the minimum quadratic system to determine the new concentration
615  * of each cell */
617 
618  /** Computes the confidence of the cell concentration (alpha) */
620  const TRandomFieldCell* cell) const;
621 
622  /** Computes the average cell concentration, or the overall average value if
623  * it has never been observed */
624  double computeMeanCellValue_DM_DMV(const TRandomFieldCell* cell) const;
625 
626  /** Computes the estimated variance of the cell concentration, or the
627  * overall average variance if it has never been observed */
628  double computeVarCellValue_DM_DMV(const TRandomFieldCell* cell) const;
629 
630  /** In the KF2 implementation, takes the auxiliary matrices and from them
631  * update the cells' mean and std values.
632  * \sa m_hasToRecoverMeanAndCov
633  */
634  void recoverMeanAndCov() const;
635 
636  /** Erase all the contents of the map */
637  virtual void internal_clear() override;
638 
639  /** Check if two cells of the gridmap (m_map) are connected, based on the
640  * provided occupancy gridmap*/
642  const mrpt::maps::COccupancyGridMap2D* m_Ocgridmap, size_t cxo_min,
643  size_t cxo_max, size_t cyo_min, size_t cyo_max, const size_t seed_cxo,
644  const size_t seed_cyo, const size_t objective_cxo,
645  const size_t objective_cyo);
646 };
647 
648 } // End of namespace
649 
650 // Specializations MUST occur at the same namespace:
651 namespace utils
652 {
653 template <>
654 struct TEnumTypeFiller<maps::CRandomFieldGridMap2D::TMapRepresentation>
655 {
657  static void fill(bimap<enum_t, std::string>& m_map)
658  {
659  m_map.insert(maps::CRandomFieldGridMap2D::mrKernelDM, "mrKernelDM");
660  m_map.insert(
662  m_map.insert(
664  "mrKalmanApproximate");
665  m_map.insert(maps::CRandomFieldGridMap2D::mrKernelDMV, "mrKernelDMV");
667  }
668 };
669 } // End of namespace
670 } // End of namespace
671 
672 #endif
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:32
Simple, scalar (1-dim) constraint (edge) for a GMRF.
std::string GMRF_gridmap_image_file
image name of the occupancy_gridmap
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
Parameters for CMetricMap::compute3DMatchingRatio()
float sigma
The sigma of the "Parzen"-kernel Gaussian.
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)...
void clear()
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both bas...
double evaluateResidual() const override
Return the residual/error of this observation.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:25
unsigned __int16 uint16_t
Definition: rptypes.h:44
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...
float KF_defaultCellMeanValue
The default value for the mean of cells&#39; concentration.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:118
double getInformation() const override
Return the inverse of the variance of this constraint.
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...
Base class for user-supplied objects capable of describing cells connectivity, used to build prior fa...
void insertObservation_GMRF(double normReading, const mrpt::math::TPoint2D &point, const bool update_map, const bool time_invariant, const double reading_information)
The implementation of "insertObservation" for the Gaussian Markov Random Field map model...
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
std::vector< std::list< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
void internal_loadFromConfigFile_common(const mrpt::utils::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
void internal_dumpToTextStream_common(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:42
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:76
double Lambda
"Information" of the observation (=inverse of the variance)
mrpt::system::TTimeStamp last_updated
[Dynamic maps only] The timestamp of the last time the cell was updated
Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGri...
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
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 gmrf_mean
[GMRF only] The mean value of this cell
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
float cutoffRadius
The cutoff radius for updating cells.
void enableVerbose(bool enable_verbose)
Another alias for "mrKernelDM", for backwards compatibility (see discussion in mrpt::maps::CRandomFie...
TMapRepresentation getMapType()
Return the type of the random-field grid map, according to parameters passed on construction.
void enableProfiler(bool enable=true)
void evalJacobian(double &dr_dx) const override
Returns the derivative of the residual wrt the node value.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
double computeMeanCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the average cell concentration, or the overall average value if it has never been observed...
virtual bool getEdgeInformation(const CRandomFieldGridMap2D *parent, size_t icx, size_t icy, size_t jcx, size_t jcy, double &out_edge_information)=0
Implement the check of whether node i=(icx,icy) is connected with node j=(jcx,jcy).
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells&#39; mean and std ...
This class allows loading and storing values and vectors of different types from a configuration text...
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
mrpt::graphs::ScalarFactorGraph m_gmrf
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.
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha
double getInformation() const override
Return the inverse of the variance of this constraint.
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
bool time_invariant
whether the observation will lose weight (lambda) as time goes on (default false) ...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:41
float R_min
Limits for normalization of sensor readings.
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
TRandomFieldCell(double kfmean_dm_mean=1e-20, double kfstd_dmmeanw=0)
Constructor.
virtual bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
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) override
Changes the size of the grid, maintaining previous contents.
void updateMapEstimation_GMRF()
solves the minimum quadratic system to determine the new concentration of each cell ...
double kf_mean
[KF-methods only] The mean value of this cell
The contents of each cell in a CRandomFieldGridMap2D map.
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)...
const GLubyte * c
Definition: glext.h:6313
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:34
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=nullptr)
Changes the size of the grid, erasing previous contents.
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:32
void insertObservation_KernelDM_DMV(double normReading, const mrpt::math::TPoint2D &point, bool is_DMV)
The implementation of "insertObservation" for Achim Lilienthal&#39;s map models DM & DM+V.
float cell2float(const TRandomFieldCell &c) const override
double GMRF_lambdaObs
The initial information (Lambda) of each observation (this information will decrease with time) ...
double dm_sigma_omega
[DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; s...
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
ConnectivityDescriptor::Ptr m_gmrf_connectivity
Empty: default.
GLsizei const GLchar ** string
Definition: glext.h:4101
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
void setCellsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
double updated_std
[Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve ...
double GMRF_saturate_min
(Default:-inf,+inf) Saturate the estimated mean in these limits
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see disc...
float KF_covSigma
The "sigma" for the initial covariance value between cells (in meters).
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:55
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
The implementation in this class just calls all the corresponding method of the contained metric maps...
virtual void internal_clear() override
Erase all the contents of the map.
virtual void getAsBitmapFile(mrpt::utils::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
double dmv_var_mean
[Kernel DM-V only] The cumulative weighted variance of this cell
bool GMRF_use_occupancy_information
whether to use information of an occupancy_gridmap map for building the GMRF
std::string GMRF_simplemap_file
simplemap_file name of the occupancy_gridmap
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
GLenum GLint GLint y
Definition: glext.h:3538
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
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_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
virtual CRandomFieldGridMap2D::TInsertionOptionsCommon * getCommonInsertOptions()=0
Get the part of the options common to all CRandomFieldGridMap2D classes.
double evaluateResidual() const override
Return the residual/error of this observation.
double Lambda
"Information" of the observation (=inverse of the variance)
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:75
Simple, scalar (1-dim) constraint (edge) for a GMRF.
double dm_mean
[Kernel-methods only] The cumulative weighted readings of this cell
size_t GMRF_gridmap_image_cx
Pixel coordinates of the origin for the occupancy_gridmap.
std::deque< TPriorFactorGMRF > m_mrf_factors_priors
Vector with the precomputed priors for each GMRF model.
GLenum GLint x
Definition: glext.h:3538
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
Lightweight 2D point.
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
double GMRF_gridmap_image_res
occupancy_gridmap resolution: size of each pixel (m)
GLenum const GLfloat * params
Definition: glext.h:3534
float KF_initialCellStd
The initial standard deviation of each cell&#39;s concentration (will be stored both at each cell&#39;s struc...
size_t GMRF_gridmap_image_cy
Pixel coordinates of the origin for the occupancy_gridmap.
utils::CDynamicGrid< TRandomFieldCell > BASE
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...
double kf_std
[KF-methods only] The standard deviation value of this cell
double GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
void getAsMatlab3DGraphScript(std::string &out_script) const
Return a large text block with a MATLAB script to plot the contents of this map.
void insertIndividualReading(const double sensorReading, const mrpt::math::TPoint2D &point, const bool update_map=true, const bool time_invariant=true, const double reading_stddev=.0)
Direct update of the map with a reading in a given position of the map, using the appropriate method ...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
See docs in base class: in this class this always returns 0.
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019