Main MRPT website > C++ reference for MRPT 1.5.6
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-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 <mrpt/maps/link_pragmas.h>
23 #include <list>
24 
25 namespace mrpt
26 {
27 namespace maps
28 {
29  class COccupancyGridMap2D;
30 
31  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CRandomFieldGridMap2D , CMetricMap, MAPS_IMPEXP )
32 
33  // Pragma defined to ensure no structure packing: since we'll serialize TRandomFieldCell to streams, we want it not to depend on compiler options, etc.
34 #if defined(MRPT_IS_X86_AMD64)
35 #pragma pack(push,1)
36 #endif
37 
38  /** The contents of each cell in a CRandomFieldGridMap2D map.
39  * \ingroup mrpt_maps_grp
40  **/
42  {
43  /** Constructor */
44  TRandomFieldCell(double kfmean_dm_mean = 1e-20, double kfstd_dmmeanw = 0) :
45  kf_mean (kfmean_dm_mean),
46  kf_std (kfstd_dmmeanw),
47  dmv_var_mean (0),
48  last_updated(mrpt::system::now()),
49  updated_std (kfstd_dmmeanw)
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, cell->kf_std, etc..
55  // but accessing cell->kf_mean would also modify (i.e. ARE the same memory slot) cell->dm_mean, for example.
56 
57  // Note 2: If the number of type of fields are changed in the future,
58  // *PLEASE* also update the writeToStream() and readFromStream() methods!!
59 
60  union
61  {
62  double kf_mean; //!< [KF-methods only] The mean value of this cell
63  double dm_mean; //!< [Kernel-methods only] The cumulative weighted readings of this cell
64  double gmrf_mean; //!< [GMRF only] The mean value of this cell
65  };
66 
67  union
68  {
69  double kf_std; //!< [KF-methods only] The standard deviation value of this cell
70  double dm_mean_w; //!< [Kernel-methods only] The cumulative weights (concentration = alpha * dm_mean / dm_mean_w + (1-alpha)*r0 )
71  double gmrf_std;
72  };
73 
74  double dmv_var_mean; //!< [Kernel DM-V only] The cumulative weighted variance of this cell
75 
76  mrpt::system::TTimeStamp last_updated; //!< [Dynamic maps only] The timestamp of the last time the cell was updated
77  double updated_std; //!< [Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve
78  };
79 
80 #if defined(MRPT_IS_X86_AMD64)
81 #pragma pack(pop)
82 #endif
83 
84  /** CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property which is estimated by this map, either
85  * as a simple value or as a probility distribution (for each cell).
86  *
87  * There are a number of methods available to build the MRF grid-map, depending on the value of
88  * `TMapRepresentation maptype` passed in the constructor.
89  *
90  * The following papers describe the mapping alternatives implemented here:
91  * - `mrKernelDM`: A Gaussian kernel-based method. See:
92  * - "Building gas concentration gridmaps with a mobile robot", Lilienthal, A. and Duckett, T., Robotics and Autonomous Systems, v.48, 2004.
93  * - `mrKernelDMV`: A kernel-based method. See:
94  * - "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.
95  * - `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.
96  * - `mrKalmanApproximate`: A compressed/sparse Kalman filter approach. See:
97  * - "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.
98  * - `mrGMRF_SD`: A Gaussian Markov Random Field (GMRF) estimator, with these constraints:
99  * - `mrGMRF_SD`: Each cell only connected to its 4 immediate neighbors (Up, down, left, right).
100  * - (Removed in MRPT 1.5.0: `mrGMRF_G`: Each cell connected to a square area of neighbors cells)
101  * - See papers:
102  * - "Time-variant gas distribution mapping with obstacle information", Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots, 40(1), 1-16, 2016.
103  *
104  * Note that this class is virtual, since derived classes still have to implement:
105  * - mrpt::maps::CMetricMap::internal_computeObservationLikelihood()
106  * - mrpt::maps::CMetricMap::internal_insertObservation()
107  * - Serialization methods: writeToStream() and readFromStream()
108  *
109  * [GMRF only] A custom connectivity pattern between cells can be defined by calling setCellsConnectivity().
110  *
111  * \sa mrpt::maps::CGasConcentrationGridMap2D, mrpt::maps::CWirelessPowerGridMap2D, mrpt::maps::CMetricMap, mrpt::utils::CDynamicGrid, The application icp-slam, mrpt::maps::CMultiMetricMap
112  * \ingroup mrpt_maps_grp
113  */
115  public mrpt::maps::CMetricMap,
116  public mrpt::utils::CDynamicGrid<TRandomFieldCell>,
117  public mrpt::utils::COutputLogger
118  {
120 
121  // This must be added to any CSerializable derived class:
123  public:
124 
125  /** Calls the base CMetricMap::clear
126  * Declared here to avoid ambiguity between the two clear() in both base classes.
127  */
128  inline void clear() { CMetricMap::clear(); }
129 
130  // This method is just used for the ::saveToTextFile() method in base class.
132  {
133  return c.kf_mean;
134  }
135 
136  /** The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
137  */
139  {
140  mrKernelDM = 0, //!< Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D)
141  mrAchim = 0, //!< Another alias for "mrKernelDM", for backwards compatibility (see discussion in mrpt::maps::CRandomFieldGridMap2D)
142  mrKalmanFilter, //!< "Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D)
143  mrKalmanApproximate, //!< (see discussion in mrpt::maps::CRandomFieldGridMap2D)
144  mrKernelDMV, //!< Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D)
145  // Removed in MRPT 1.5.0: mrGMRF_G, //!< Gaussian Markov Random Field, Gaussian prior weights between neighboring cells up to a certain distance (see discussion in mrpt::maps::CRandomFieldGridMap2D)
146  mrGMRF_SD //!< Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see discussion in mrpt::maps::CRandomFieldGridMap2D)
147  };
148 
149  /** Constructor */
151  TMapRepresentation mapType = mrKernelDM,
152  double x_min = -2, double x_max = 2,
153  double y_min = -2, double y_max = 2,
154  double resolution = 0.1
155  );
156 
157  /** Destructor */
158  virtual ~CRandomFieldGridMap2D();
159 
160  /** Returns true if the map is empty/no observation has been inserted (in this class it always return false,
161  * unless redefined otherwise in base classes)
162  */
163  virtual bool isEmpty() const MRPT_OVERRIDE;
164 
165  /** Save the current map as a graphical file (BMP,PNG,...).
166  * The file format will be derived from the file extension (see CImage::saveToFile )
167  * It depends on the map representation model:
168  * mrAchim: Each pixel is the ratio \f$ \sum{\frac{wR}{w}} \f$
169  * mrKalmanFilter: Each pixel is the mean value of the Gaussian that represents each cell.
170  *
171  * \sa \a getAsBitmapFile()
172  */
173  virtual void saveAsBitmapFile(const std::string &filName) const;
174 
175  /** Returns an image just as described in \a saveAsBitmapFile */
176  virtual void getAsBitmapFile(mrpt::utils::CImage &out_img) const;
177 
178  /** Like saveAsBitmapFile(), but returns the data in matrix form (first row in the matrix is the upper (y_max) part of the map) */
179  virtual void getAsMatrix( mrpt::math::CMatrixDouble &out_mat) const;
180 
181  /** Parameters common to any derived class.
182  * Derived classes should derive a new struct from this one, plus "public utils::CLoadableOptions",
183  * and call the internal_* methods where appropiate to deal with the variables declared here.
184  * Derived classes instantions of their "TInsertionOptions" MUST set the pointer "m_insertOptions_common" upon construction.
185  */
187  {
188  TInsertionOptionsCommon(); //!< Default values loader
189 
190  /** See utils::CLoadableOptions */
191  void internal_loadFromConfigFile_common(
193  const std::string &section);
194 
195  void internal_dumpToTextStream_common(mrpt::utils::CStream &out) const; //!< See utils::CLoadableOptions
196 
197  /** @name Kernel methods (mrKernelDM, mrKernelDMV)
198  @{ */
199  float sigma; //!< The sigma of the "Parzen"-kernel Gaussian
200  float cutoffRadius; //!< The cutoff radius for updating cells.
201  float R_min,R_max; //!< Limits for normalization of sensor readings.
202  double dm_sigma_omega; //!< [DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; see CRandomFieldGridMap2D) */
203  /** @} */
204 
205  /** @name Kalman-filter methods (mrKalmanFilter, mrKalmanApproximate)
206  @{ */
207  float KF_covSigma; //!< The "sigma" for the initial covariance value between cells (in meters).
208  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).
209  float KF_observationModelNoise; //!< The sensor model noise (in normalized concentration units).
210  float KF_defaultCellMeanValue; //!< The default value for the mean of cells' concentration.
211  uint16_t KF_W_size; //!< [mrKalmanApproximate] The size of the window of neighbor cells.
212  /** @} */
213 
214  /** @name Gaussian Markov Random Fields methods (mrGMRF_SD)
215  @{ */
216  double GMRF_lambdaPrior; //!< The information (Lambda) of fixed map constraints
217  double GMRF_lambdaObs; //!< The initial information (Lambda) of each observation (this information will decrease with time)
218  double GMRF_lambdaObsLoss; //!< The loss of information of the observations with each iteration
219 
220  bool GMRF_use_occupancy_information; //!< whether to use information of an occupancy_gridmap map for building the GMRF
221  std::string GMRF_simplemap_file; //!< simplemap_file name of the occupancy_gridmap
222  std::string GMRF_gridmap_image_file; //!< image name of the occupancy_gridmap
223  double GMRF_gridmap_image_res; //!< occupancy_gridmap resolution: size of each pixel (m)
224  size_t GMRF_gridmap_image_cx; //!< Pixel coordinates of the origin for the occupancy_gridmap
225  size_t GMRF_gridmap_image_cy; //!< Pixel coordinates of the origin for the occupancy_gridmap
226 
227  double GMRF_saturate_min, GMRF_saturate_max; //!< (Default:-inf,+inf) Saturate the estimated mean in these limits
228  bool GMRF_skip_variance; //!< (Default:false) Skip the computation of the variance, just compute the mean
229  /** @} */
230  };
231 
232  /** Changes the size of the grid, maintaining previous contents. \sa setSize */
233  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;
234 
235  /** Changes the size of the grid, erasing previous contents.
236  * \param[in] connectivity_descriptor Optional user-supplied object that will visit all grid cells to define their connectivity with neighbors and the strength of existing edges. If present, it overrides all options in insertionOptions
237  * \sa resize
238  */
239  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);
240 
241  /** Base class for user-supplied objects capable of describing cells connectivity, used to build prior factors of the MRF graph. \sa setCellsConnectivity() */
243  {
244  ConnectivityDescriptor();
245  virtual ~ConnectivityDescriptor();
246 
247  /** Implement the check of whether node i=(icx,icy) is connected with node j=(jcx,jcy).
248  * This visitor method will be called only for immediate neighbors.
249  * \return true if connected (and the "information" value should be also updated in out_edge_information), false otherwise.
250  */
251  virtual bool getEdgeInformation(
252  const CRandomFieldGridMap2D *parent, //!< The parent map on which we are running
253  size_t icx, size_t icy, //!< (cx,cy) for node "i"
254  size_t jcx, size_t jcy, //!< (cx,cy) for node "j"
255  double &out_edge_information //!< Must output here the inverse of the variance of the constraint edge.
256  ) = 0;
257  };
258  typedef stlplus::smart_ptr<ConnectivityDescriptor> ConnectivityDescriptorPtr;
259 
260  /** Sets a custom object to define the connectivity between cells. Must call clear() or setSize() afterwards for the changes to take place. */
261  void setCellsConnectivity(const ConnectivityDescriptorPtr &new_connectivity_descriptor);
262 
263  /** See docs in base class: in this class this always returns 0 */
264  float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
265 
266  /** The implementation in this class just calls all the corresponding method of the contained metric maps */
267  virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
268 
269  /** Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the concentration of each cell.
270  * This method can only be called in a KF map model.
271  * \sa getAsMatlab3DGraphScript */
272  virtual void saveAsMatlab3DGraph(const std::string &filName) const;
273 
274  /** Return a large text block with a MATLAB script to plot the contents of this map \sa saveAsMatlab3DGraph
275  * This method can only be called in a KF map model */
276  void getAsMatlab3DGraphScript(std::string &out_script) const;
277 
278  /** Returns a 3D object representing the map (mean) */
279  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const MRPT_OVERRIDE;
280 
281  /** Returns two 3D objects representing the mean and variance maps */
282  virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &meanObj, mrpt::opengl::CSetOfObjectsPtr &varObj ) const;
283 
284  TMapRepresentation getMapType(); //!< Return the type of the random-field grid map, according to parameters passed on construction.
285 
286  /** Direct update of the map with a reading in a given position of the map, using
287  * the appropriate method according to mapType passed in the constructor.
288  *
289  * This is a direct way to update the map, an alternative to the generic insertObservation() method which works with mrpt::obs::CObservation objects.
290  */
292  const double sensorReading, //!< [in] The value observed in the (x,y) position
293  const mrpt::math::TPoint2D & point, //!< [in] The (x,y) location
294  const bool update_map = true, //!< [in] Run a global map update after inserting this observatin (algorithm-dependant)
295  const bool time_invariant = true, //!< [in] Whether the observation "vanishes" with time (false) or not (true) [Only for GMRF methods]
296  const double reading_stddev = .0 //!< [in] The uncertainty (standard deviation) of the reading. Default="0.0" means use the default settings per map-wide parameters.
297  );
298 
302  };
303 
304  /** Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form of the expected variance). */
305  virtual void predictMeasurement(
306  const double x, //!< [in] Query X coordinate
307  const double y, //!< [in] Query Y coordinate
308  double &out_predict_response, //!< [out] The output value
309  double &out_predict_response_variance, //!< [out] The output variance
310  bool do_sensor_normalization, //!< [in] Whether to renormalize the prediction to a predefined interval (`R` values in insertionOptions)
311  const TGridInterpolationMethod interp_method = gimNearest //!< [in] Interpolation method
312  );
313 
314  /** Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based methods). */
315  void getMeanAndCov( mrpt::math::CVectorDouble &out_means, mrpt::math::CMatrixDouble &out_cov) const;
316 
317  /** Return the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods). */
318  void getMeanAndSTD( mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD) const;
319 
320  /** Load the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods). */
322 
323  void updateMapEstimation(); //!< Run the method-specific procedure required to ensure that the mean & variances are up-to-date with all inserted observations.
324 
325  void enableVerbose(bool enable_verbose) { this->setMinLoggingLevel(mrpt::utils::LVL_DEBUG); }
326  bool isEnabledVerbose() const { return this->getMinLoggingLevel()== mrpt::utils::LVL_DEBUG; }
327 
328  void enableProfiler(bool enable = true) { this->m_gmrf.enableProfiler(enable); }
329  bool isProfilerEnabled() const { return this->m_gmrf.isProfilerEnabled() ; }
330 
331  protected:
333 
334  /** Common options to all random-field grid maps: pointer that is set to the derived-class instance of "insertOptions" upon construction of this class. */
336 
337  /** Get the part of the options common to all CRandomFieldGridMap2D classes */
339 
340  TMapRepresentation m_mapType; //!< The map representation type of this map, as passed in the constructor
341 
342  mrpt::math::CMatrixD m_cov; //!< The whole covariance matrix, used for the Kalman Filter map representation.
343 
344  /** The compressed band diagonal matrix for the KF2 implementation.
345  * The format is a Nx(W^2+2W+1) matrix, one row per cell in the grid map with the
346  * cross-covariances between each cell and half of the window around it in the grid.
347  */
349  mutable bool m_hasToRecoverMeanAndCov; //!< Only for the KF2 implementation.
350 
351  /** @name Auxiliary vars for DM & DM+V methods
352  @{ */
354  std::vector<float> m_DM_gaussWindow;
357  /** @} */
358 
360 
362 
364  {
365  double obsValue; //!< Observation value
366  double Lambda; //!< "Information" of the observation (=inverse of the variance)
367  bool time_invariant; //!< whether the observation will lose weight (lambda) as time goes on (default false)
368 
369  double evaluateResidual() const MRPT_OVERRIDE;
370  double getInformation() const MRPT_OVERRIDE;
371  void evalJacobian(double &dr_dx) const MRPT_OVERRIDE;
372 
374  private:
376  };
377 
379  {
380  double Lambda; //!< "Information" of the observation (=inverse of the variance)
381 
382  double evaluateResidual() const MRPT_OVERRIDE;
383  double getInformation() const MRPT_OVERRIDE;
384  void evalJacobian(double &dr_dx_i, double &dr_dx_j) const MRPT_OVERRIDE;
385 
387  private:
389  };
390 
391  // Important: converted to a std::list<> so pointers are NOT invalidated upon deletion.
392  std::vector<std::list<TObservationGMRF> > m_mrf_factors_activeObs; //!< Vector with the active observations and their respective Information
393  std::deque<TPriorFactorGMRF> m_mrf_factors_priors; //!< Vector with the precomputed priors for each GMRF model
394 
395  /** The implementation of "insertObservation" for Achim Lilienthal's map models DM & DM+V.
396  * \param normReading Is a [0,1] normalized concentration reading.
397  * \param point Is the sensor location on the map
398  * \param is_DMV = false -> map type is Kernel DM; true -> map type is DM+V
399  */
401  double normReading,
402  const mrpt::math::TPoint2D &point,
403  bool is_DMV );
404 
405  /** The implementation of "insertObservation" for the (whole) Kalman Filter map model.
406  * \param normReading Is a [0,1] normalized concentration reading.
407  * \param point Is the sensor location on the map
408  */
410  double normReading,
411  const mrpt::math::TPoint2D &point );
412 
413  /** The implementation of "insertObservation" for the Efficient Kalman Filter map model.
414  * \param normReading Is a [0,1] normalized concentration reading.
415  * \param point Is the sensor location on the map
416  */
418  double normReading,
419  const mrpt::math::TPoint2D &point );
420 
421  /** The implementation of "insertObservation" for the Gaussian Markov Random Field map model.
422  * \param normReading Is a [0,1] normalized concentration reading.
423  * \param point Is the sensor location on the map
424  */
425  void insertObservation_GMRF(double normReading,const mrpt::math::TPoint2D &point, const bool update_map,const bool time_invariant, const double reading_information);
426 
427  /** solves the minimum quadratic system to determine the new concentration of each cell */
429 
430  /** Computes the confidence of the cell concentration (alpha) */
431  double computeConfidenceCellValue_DM_DMV (const TRandomFieldCell *cell ) const;
432 
433  /** Computes the average cell concentration, or the overall average value if it has never been observed */
434  double computeMeanCellValue_DM_DMV (const TRandomFieldCell *cell ) const;
435 
436  /** Computes the estimated variance of the cell concentration, or the overall average variance if it has never been observed */
437  double computeVarCellValue_DM_DMV (const TRandomFieldCell *cell ) const;
438 
439  /** In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std values.
440  * \sa m_hasToRecoverMeanAndCov
441  */
442  void recoverMeanAndCov() const;
443 
444  /** Erase all the contents of the map */
445  virtual void internal_clear() MRPT_OVERRIDE;
446 
447  /** Check if two cells of the gridmap (m_map) are connected, based on the provided occupancy gridmap*/
449  const mrpt::maps::COccupancyGridMap2D *m_Ocgridmap,
450  size_t cxo_min,
451  size_t cxo_max,
452  size_t cyo_min,
453  size_t cyo_max,
454  const size_t seed_cxo,
455  const size_t seed_cyo,
456  const size_t objective_cxo,
457  const size_t objective_cyo);
458  };
460 
461 
462  } // End of namespace
463 
464 
465  // Specializations MUST occur at the same namespace:
466  namespace utils
467  {
468  template <>
470  {
473  {
474  m_map.insert(maps::CRandomFieldGridMap2D::mrKernelDM, "mrKernelDM");
475  m_map.insert(maps::CRandomFieldGridMap2D::mrKalmanFilter, "mrKalmanFilter");
476  m_map.insert(maps::CRandomFieldGridMap2D::mrKalmanApproximate, "mrKalmanApproximate");
477  m_map.insert(maps::CRandomFieldGridMap2D::mrKernelDMV, "mrKernelDMV");
479  }
480  };
481  } // End of namespace
482 } // End of namespace
483 
484 #endif
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
Simple, scalar (1-dim) constraint (edge) for a GMRF.
std::string GMRF_gridmap_image_file
image name of the occupancy_gridmap
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
Parameters for CMetricMap::compute3DMatchingRatio()
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...
float sigma
The sigma of the "Parzen"-kernel Gaussian.
ConnectivityDescriptorPtr m_gmrf_connectivity
Empty: default.
void clear()
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both bas...
double computeMeanCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the average cell concentration, or the overall average value if it has never been observed...
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
unsigned __int16 uint16_t
Definition: rptypes.h:46
double getInformation() const MRPT_OVERRIDE
Return the inverse of the variance of this constraint.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
float KF_defaultCellMeanValue
The default value for the mean of cells' concentration.
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...
const GLfloat * c
Definition: glew.h:10088
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.
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...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
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.
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:23
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
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.
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)
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.
void getAsMatlab3DGraphScript(std::string &out_script) const
Return a large text block with a MATLAB script to plot the contents of this map.
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha * dm_mean / dm_mean_w + (1-alpha)...
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...
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const MRPT_OVERRIDE
Returns the derivative of the residual wrt the node values.
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:38
TRandomFieldCell(double kfmean_dm_mean=1e-20, double kfstd_dmmeanw=0)
Constructor.
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
#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...
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
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 evaluateResidual() const MRPT_OVERRIDE
Return the residual/error of this observation.
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)...
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:43
double getInformation() const MRPT_OVERRIDE
Return the inverse of the variance of this constraint.
"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:28
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.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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...
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map (mean)
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.
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 evaluateResidual() const MRPT_OVERRIDE
Return the residual/error of this observation.
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
double updated_std
[Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve ...
A class for storing an occupancy grid map.
virtual void getAsBitmapFile(mrpt::utils::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
void setCellsConnectivity(const ConnectivityDescriptorPtr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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.
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:72
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
stlplus::smart_ptr< ConnectivityDescriptor > ConnectivityDescriptorPtr
void evalJacobian(double &dr_dx) const MRPT_OVERRIDE
Returns the derivative of the residual wrt the node value.
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.
GLfloat * params
Definition: glew.h:1436
float cell2float(const TRandomFieldCell &c) const MRPT_OVERRIDE
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...
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 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:69
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.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Lightweight 2D point.
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 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)
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std ...
float KF_initialCellStd
The initial standard deviation of each cell's concentration (will be stored both at each cell's struc...
size_t GMRF_gridmap_image_cy
Pixel coordinates of the origin for the occupancy_gridmap.
utils::CDynamicGrid< TRandomFieldCell > BASE
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
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...
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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.
virtual void internal_clear() MRPT_OVERRIDE
Erase all the contents of the 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 ...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018