Main MRPT website > C++ reference for MRPT 1.9.9
CRandomFieldGridMap3D.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-2018, 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 #pragma once
11 
13 #include <mrpt/math/types_math.h>
19 
20 // Fwdr decl:
21 class vtkStructuredGrid;
22 
23 namespace mrpt
24 {
25 namespace maps
26 {
27 /** The contents of each voxel in a CRandomFieldGridMap3D map.
28  * \ingroup mrpt_maps_grp
29  **/
30 #if defined(MRPT_IS_X86_AMD64) // Pragma defined to ensure no structure
31 // packing: since we'll serialize
32 // TRandomFieldVoxel to streams, we want it not
33 // to depend on compiler options, etc.
34 #pragma pack(push, 1)
35 #endif
37 {
38  /** Mean and sigma (standard deviation) estimated values for the voxel. */
40 
41  /** Constructor */
42  TRandomFieldVoxel(double _mean_value = .0, double _stddev_value = .0)
43  : mean_value(_mean_value), stddev_value(_stddev_value)
44  {
45  }
46 };
47 #if defined(MRPT_IS_X86_AMD64)
48 #pragma pack(pop)
49 #endif
50 
51 /** CRandomFieldGridMap3D represents a 3D regular grid where each voxel is
52  * associated one real-valued property which is to be estimated by this class.
53  *
54  * This class implements a Gaussian Markov Random Field (GMRF) estimator, with
55  * each voxel being connected to its
56  * 6 immediate neighbors (Up, down, left, right, front, back).
57  * - See papers:
58  * - "Time-variant gas distribution mapping with obstacle information",
59  * Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots,
60  * 40(1), 1-16, 2016.
61  *
62  * Note that this class does not derive from mrpt::maps::CMetricMap since the
63  * estimated values do not have sensor-especific semantics,
64  * i.e. the grid can be used to estimate temperature, gas concentration, etc.
65  *
66  * Usage:
67  * - Define grid size with either constructor or via `setSize()`.
68  * - Initialize the map with `initialize()`. This resets the contents of the
69  * map, so previously-added observations will be lost.
70  * - Add observations of 3D voxels with `insertIndividualReading()`
71  *
72  * Custom connectivity patterns can be defined with setVoxelsConnectivity().
73  *
74  * \sa mrpt::maps::CRandomFieldGridMap3D
75  * \ingroup mrpt_maps_grp
76  * \note [New in MRPT 1.5.0]
77  */
79  : public mrpt::containers::CDynamicGrid3D<TRandomFieldVoxel>,
82 {
84 
86  public:
87  /** [default:false] Enables a profiler to show a performance report at
88  * application end. */
89  static bool ENABLE_GMRF_PROFILER;
90 
91  /** Constructor.
92  * If you set call_initialize_now to false, the object will be initialized
93  * immediately (without the heavy initialization of the GMRF),
94  * but you then must call `setSize()` or `clear()` later to properly
95  * initialize the object before using it to insert observations.
96  */
98  double x_min = -2, double x_max = 2, double y_min = -2,
99  double y_max = 2, double z_min = -2, double z_max = 2,
100  double voxel_size = 0.5, bool call_initialize_now = true);
101 
102  /** Erases all added observations and start again with an empty gridmap. */
103  void clear() override;
104 
105  /** Save the current estimated mean values to a CSV file (compatible with
106  * Paraview) with fields `x y z mean_value`.
107  * Optionally, std deviations can be also saved to another file with fields
108  * `x y z stddev_value`, if `filName_stddev` is provided.
109  * \return false on error writing to file
110  * \sa saveAsVtkStructuredGrid
111  */
112  bool saveAsCSV(
113  const std::string& filName_mean,
114  const std::string& filName_stddev = std::string()) const;
115 
116  /** Save the current estimated grid to a VTK file (.vts) as a "structured
117  * grid". \sa saveAsCSV */
118  bool saveAsVtkStructuredGrid(const std::string& fil) const;
119 
120  /** Parameters common to any derived class.
121  * Derived classes should derive a new struct from this one, plus "public
122  * utils::CLoadableOptions",
123  * and call the internal_* methods where appropiate to deal with the
124  * variables declared here.
125  * Derived classes instantions of their "TInsertionOptions" MUST set the
126  * pointer "m_insertOptions_common" upon construction.
127  */
129  {
130  /** Default values loader */
132 
133  /** See utils::CLoadableOptions */
134  void loadFromConfigFile(
136  const std::string& section);
137 
138  /** See utils::CLoadableOptions */
139  void dumpToTextStream(std::ostream& out) const;
140 
141  /** @name Gaussian Markov Random Fields method
142  @{ */
143  /** The information (Lambda) of fixed map constraints */
145  /** (Default:false) Skip the computation of the variance, just compute
146  * the mean */
148  /** @} */
149  };
150 
151  /** \sa updateMapEstimation() */
153 
154  /** Changes the size of the grid, maintaining previous contents. \sa setSize
155  */
156  virtual void resize(
157  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
158  double new_z_min, double new_z_max,
159  const TRandomFieldVoxel& defaultValueNewvoxels,
160  double additionalMarginMeters = 2.0) override;
161 
162  /** Changes the size of the grid, erasing previous contents.If
163  * `resolution_z`<0, the same resolution will be used for all dimensions
164  * x,y,z as given in `resolution_xy` \sa resize.*/
165  virtual void setSize(
166  const double x_min, const double x_max, const double y_min,
167  const double y_max, const double z_min, const double z_max,
168  const double resolution_xy, const double resolution_z = -1.0,
169  const TRandomFieldVoxel* fill_value = nullptr) override;
170 
171  /** Base class for user-supplied objects capable of describing voxels
172  * connectivity, used to build prior factors of the MRF graph. \sa
173  * setvoxelsConnectivity() */
175  {
177  // Virtual destructor for polymorphic type.
179  /** Implement the check of whether node i=(icx,icy,icz) is connected
180  * with node j=(jcx,jcy,jcy).
181  * This visitor method will be called only for immediate neighbors.
182  * \return true if connected (and the "information" value should be also
183  * updated in out_edge_information), false otherwise.
184  */
185  virtual bool getEdgeInformation(
186  /** The parent map on which we are running */
187  const CRandomFieldGridMap3D* parent,
188  /** (cx,cy,cz) for node "i" */
189  size_t icx, size_t icy, size_t icz,
190  /** (cx,cy,cz) for node "j" */
191  size_t jcx, size_t jcy, size_t jcz,
192  /** Must output here the inverse of the variance of the constraint
193  edge. */
194  double& out_edge_information) = 0;
195  };
196 
197  /** Sets a custom object to define the connectivity between voxels. Must
198  * call clear() or setSize() afterwards for the changes to take place. */
200  const ConnectivityDescriptor::Ptr& new_connectivity_descriptor);
201 
203  {
206  };
207 
208  /** Direct update of the map with a reading in a given position of the map.
209  * \return false if point is out of the grid extension.
210  */
212  /** [in] The value observed in the (x,y,z) position */
213  const double sensorReading,
214  /** [in] The variance of the sensor observation */
215  const double sensorVariance,
216  /** [in] The (x,y,z) location */
217  const mrpt::math::TPoint3D& point,
218  /** [in] Voxel interpolation method: how many voxels will be
219  affected by the reading */
220  const TVoxelInterpolationMethod method,
221  /** [in] Run a global map update after inserting this observation
222  (algorithm-dependant) */
223  const bool update_map);
224 
225  /** Run the method-specific procedure required to ensure that the mean &
226  * variances are up-to-date with all inserted observations, using parameters
227  * in insertionOptions */
228  void updateMapEstimation();
229 
230  /** Returns the 3D grid contents as an VTK grid. */
232  vtkStructuredGrid* output,
233  const std::string& label_mean = std::string("mean"),
234  const std::string& label_stddev = std::string("stddev")) const;
235 
236  protected:
237  /** Internal: called called after each change of resolution, size, etc. to
238  * build the prior factor information */
239  void internal_initialize(bool erase_prev_contents = true);
240 
241  /** Empty: default */
243 
245 
248  {
249  /** Observation value */
250  double obsValue;
251  /** "Information" of the observation (=inverse of the variance) */
252  double Lambda;
253 
254  double evaluateResidual() const override;
255  double getInformation() const override;
256  void evalJacobian(double& dr_dx) const override;
257 
259  : obsValue(.0), Lambda(.0), m_parent(&parent)
260  {
261  }
262 
263  private:
265  };
266 
269  {
270  /** "Information" of the observation (=inverse of the variance) */
271  double Lambda;
272 
273  double evaluateResidual() const override;
274  double getInformation() const override;
275  void evalJacobian(double& dr_dx_i, double& dr_dx_j) const override;
276 
278  : Lambda(.0), m_parent(&parent)
279  {
280  }
281 
282  private:
284  };
285 
286  /** Vector with the active observations and their respective Information,
287  * for each map cell. */
288  std::vector<std::deque<TObservationGMRF>> m_mrf_factors_activeObs;
289  /** Vector with the precomputed priors for each GMRF model */
290  std::deque<TPriorFactorGMRF> m_mrf_factors_priors;
291 };
292 
293 } // End of namespace
294 } // End of namespace
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
Simple, scalar (1-dim) constraint (edge) for a GMRF.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::graphs::ScalarFactorGraph m_gmrf
The contents of each voxel in a CRandomFieldGridMap3D map.
virtual bool getEdgeInformation(const CRandomFieldGridMap3D *parent, size_t icx, size_t icy, size_t icz, size_t jcx, size_t jcy, size_t jcz, double &out_edge_information)=0
Implement the check of whether node i=(icx,icy,icz) is connected with node j=(jcx,jcy,jcy).
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
void evalJacobian(double &dr_dx) const override
Returns the derivative of the residual wrt the node value.
void internal_initialize(bool erase_prev_contents=true)
Internal: called called after each change of resolution, size, etc.
double getInformation() const override
Return the inverse of the variance of this constraint.
double evaluateResidual() const override
Return the residual/error of this observation.
void dumpToTextStream(std::ostream &out) const
See utils::CLoadableOptions.
double getInformation() const override
Return the inverse of the variance of this constraint.
void setVoxelsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between voxels.
double evaluateResidual() const override
Return the residual/error of this observation.
This class allows loading and storing values and vectors of different types from a configuration text...
double Lambda
"Information" of the observation (=inverse of the variance)
A 3D rectangular grid of dynamic size which stores any kind of data at each voxel.
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, double new_z_min, double new_z_max, const TRandomFieldVoxel &defaultValueNewvoxels, double additionalMarginMeters=2.0) override
Changes the size of the grid, maintaining previous contents.
Versatile class for consistent logging and management of output messages.
std::vector< std::deque< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information, for each map cell...
double Lambda
"Information" of the observation (=inverse of the variance)
void clear() override
Erases all added observations and start again with an empty gridmap.
CRandomFieldGridMap3D(double x_min=-2, double x_max=2, double y_min=-2, double y_max=2, double z_min=-2, double z_max=2, double voxel_size=0.5, bool call_initialize_now=true)
Constructor.
ConnectivityDescriptor::Ptr m_gmrf_connectivity
Empty: default.
GLsizei const GLchar ** string
Definition: glext.h:4101
double mean_value
Mean and sigma (standard deviation) estimated values for the voxel.
bool saveAsCSV(const std::string &filName_mean, const std::string &filName_stddev=std::string()) const
Save the current estimated mean values to a CSV file (compatible with Paraview) with fields x y z mea...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
CRandomFieldGridMap3D represents a 3D regular grid where each voxel is associated one real-valued pro...
void getAsVtkStructuredGrid(vtkStructuredGrid *output, const std::string &label_mean=std::string("mean"), const std::string &label_stddev=std::string("stddev")) const
Returns the 3D grid contents as an VTK grid.
bool saveAsVtkStructuredGrid(const std::string &fil) const
Save the current estimated grid to a VTK file (.vts) as a "structured grid".
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
bool insertIndividualReading(const double sensorReading, const double sensorVariance, const mrpt::math::TPoint3D &point, const TVoxelInterpolationMethod method, const bool update_map)
Direct update of the map with a reading in a given position of the map.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
Simple, scalar (1-dim) constraint (edge) for a GMRF.
Lightweight 3D point.
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
static bool ENABLE_GMRF_PROFILER
[default:false] Enables a profiler to show a performance report at application end.
virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double z_min, const double z_max, const double resolution_xy, const double resolution_z=-1.0, const TRandomFieldVoxel *fill_value=nullptr) override
Changes the size of the grid, erasing previous contents.If resolution_z<0, the same resolution will b...
std::deque< TPriorFactorGMRF > m_mrf_factors_priors
Vector with the precomputed priors for each GMRF model.
Base class for user-supplied objects capable of describing voxels connectivity, used to build prior f...
TRandomFieldVoxel(double _mean_value=.0, double _stddev_value=.0)
Constructor.
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019