Main MRPT website > C++ reference for MRPT 1.5.7
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-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 #pragma once
11 
13 #include <mrpt/utils/types_math.h>
19 
20 #include <mrpt/maps/link_pragmas.h>
21 
22 // Fwdr decl:
23 class vtkStructuredGrid;
24 
25 namespace mrpt
26 {
27 namespace maps
28 {
30 
31  /** The contents of each voxel in a CRandomFieldGridMap3D map.
32  * \ingroup mrpt_maps_grp
33  **/
34 #if defined(MRPT_IS_X86_AMD64) // Pragma defined to ensure no structure packing: since we'll serialize TRandomFieldVoxel to streams, we want it not to depend on compiler options, etc.
35 #pragma pack(push,1)
36 #endif
38  {
39  double mean_value, stddev_value; //!< 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),
44  stddev_value(_stddev_value)
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 associated one real-valued property which is to be estimated by this class.
52  *
53  * This class implements a Gaussian Markov Random Field (GMRF) estimator, with each voxel being connected to its
54  * 6 immediate neighbors (Up, down, left, right, front, back).
55  * - See papers:
56  * - "Time-variant gas distribution mapping with obstacle information", Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots, 40(1), 1-16, 2016.
57  *
58  * Note that this class does not derive from mrpt::maps::CMetricMap since the estimated values do not have sensor-especific semantics,
59  * i.e. the grid can be used to estimate temperature, gas concentration, etc.
60  *
61  * Usage:
62  * - Define grid size with either constructor or via `setSize()`.
63  * - Initialize the map with `initialize()`. This resets the contents of the map, so previously-added observations will be lost.
64  * - Add observations of 3D voxels with `insertIndividualReading()`
65  *
66  * Custom connectivity patterns can be defined with setVoxelsConnectivity().
67  *
68  * \sa mrpt::maps::CRandomFieldGridMap3D
69  * \ingroup mrpt_maps_grp
70  * \note [New in MRPT 1.5.0]
71  */
73  public mrpt::utils::CDynamicGrid3D<TRandomFieldVoxel>,
75  public mrpt::utils::COutputLogger
76  {
78 
79  // This must be added to any CSerializable derived class:
81  public:
82  static bool ENABLE_GMRF_PROFILER; //!< [default:false] Enables a profiler to show a performance report at application end.
83 
84  /** Constructor.
85  * If you set call_initialize_now to false, the object will be initialized immediately (without the heavy initialization of the GMRF),
86  * but you then must call `setSize()` or `clear()` later to properly initialize the object before using it to insert observations.
87  */
89  double x_min = -2, double x_max = 2,
90  double y_min = -2, double y_max = 2,
91  double z_min = -2, double z_max = 2,
92  double voxel_size = 0.5,
93  bool call_initialize_now = true
94  );
95 
96  /** Erases all added observations and start again with an empty gridmap. */
97  void clear() MRPT_OVERRIDE;
98 
99  /** Save the current estimated mean values to a CSV file (compatible with Paraview) with fields `x y z mean_value`.
100  * Optionally, std deviations can be also saved to another file with fields `x y z stddev_value`, if `filName_stddev` is provided.
101  * \return false on error writing to file
102  * \sa saveAsVtkStructuredGrid
103  */
104  bool saveAsCSV(const std::string &filName_mean, const std::string &filName_stddev = std::string() ) const;
105 
106  /** Save the current estimated grid to a VTK file (.vts) as a "structured grid". \sa saveAsCSV */
107  bool saveAsVtkStructuredGrid(const std::string &fil) const;
108 
109  /** Parameters common to any derived class.
110  * Derived classes should derive a new struct from this one, plus "public utils::CLoadableOptions",
111  * and call the internal_* methods where appropiate to deal with the variables declared here.
112  * Derived classes instantions of their "TInsertionOptions" MUST set the pointer "m_insertOptions_common" upon construction.
113  */
114  struct MAPS_IMPEXP TInsertionOptions : public mrpt::utils::CLoadableOptions
115  {
116  TInsertionOptions(); //!< Default values loader
117 
118  /** See utils::CLoadableOptions */
119  void loadFromConfigFile(
121  const std::string &section);
122 
123  void dumpToTextStream(mrpt::utils::CStream &out) const; //!< See utils::CLoadableOptions
124 
125  /** @name Gaussian Markov Random Fields method
126  @{ */
127  double GMRF_lambdaPrior; //!< The information (Lambda) of fixed map constraints
128  bool GMRF_skip_variance; //!< (Default:false) Skip the computation of the variance, just compute the mean
129  /** @} */
130  };
131 
132  TInsertionOptions insertionOptions; //!< \sa updateMapEstimation()
133 
134  /** Changes the size of the grid, maintaining previous contents. \sa setSize */
135  virtual void resize(
136  double new_x_min, double new_x_max,
137  double new_y_min, double new_y_max,
138  double new_z_min, double new_z_max,
139  const TRandomFieldVoxel& defaultValueNewvoxels, double additionalMarginMeters = 2.0) MRPT_OVERRIDE;
140 
141  /** Changes the size of the grid, erasing previous contents.If `resolution_z`<0, the same resolution will be used for all dimensions x,y,z as given in `resolution_xy` \sa resize.*/
142  virtual void setSize(
143  const double x_min, const double x_max,
144  const double y_min, const double y_max,
145  const double z_min, const double z_max,
146  const double resolution_xy, const double resolution_z = -1.0,
147  const TRandomFieldVoxel* fill_value = NULL) MRPT_OVERRIDE;
148 
149  /** Base class for user-supplied objects capable of describing voxels connectivity, used to build prior factors of the MRF graph. \sa setvoxelsConnectivity() */
151  {
152  //Virtual destructor for polymorphic type.
154  /** Implement the check of whether node i=(icx,icy,icz) is connected with node j=(jcx,jcy,jcy).
155  * This visitor method will be called only for immediate neighbors.
156  * \return true if connected (and the "information" value should be also updated in out_edge_information), false otherwise.
157  */
158  virtual bool getEdgeInformation(
159  const CRandomFieldGridMap3D *parent, //!< The parent map on which we are running
160  size_t icx, size_t icy, size_t icz, //!< (cx,cy,cz) for node "i"
161  size_t jcx, size_t jcy, size_t jcz, //!< (cx,cy,cz) for node "j"
162  double &out_edge_information //!< Must output here the inverse of the variance of the constraint edge.
163  ) = 0;
164  };
165  typedef std::shared_ptr<ConnectivityDescriptor> ConnectivityDescriptorPtr;
166 
167  /** Sets a custom object to define the connectivity between voxels. Must call clear() or setSize() afterwards for the changes to take place. */
168  void setVoxelsConnectivity(const ConnectivityDescriptorPtr &new_connectivity_descriptor);
169 
173  };
174 
175  /** Direct update of the map with a reading in a given position of the map.
176  * \return false if point is out of the grid extension.
177  */
179  const double sensorReading, //!< [in] The value observed in the (x,y,z) position
180  const double sensorVariance, //!< [in] The variance of the sensor observation
181  const mrpt::math::TPoint3D & point, //!< [in] The (x,y,z) location
182  const TVoxelInterpolationMethod method, //!< [in] Voxel interpolation method: how many voxels will be affected by the reading
183  const bool update_map //!< [in] Run a global map update after inserting this observation (algorithm-dependant)
184  );
185 
186  void updateMapEstimation(); //!< Run the method-specific procedure required to ensure that the mean & variances are up-to-date with all inserted observations, using parameters in insertionOptions
187 
188  /** Returns the 3D grid contents as an VTK grid. */
189  void getAsVtkStructuredGrid(vtkStructuredGrid* output, const std::string &label_mean = std::string("mean"), const std::string &label_stddev = std::string("stddev") ) const;
190 
191  protected:
192  /** Internal: called called after each change of resolution, size, etc. to build the prior factor information */
193  void internal_initialize(bool erase_prev_contents = true);
194 
196 
198 
200  {
201  double obsValue; //!< Observation value
202  double Lambda; //!< "Information" of the observation (=inverse of the variance)
203 
204  double evaluateResidual() const MRPT_OVERRIDE;
205  double getInformation() const MRPT_OVERRIDE;
206  void evalJacobian(double &dr_dx) const MRPT_OVERRIDE;
207 
209  private:
211  };
212 
214  {
215  double Lambda; //!< "Information" of the observation (=inverse of the variance)
216 
217  double evaluateResidual() const MRPT_OVERRIDE;
218  double getInformation() const MRPT_OVERRIDE;
219  void evalJacobian(double &dr_dx_i, double &dr_dx_j) const MRPT_OVERRIDE;
220 
222  private:
224  };
225 
226  std::vector<std::deque<TObservationGMRF> > m_mrf_factors_activeObs; //!< Vector with the active observations and their respective Information, for each map cell.
227  std::deque<TPriorFactorGMRF> m_mrf_factors_priors; //!< Vector with the precomputed priors for each GMRF model
228 
229 
230  };
232 
233  } // End of namespace
234 } // 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.
mrpt::graphs::ScalarFactorGraph m_gmrf
double evaluateResidual() const MRPT_OVERRIDE
Return the residual/error of this observation.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
The contents of each voxel in a CRandomFieldGridMap3D map.
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
void internal_initialize(bool erase_prev_contents=true)
Internal: called called after each change of resolution, size, etc.
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) MRPT_OVERRIDE
Changes the size of the grid, maintaining previous contents.
STL namespace.
void clear() MRPT_OVERRIDE
Erases all added observations and start again with an empty gridmap.
A 3D rectangular grid of dynamic size which stores any kind of data at each voxel.
This class allows loading and storing values and vectors of different types from a configuration text...
ConnectivityDescriptorPtr m_gmrf_connectivity
Empty: default.
std::shared_ptr< ConnectivityDescriptor > ConnectivityDescriptorPtr
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#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...
double Lambda
"Information" of the observation (=inverse of the variance)
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)
double getInformation() const MRPT_OVERRIDE
Return the inverse of the variance of this constraint.
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.
void evalJacobian(double &dr_dx) const MRPT_OVERRIDE
Returns the derivative of the residual wrt the node value.
double stddev_value
Mean and sigma (standard deviation) estimated values for the voxel.
GLsizei const GLchar ** string
Definition: glext.h:3919
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...
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=NULL) MRPT_OVERRIDE
Changes the size of the grid, erasing previous contents.If resolution_z<0, the same resolution will b...
utils::CDynamicGrid3D< TRandomFieldVoxel > BASE
void setVoxelsConnectivity(const ConnectivityDescriptorPtr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between voxels.
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:3908
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const MRPT_OVERRIDE
Returns the derivative of the residual wrt the node values.
double getInformation() const MRPT_OVERRIDE
Return the inverse of the variance of this constraint.
double evaluateResidual() const MRPT_OVERRIDE
Return the residual/error of this observation.
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 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.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
static bool ENABLE_GMRF_PROFILER
[default:false] Enables a profiler to show a performance report at application end.
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019