MRPT  1.9.9
CRandomFieldGridMap3D.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-2019, 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/math/TPoint3D.h>
18 
19 namespace mrpt::maps
20 {
21 /** The contents of each voxel in a CRandomFieldGridMap3D map.
22  * \ingroup mrpt_maps_grp
23  **/
24 #if defined(MRPT_IS_X86_AMD64) // Pragma defined to ensure no structure
25 // packing: since we'll serialize
26 // TRandomFieldVoxel to streams, we want it not
27 // to depend on compiler options, etc.
28 #pragma pack(push, 1)
29 #endif
31 {
32  /** Mean and sigma (standard deviation) estimated values for the voxel. */
34 
35  /** Constructor */
36  TRandomFieldVoxel(double _mean_value = .0, double _stddev_value = .0)
37  : mean_value(_mean_value), stddev_value(_stddev_value)
38  {
39  }
40 };
41 #if defined(MRPT_IS_X86_AMD64)
42 #pragma pack(pop)
43 #endif
44 
45 /** CRandomFieldGridMap3D represents a 3D regular grid where each voxel is
46  * associated one real-valued property which is to be estimated by this class.
47  *
48  * This class implements a Gaussian Markov Random Field (GMRF) estimator, with
49  * each voxel being connected to its
50  * 6 immediate neighbors (Up, down, left, right, front, back).
51  * - See papers:
52  * - "Time-variant gas distribution mapping with obstacle information",
53  * Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots,
54  * 40(1), 1-16, 2016.
55  *
56  * Note that this class does not derive from mrpt::maps::CMetricMap since the
57  * estimated values do not have sensor-especific semantics,
58  * i.e. the grid can be used to estimate temperature, gas concentration, etc.
59  *
60  * Usage:
61  * - Define grid size with either constructor or via `setSize()`.
62  * - Initialize the map with `initialize()`. This resets the contents of the
63  * 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::containers::CDynamicGrid3D<TRandomFieldVoxel>,
76 {
78 
80  public:
81  /** [default:false] Enables a profiler to show a performance report at
82  * application end. */
83  static bool ENABLE_GMRF_PROFILER;
84 
85  /** Constructor.
86  * If you set call_initialize_now to false, the object will be initialized
87  * immediately (without the heavy initialization of the GMRF),
88  * but you then must call `setSize()` or `clear()` later to properly
89  * initialize the object before using it to insert observations.
90  */
92  double x_min = -2, double x_max = 2, double y_min = -2,
93  double y_max = 2, double z_min = -2, double z_max = 2,
94  double voxel_size = 0.5, bool call_initialize_now = true);
95 
96  /** Erases all added observations and start again with an empty gridmap. */
97  void clear() override;
98 
99  /** Save the current estimated mean values to a CSV file (compatible with
100  * Paraview) with fields `x y z mean_value`.
101  * Optionally, std deviations can be also saved to another file with fields
102  * `x y z stddev_value`, if `filName_stddev` is provided.
103  * \return false on error writing to file
104  */
105  bool saveAsCSV(
106  const std::string& filName_mean,
107  const std::string& filName_stddev = std::string()) const;
108 
109  /** Parameters common to any derived class.
110  * Derived classes should derive a new struct from this one, plus "public
111  * utils::CLoadableOptions",
112  * and call the internal_* methods where appropiate to deal with the
113  * variables declared here.
114  * Derived classes instantions of their "TInsertionOptions" MUST set the
115  * pointer "m_insertOptions_common" upon construction.
116  */
118  {
119  /** Default values loader */
121 
122  /** See utils::CLoadableOptions */
123  void loadFromConfigFile(
124  const mrpt::config::CConfigFileBase& source,
125  const std::string& section) override;
126 
127  /** See utils::CLoadableOptions */
128  void dumpToTextStream(std::ostream& out) const override;
129 
130  /** @name Gaussian Markov Random Fields method
131  @{ */
132  /** The information (Lambda) of fixed map constraints */
133  double GMRF_lambdaPrior{0.01f};
134  /** (Default:false) Skip the computation of the variance, just compute
135  * the mean */
136  bool GMRF_skip_variance{false};
137  /** @} */
138  };
139 
140  /** \sa updateMapEstimation() */
142 
143  /** Changes the size of the grid, maintaining previous contents. \sa setSize
144  */
145  void resize(
146  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
147  double new_z_min, double new_z_max,
148  const TRandomFieldVoxel& defaultValueNewvoxels,
149  double additionalMarginMeters = 2.0) override;
150 
151  /** Changes the size of the grid, erasing previous contents.If
152  * `resolution_z`<0, the same resolution will be used for all dimensions
153  * x,y,z as given in `resolution_xy` \sa resize.*/
154  void setSize(
155  const double x_min, const double x_max, const double y_min,
156  const double y_max, const double z_min, const double z_max,
157  const double resolution_xy, const double resolution_z = -1.0,
158  const TRandomFieldVoxel* fill_value = nullptr) override;
159 
160  /** Base class for user-supplied objects capable of describing voxels
161  * connectivity, used to build prior factors of the MRF graph. \sa
162  * setvoxelsConnectivity() */
164  {
166  // Virtual destructor for polymorphic type.
167  virtual ~ConnectivityDescriptor() = default;
168  /** Implement the check of whether node i=(icx,icy,icz) is connected
169  * with node j=(jcx,jcy,jcy).
170  * This visitor method will be called only for immediate neighbors.
171  * \return true if connected (and the "information" value should be also
172  * updated in out_edge_information), false otherwise.
173  */
174  virtual bool getEdgeInformation(
175  /** The parent map on which we are running */
176  const CRandomFieldGridMap3D* parent,
177  /** (cx,cy,cz) for node "i" */
178  size_t icx, size_t icy, size_t icz,
179  /** (cx,cy,cz) for node "j" */
180  size_t jcx, size_t jcy, size_t jcz,
181  /** Must output here the inverse of the variance of the constraint
182  edge. */
183  double& out_edge_information) = 0;
184  };
185 
186  /** Sets a custom object to define the connectivity between voxels. Must
187  * call clear() or setSize() afterwards for the changes to take place. */
189  const ConnectivityDescriptor::Ptr& new_connectivity_descriptor);
190 
192  {
195  };
196 
197  /** Direct update of the map with a reading in a given position of the map.
198  * \return false if point is out of the grid extension.
199  */
201  /** [in] The value observed in the (x,y,z) position */
202  const double sensorReading,
203  /** [in] The variance of the sensor observation */
204  const double sensorVariance,
205  /** [in] The (x,y,z) location */
206  const mrpt::math::TPoint3D& point,
207  /** [in] Voxel interpolation method: how many voxels will be
208  affected by the reading */
209  const TVoxelInterpolationMethod method,
210  /** [in] Run a global map update after inserting this observation
211  (algorithm-dependant) */
212  const bool update_map);
213 
214  /** Run the method-specific procedure required to ensure that the mean &
215  * variances are up-to-date with all inserted observations, using parameters
216  * in insertionOptions */
217  void updateMapEstimation();
218 
219  protected:
220  /** Internal: called called after each change of resolution, size, etc. to
221  * build the prior factor information */
222  void internal_initialize(bool erase_prev_contents = true);
223 
224  /** Empty: default */
226 
228 
231  {
232  /** Observation value */
233  double obsValue;
234  /** "Information" of the observation (=inverse of the variance) */
235  double Lambda;
236 
237  double evaluateResidual() const override;
238  double getInformation() const override;
239  void evalJacobian(double& dr_dx) const override;
240 
242  : obsValue(.0), Lambda(.0), m_parent(&parent)
243  {
244  }
245 
246  private:
248  };
249 
252  {
253  /** "Information" of the observation (=inverse of the variance) */
254  double Lambda;
255 
256  double evaluateResidual() const override;
257  double getInformation() const override;
258  void evalJacobian(double& dr_dx_i, double& dr_dx_j) const override;
259 
261  : Lambda(.0), m_parent(&parent)
262  {
263  }
264 
265  private:
267  };
268 
269  /** Vector with the active observations and their respective Information,
270  * for each map cell. */
271  std::vector<std::deque<TObservationGMRF>> m_mrf_factors_activeObs;
272  /** Vector with the precomputed priors for each GMRF model */
273  std::deque<TPriorFactorGMRF> m_mrf_factors_priors;
274 };
275 
276 } // namespace mrpt::maps
void dumpToTextStream(std::ostream &out) const override
See utils::CLoadableOptions.
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.
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.
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.
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...
CRandomFieldGridMap3D represents a 3D regular grid where each voxel is associated one real-valued pro...
mrpt::vision::TStereoCalibResults out
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
See utils::CLoadableOptions.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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.
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.
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: 9690a7a25 Mon Feb 24 17:42:02 2020 +0100 at lun feb 24 18:00:10 CET 2020