MRPT  2.0.4
CRandomFieldGridMap2D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
15 #include <mrpt/img/CImage.h>
16 #include <mrpt/maps/CMetricMap.h>
17 #include <mrpt/math/CMatrixD.h>
20 #include <list>
21 
22 namespace mrpt::maps
23 {
24 class COccupancyGridMap2D;
25 
26 // Pragma defined to ensure no structure packing: since we'll serialize
27 // TRandomFieldCell to streams, we want it not to depend on compiler options,
28 // etc.
29 #if defined(MRPT_IS_X86_AMD64)
30 #pragma pack(push, 1)
31 #endif
32 
33 /** The contents of each cell in a CRandomFieldGridMap2D map.
34  * \ingroup mrpt_maps_grp
35  **/
37 {
38  /** Constructor */
39  TRandomFieldCell(double kfmean_dm_mean = 1e-20, double kfstd_dmmeanw = 0)
40  : param1_(kfmean_dm_mean),
41  param2_(kfstd_dmmeanw),
42  last_updated(mrpt::system::now()),
43  updated_std(kfstd_dmmeanw)
44  {
45  }
46 
47  // *Note*: Use unions to share memory between data fields, since only a set
48  // of the variables will be used for each mapping strategy.
49  // You can access to a "TRandomFieldCell *cell" like: cell->kf_mean,
50  // cell->kf_std, etc..
51  // but accessing cell->kf_mean would also modify (i.e. ARE the same memory
52  // slot) cell->dm_mean, for example.
53 
54  // Note 2: If the number of type of fields are changed in the future,
55  // *PLEASE* also update the writeToStream() and readFromStream() methods!!
56 
57  double param1_ = 0;
58  /** [KF-methods only] The mean value of this cell */
59  double& kf_mean() { return param1_; }
60  const double& kf_mean() const { return param1_; }
61  /** [Kernel-methods only] The cumulative weighted readings of this cell
62  */
63  double& dm_mean() { return param1_; }
64  const double& dm_mean() const { return param1_; }
65  /** [GMRF only] The mean value of this cell */
66  double& gmrf_mean() { return param1_; }
67  const double& gmrf_mean() const { return param1_; }
68 
69  double param2_ = 0;
70 
71  /** [KF-methods only] The standard deviation value of this cell */
72  double& kf_std() { return param2_; }
73  const double& kf_std() const { return param2_; }
74  /** [Kernel-methods only] The cumulative weights (concentration = alpha
75  * * dm_mean / dm_mean_w + (1-alpha)*r0 ) */
76  double& dm_mean_w() { return param2_; }
77  const double& dm_mean_w() const { return param2_; }
78  double& gmrf_std() { return param2_; }
79  const double& gmrf_std() const { return param2_; }
80 
81  /** [Kernel DM-V only] The cumulative weighted variance of this cell */
82  double dmv_var_mean{0};
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 }; // namespace mrpt::maps
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::containers::CDynamicGrid, The application icp-slam,
149  *mrpt::maps::CMultiMetricMap
150  * \ingroup mrpt_maps_grp
151  */
153  : public mrpt::maps::CMetricMap,
154  public mrpt::containers::CDynamicGrid<TRandomFieldCell>,
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 mrpt::d2f(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  ~CRandomFieldGridMap2D() override;
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  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::img::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 */
249  const mrpt::config::CConfigFileBase& source,
250  const std::string& section);
251 
252  /** See utils::CLoadableOptions */
253  void internal_dumpToTextStream_common(std::ostream& out) const;
254 
255  /** @name Kernel methods (mrKernelDM, mrKernelDMV)
256  @{ */
257  /** The sigma of the "Parzen"-kernel Gaussian */
258  float sigma{0.15f};
259  /** The cutoff radius for updating cells. */
261  /** Limits for normalization of sensor readings. */
262  float R_min{0}, R_max{3};
263  /** [DM/DM+V methods] The scaling parameter for the confidence "alpha"
264  * values (see the IROS 2009 paper; see CRandomFieldGridMap2D) */
265  double dm_sigma_omega{0.05};
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{0.35f};
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). */
276  float KF_initialCellStd{1.0};
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. */
282  uint16_t KF_W_size{4};
283  /** @} */
284 
285  /** @name Gaussian Markov Random Fields methods (mrGMRF_SD)
286  @{ */
287  /** The information (Lambda) of fixed map constraints */
288  double GMRF_lambdaPrior{0.01f};
289  /** The initial information (Lambda) of each observation (this
290  * information will decrease with time) */
291  double GMRF_lambdaObs{10.0f};
292  /** The loss of information of the observations with each iteration */
293  double GMRF_lambdaObsLoss{0.0f};
294 
295  /** whether to use information of an occupancy_gridmap map for building
296  * the GMRF */
298  /** simplemap_file name of the occupancy_gridmap */
299  std::string GMRF_simplemap_file;
300  /** image name of the occupancy_gridmap */
302  /** occupancy_gridmap resolution: size of each pixel (m) */
303  double GMRF_gridmap_image_res{0.01f};
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 */
313  bool GMRF_skip_variance{false};
314  /** @} */
315  };
316 
317  /** Changes the size of the grid, maintaining previous contents. \sa setSize
318  */
319  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  {
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  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
392 
393  /** Returns two 3D objects representing the mean and variance maps */
394  virtual void getAs3DObject(
396  mrpt::opengl::CSetOfObjects::Ptr& varObj) const;
397 
398  /** Return the type of the random-field grid map, according to parameters
399  * passed on construction. */
401 
402  /** Direct update of the map with a reading in a given position of the map,
403  * using
404  * the appropriate method according to mapType passed in the constructor.
405  *
406  * This is a direct way to update the map, an alternative to the generic
407  * insertObservation() method which works with mrpt::obs::CObservation
408  * objects.
409  */
411  /** [in] The value observed in the (x,y) position */
412  const double sensorReading,
413  /** [in] The (x,y) location */
414  const mrpt::math::TPoint2D& point,
415  /** [in] Run a global map update after inserting this observatin
416  (algorithm-dependant) */
417  const bool update_map = true,
418  /** [in] Whether the observation "vanishes" with time (false) or not
419  (true) [Only for GMRF methods] */
420  const bool time_invariant = true,
421  /** [in] The uncertainty (standard deviation) of the reading.
422  Default="0.0" means use the default settings per map-wide parameters.
423  */
424  const double reading_stddev = .0);
425 
427  {
430  };
431 
432  /** Returns the prediction of the measurement at some (x,y) coordinates, and
433  * its certainty (in the form of the expected variance). */
434  virtual void predictMeasurement(
435  /** [in] Query X coordinate */
436  const double x,
437  /** [in] Query Y coordinate */
438  const double y,
439  /** [out] The output value */
440  double& out_predict_response,
441  /** [out] The output variance */
442  double& out_predict_response_variance,
443  /** [in] Whether to renormalize the prediction to a predefined
444  interval (`R` values in insertionOptions) */
445  bool do_sensor_normalization,
446  /** [in] Interpolation method */
447  const TGridInterpolationMethod interp_method = gimNearest);
448 
449  /** Return the mean and covariance vector of the full Kalman filter estimate
450  * (works for all KF-based methods). */
451  void getMeanAndCov(
452  mrpt::math::CVectorDouble& out_means,
453  mrpt::math::CMatrixDouble& out_cov) const;
454 
455  /** Return the mean and STD vectors of the full Kalman filter estimate
456  * (works for all KF-based methods). */
457  void getMeanAndSTD(
458  mrpt::math::CVectorDouble& out_means,
459  mrpt::math::CVectorDouble& out_STD) const;
460 
461  /** Load the mean and STD vectors of the full Kalman filter estimate (works
462  * for all KF-based methods). */
463  void setMeanAndSTD(
464  mrpt::math::CVectorDouble& out_means,
465  mrpt::math::CVectorDouble& out_STD);
466 
467  /** Run the method-specific procedure required to ensure that the mean &
468  * variances are up-to-date with all inserted observations. */
469  void updateMapEstimation();
470 
471  void enableVerbose(bool enable_verbose)
472  {
474  }
475  bool isEnabledVerbose() const
476  {
478  }
479 
480  void enableProfiler(bool enable = true)
481  {
482  this->m_gmrf.enableProfiler(enable);
483  }
484  bool isProfilerEnabled() const { return this->m_gmrf.isProfilerEnabled(); }
485 
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. */
514  mutable bool m_hasToRecoverMeanAndCov{true};
515 
516  /** @name Auxiliary vars for DM & DM+V methods
517  @{ */
518  float m_DM_lastCutOff{0};
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  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 } // namespace mrpt::maps
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
Simple, scalar (1-dim) constraint (edge) for a GMRF.
std::string GMRF_gridmap_image_file
image name of the occupancy_gridmap
const double & dm_mean() const
const double & gmrf_mean() const
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)...
double & dm_mean()
[Kernel-methods only] The cumulative weighted readings of this cell
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
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 "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
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
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.
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 setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
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
mrpt::vision::TStereoCalibParams params
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_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.
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 ...
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
void internal_dumpToTextStream_common(std::ostream &out) const
See utils::CLoadableOptions.
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.
const double & kf_mean() const
double getInformation() const override
Return the inverse of the variance of this constraint.
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
~CRandomFieldGridMap2D() override
Destructor.
VerbosityLevel getMinLoggingLevel() const
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
bool time_invariant
whether the observation will lose weight (lambda) as time goes on (default false) ...
float d2f(const double d)
shortcut for static_cast<float>(double)
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.
This class allows loading and storing values and vectors of different types from a configuration text...
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
double & kf_mean()
[KF-methods only] The mean value of this cell
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 ...
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)...
Versatile class for consistent logging and management of output messages.
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
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.
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...
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
virtual void getAsBitmapFile(mrpt::img::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
ConnectivityDescriptor::Ptr m_gmrf_connectivity
Empty: default.
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
void internal_loadFromConfigFile_common(const mrpt::config::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
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
const double & dm_mean_w() const
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...
double & gmrf_mean()
[GMRF only] The mean value of this cell
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:52
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:85
mrpt::vision::TStereoCalibResults out
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
The implementation in this class just calls all the corresponding method of the contained metric maps...
void internal_clear() override
Erase all the contents of the map.
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
const double & gmrf_std() const
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
MRPT_FILL_ENUM_MEMBER(mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation, mrKernelDM)
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)
Simple, scalar (1-dim) constraint (edge) for a GMRF.
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.
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
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)
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.
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 GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020