Main MRPT website > C++ reference for MRPT 1.5.7
CHeightGridMap2D_MRF.cpp
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 #include "maps-precomp.h" // Precomp header
12 #include <mrpt/poses/CPose2D.h>
13 #include <mrpt/poses/CPose3D.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::maps;
17 using namespace mrpt::obs;
18 using namespace mrpt::utils;
19 using namespace mrpt::poses;
20 using namespace std;
21 using namespace mrpt::math;
22 
23 // =========== Begin of Map definition ============
25 
27  run_map_estimation_at_ctor(true),
28  min_x(-2),
29  max_x(2),
30  min_y(-2),
31  max_y(2),
32  resolution(0.10f),
33  mapType(mrGMRF_SD)
34 {
35 }
36 
38 {
39  // [<sectionNamePrefix>+"_creationOpts"]
40  const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
41  MRPT_LOAD_CONFIG_VAR(run_map_estimation_at_ctor, bool, source,sSectCreation);
42  MRPT_LOAD_CONFIG_VAR(min_x, double, source,sSectCreation);
43  MRPT_LOAD_CONFIG_VAR(max_x, double, source,sSectCreation);
44  MRPT_LOAD_CONFIG_VAR(min_y, double, source,sSectCreation);
45  MRPT_LOAD_CONFIG_VAR(max_y, double, source,sSectCreation);
46  MRPT_LOAD_CONFIG_VAR(resolution, double, source,sSectCreation);
47  mapType = source.read_enum<CHeightGridMap2D_MRF::TMapRepresentation>(sSectCreation,"mapType",mapType);
48 
49  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
50 }
51 
53 {
55  LOADABLEOPTS_DUMP_VAR(run_map_estimation_at_ctor , bool);
56  LOADABLEOPTS_DUMP_VAR(min_x , double);
57  LOADABLEOPTS_DUMP_VAR(max_x , double);
58  LOADABLEOPTS_DUMP_VAR(min_y , double);
59  LOADABLEOPTS_DUMP_VAR(max_y , double);
60  LOADABLEOPTS_DUMP_VAR(resolution , double);
61 
62  this->insertionOpts.dumpToTextStream(out);
63 }
64 
66 {
67  const CHeightGridMap2D_MRF::TMapDefinition &def = *dynamic_cast<const CHeightGridMap2D_MRF::TMapDefinition*>(&_def);
69  obj->insertionOptions = def.insertionOpts;
70  return obj;
71 }
72 // =========== End of Map definition Block =========
73 
75 
76 // Constructor
78  TMapRepresentation mapType,
79  double x_min, double x_max,
80  double y_min, double y_max, double resolution,
81  bool run_first_map_estimation_now
82  ) :
83  CRandomFieldGridMap2D(mapType, x_min,x_max,y_min,y_max,resolution ),
84  insertionOptions()
85 {
86  m_rfgm_run_update_upon_clear = run_first_map_estimation_now;
87  // Set the grid to initial values (and adjusts the KF covariance matrix!)
88  // Also, calling clear() is mandatory to end initialization of our base class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
90 }
91 
93 {
94  const TRandomFieldCell *cell = cellByPos(x,y);
95  if (!cell) return false;
96  this->insertIndividualReading(z, mrpt::math::TPoint2D(x,y), params.update_map_after_insertion, true /*time invariant*/, params.pt_z_std);
97  return true;
98 }
100  return m_resolution;
101 }
103  return m_size_x;
104 }
106  return m_size_y;
107 }
108 bool CHeightGridMap2D_MRF::dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const {
109  const TRandomFieldCell *cell = cellByIndex(cx,cy);
110  if (cell && cell->kf_mean) {
111  z_out = cell->kf_mean;
112  return true;
113  } else return false;
114 }
115 bool CHeightGridMap2D_MRF::dem_get_z(const double x, const double y, double &z_out) const {
116  const TRandomFieldCell *cell = cellByPos(x,y);
117  if (cell && cell->kf_mean) {
118  z_out = cell->kf_mean;
119  return true;
120  } else return false;
121 }
123  this->updateMapEstimation();
124 }
125 
127 {
128  // Just do the generic clear:
130  // Anything else special for this derived class?
131 }
132 
133 
135 {
136  return dem_internal_insertObservation(obs,robotPose);
137 }
138 
139 /*---------------------------------------------------------------
140  computeObservationLikelihood
141  ---------------------------------------------------------------*/
143  const CObservation *obs,
144  const CPose3D &takenFrom )
145 {
146  MRPT_UNUSED_PARAM(obs);MRPT_UNUSED_PARAM(takenFrom);
147  THROW_EXCEPTION("Not implemented yet!");
148 }
149 
150 /*---------------------------------------------------------------
151  Implements the writing to a CStream capability of CSerializable objects
152  ---------------------------------------------------------------*/
154 {
155  if (version)
156  *version = 0;
157  else
158  {
159  dyngridcommon_writeToStream(out);
160 
161  // To assure compatibility: The size of each cell:
162  uint32_t n = static_cast<uint32_t>(sizeof( TRandomFieldCell ));
163  out << n;
164 
165  // Save the map contents:
166  n = static_cast<uint32_t>(m_map.size());
167  out << n;
168 
169  // Save the "m_map": This requires special handling for big endian systems:
170 #if MRPT_IS_BIG_ENDIAN
171  for (uint32_t i=0;i<n;i++)
172  {
173  out << m_map[i].kf_mean << m_map[i].dm_mean << m_map[i].dmv_var_mean;
174  }
175 #else
176  // Little endian: just write all at once:
177  out.WriteBuffer( &m_map[0], sizeof(m_map[0])*m_map.size() );
178 #endif
179 
180 
181  // Save the insertion options:
182  out << uint8_t(m_mapType)
183  << m_cov
184  << m_stackedCov;
185 
186  out << insertionOptions.sigma
187  << insertionOptions.cutoffRadius
188  << insertionOptions.R_min
189  << insertionOptions.R_max
190  << insertionOptions.KF_covSigma
191  << insertionOptions.KF_initialCellStd
192  << insertionOptions.KF_observationModelNoise
193  << insertionOptions.KF_defaultCellMeanValue
194  << insertionOptions.KF_W_size;
195 
196  out << m_average_normreadings_mean << m_average_normreadings_var << uint64_t(m_average_normreadings_count);
197 
198  out << genericMapParams;
199  }
200 }
201 
202 /*---------------------------------------------------------------
203  Implements the reading from a CStream capability of CSerializable objects
204  ---------------------------------------------------------------*/
206 {
207  switch(version)
208  {
209  case 0:
210  {
211  dyngridcommon_readFromStream(in);
212 
213  // To assure compatibility: The size of each cell:
214  uint32_t n;
215  in >> n;
216 
217  ASSERT_EQUAL_( n , static_cast<uint32_t>( sizeof( TRandomFieldCell ) ));
218  // Load the map contents:
219  in >> n;
220  m_map.resize(n);
221 
222  // Read the note in writeToStream()
223 #if MRPT_IS_BIG_ENDIAN
224  for (uint32_t i=0;i<n;i++)
225  in >> m_map[i].kf_mean >> m_map[i].dm_mean >> m_map[i].dmv_var_mean;
226 #else
227  // Little endian: just read all at once:
228  in.ReadBuffer( &m_map[0], sizeof(m_map[0])*m_map.size() );
229 #endif
230 
231  {
232  uint8_t i;
233  in >> i;
234  m_mapType = TMapRepresentation(i);
235 
236  in >> m_cov
237  >> m_stackedCov;
238 
239  in >> insertionOptions.sigma
240  >> insertionOptions.cutoffRadius
241  >> insertionOptions.R_min
242  >> insertionOptions.R_max
243  >> insertionOptions.KF_covSigma
244  >> insertionOptions.KF_initialCellStd
245  >> insertionOptions.KF_observationModelNoise
246  >> insertionOptions.KF_defaultCellMeanValue
247  >> insertionOptions.KF_W_size;
248  }
249 
250  {
251  uint64_t N;
252  in >> m_average_normreadings_mean >> m_average_normreadings_var >> N;
253  m_average_normreadings_count = N;
254  }
255 
256  in >> genericMapParams;
257 
258  m_hasToRecoverMeanAndCov = true;
259  } break;
260  default:
262 
263  };
264 
265 }
266 
267 /*---------------------------------------------------------------
268  TInsertionOptions
269  ---------------------------------------------------------------*/
271 {
272 }
273 
274 /*---------------------------------------------------------------
275  dumpToTextStream
276  ---------------------------------------------------------------*/
278 {
279  out.printf("\n----------- [CHeightGridMap2D_MRF::TInsertionOptions] ------------ \n\n");
280  out.printf("[TInsertionOptions.Common] ------------ \n\n");
281  internal_dumpToTextStream_common(out); // Common params to all random fields maps:
282 
283 // out.printf("[TInsertionOptions.CHeightGridMap2D_MRF] ------------ \n\n");
284 // out.printf("std_windNoise_phi = %f\n", std_windNoise_phi);
285 
286  out.printf("\n");
287 }
288 
289 /*---------------------------------------------------------------
290  loadFromConfigFile
291  ---------------------------------------------------------------*/
293  const mrpt::utils::CConfigFileBase &iniFile,
294  const std::string &section)
295 {
296  // Common data fields for all random fields maps:
297  internal_loadFromConfigFile_common(iniFile,section);
298 
299  // Specific data fields for this class:
300  // ...
301 }
302 
303 /*---------------------------------------------------------------
304  getAs3DObject
305 ---------------------------------------------------------------*/
306 void CHeightGridMap2D_MRF::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj) const
307 {
308  MRPT_START
309  if (!genericMapParams.enableSaveAs3DObject) return;
311  MRPT_END
312 }
313 
314 /*---------------------------------------------------------------
315  getAs3DObject
316 ---------------------------------------------------------------*/
318  mrpt::opengl::CSetOfObjectsPtr &meanObj,
319  mrpt::opengl::CSetOfObjectsPtr &varObj ) const
320 {
321  MRPT_START
322  if (!genericMapParams.enableSaveAs3DObject) return;
323  CRandomFieldGridMap2D::getAs3DObject(meanObj,varObj);
324  MRPT_END
325 }
326 
#define ASSERT_EQUAL_(__A, __B)
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
bool run_map_estimation_at_ctor
Runs map estimation at start up (Default:true)
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
virtual bool insertIndividualPoint(const double x, const double y, const double z, const CHeightGridMap2D_Base::TPointInsertParams &params=CHeightGridMap2D_Base::TPointInsertParams()) MRPT_OVERRIDE
Update the DEM with one new point.
Extra params for insertIndividualPoint()
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
Load all map-specific params.
GLdouble GLdouble z
Definition: glext.h:3734
double resolution
See CHeightGridMap2D_MRF::CHeightGridMap2D_MRF.
virtual bool dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const MRPT_OVERRIDE
Get cell &#39;z&#39; by (cx,cy) cell indices.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::maps::CHeightGridMap2D_MRF::TMapRepresentation mapType
The kind of map representation (see CHeightGridMap2D_MRF::CHeightGridMap2D_MRF)
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
STL namespace.
void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.
CHeightGridMap2D_MRF represents digital-elevation-model over a 2D area, with uncertainty, based on a Markov-Random-Field (MRF) estimator.
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
double kf_mean
[KF-methods only] The mean value of this cell
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
The contents of each cell in a CRandomFieldGridMap2D map.
A helper class that can convert an enum value into its textual representation, and viceversa...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
This namespace contains representation of robot actions and observations.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map (mean)
int version
Definition: mrpt_jpeglib.h:898
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
virtual size_t dem_get_size_x() const MRPT_OVERRIDE
virtual void dem_update_map() MRPT_OVERRIDE
Ensure that all observations are reflected in the map estimate.
#define MRPT_START
unsigned __int64 uint64_t
Definition: rptypes.h:52
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
virtual size_t dem_get_size_y() const MRPT_OVERRIDE
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Internal method called by insertObservation()
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions insertionOpts
Observations insertion options.
GLuint in
Definition: glext.h:6301
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
GLenum GLint GLint y
Definition: glext.h:3516
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
GLenum GLint x
Definition: glext.h:3516
virtual double dem_get_resolution() const MRPT_OVERRIDE
virtual bool dem_get_z(const double x, const double y, double &z_out) const MRPT_OVERRIDE
Get cell &#39;z&#39; (x,y) by metric coordinates.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
unsigned __int32 uint32_t
Definition: rptypes.h:49
Lightweight 2D point.
GLenum const GLfloat * params
Definition: glext.h:3514
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
virtual void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.



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