MRPT  1.9.9
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-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 #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::poses;
19 using namespace std;
20 using namespace mrpt::math;
21 
22 // =========== Begin of Map definition ============
24  "CHeightGridMap2D_MRF,dem_mrf", mrpt::maps::CHeightGridMap2D_MRF)
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 
39  const std::string& sectionNamePrefix)
40 {
41  // [<sectionNamePrefix>+"_creationOpts"]
42  const std::string sSectCreation =
43  sectionNamePrefix + string("_creationOpts");
45  run_map_estimation_at_ctor, bool, source, sSectCreation);
46  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
47  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
48  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
49  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
50  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
52  sSectCreation, "mapType", mapType);
53 
54  insertionOpts.loadFromConfigFile(
55  source, sectionNamePrefix + string("_insertOpts"));
56 }
57 
59  std::ostream& out) const
60 {
61  out << mrpt::format(
62  "MAP TYPE = %s\n",
64  CHeightGridMap2D_MRF::TMapRepresentation>::value2name(mapType)
65  .c_str());
66  LOADABLEOPTS_DUMP_VAR(run_map_estimation_at_ctor, bool);
67  LOADABLEOPTS_DUMP_VAR(min_x, double);
68  LOADABLEOPTS_DUMP_VAR(max_x, double);
69  LOADABLEOPTS_DUMP_VAR(min_y, double);
70  LOADABLEOPTS_DUMP_VAR(max_y, double);
71  LOADABLEOPTS_DUMP_VAR(resolution, double);
72 
73  this->insertionOpts.dumpToTextStream(out);
74 }
75 
78 {
80  *dynamic_cast<const CHeightGridMap2D_MRF::TMapDefinition*>(&_def);
82  def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution,
84  obj->insertionOptions = def.insertionOpts;
85  return obj;
86 }
87 // =========== End of Map definition Block =========
88 
90 
91 // Constructor
93  TMapRepresentation mapType, double x_min, double x_max, double y_min,
94  double y_max, double resolution, bool run_first_map_estimation_now)
95  : CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution),
96  insertionOptions()
97 {
98  m_rfgm_run_update_upon_clear = run_first_map_estimation_now;
99  // Set the grid to initial values (and adjusts the KF covariance matrix!)
100  // Also, calling clear() is mandatory to end initialization of our base
101  // class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
103 }
104 
106  const double x, const double y, const double z,
108 {
109  const TRandomFieldCell* cell = cellByPos(x, y);
110  if (!cell) return false;
111  this->insertIndividualReading(
112  z, mrpt::math::TPoint2D(x, y), params.update_map_after_insertion,
113  true /*time invariant*/, params.pt_z_std);
114  return true;
115 }
116 double CHeightGridMap2D_MRF::dem_get_resolution() const { return m_resolution; }
117 size_t CHeightGridMap2D_MRF::dem_get_size_x() const { return m_size_x; }
118 size_t CHeightGridMap2D_MRF::dem_get_size_y() const { return m_size_y; }
120  const size_t cx, const size_t cy, double& z_out) const
121 {
122  const TRandomFieldCell* cell = cellByIndex(cx, cy);
123  if (cell && cell->kf_mean)
124  {
125  z_out = cell->kf_mean;
126  return true;
127  }
128  else
129  return false;
130 }
132  const double x, const double y, double& z_out) const
133 {
134  const TRandomFieldCell* cell = cellByPos(x, y);
135  if (cell && cell->kf_mean)
136  {
137  z_out = cell->kf_mean;
138  return true;
139  }
140  else
141  return false;
142 }
143 void CHeightGridMap2D_MRF::dem_update_map() { this->updateMapEstimation(); }
145 {
146  // Just do the generic clear:
148  // Anything else special for this derived class?
149 }
150 
152  const CObservation* obs, const CPose3D* robotPose)
153 {
154  return dem_internal_insertObservation(obs, robotPose);
155 }
156 
157 /*---------------------------------------------------------------
158  computeObservationLikelihood
159  ---------------------------------------------------------------*/
161  const CObservation* obs, const CPose3D& takenFrom)
162 {
163  MRPT_UNUSED_PARAM(obs);
164  MRPT_UNUSED_PARAM(takenFrom);
165  THROW_EXCEPTION("Not implemented yet!");
166 }
167 
170 {
171  dyngridcommon_writeToStream(out);
172 
173  // To assure compatibility: The size of each cell:
174  uint32_t n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
175  out << n;
176 
177  // Save the map contents:
178  n = static_cast<uint32_t>(m_map.size());
179  out << n;
180 
181 // Save the "m_map": This requires special handling for big endian systems:
182 #if MRPT_IS_BIG_ENDIAN
183  for (uint32_t i = 0; i < n; i++)
184  {
185  out << m_map[i].kf_mean << m_map[i].dm_mean << m_map[i].dmv_var_mean;
186  }
187 #else
188  // Little endian: just write all at once:
189  out.WriteBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
190 #endif
191 
192  // Save the insertion options:
193  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
194 
195  out << insertionOptions.sigma << insertionOptions.cutoffRadius
196  << insertionOptions.R_min << insertionOptions.R_max
197  << insertionOptions.KF_covSigma << insertionOptions.KF_initialCellStd
198  << insertionOptions.KF_observationModelNoise
199  << insertionOptions.KF_defaultCellMeanValue
200  << insertionOptions.KF_W_size;
201 
202  out << m_average_normreadings_mean << m_average_normreadings_var
203  << uint64_t(m_average_normreadings_count);
204 
205  out << genericMapParams;
206 }
207 
210 {
211  switch (version)
212  {
213  case 0:
214  {
215  dyngridcommon_readFromStream(in);
216 
217  // To assure compatibility: The size of each cell:
218  uint32_t n;
219  in >> n;
220 
221  ASSERT_EQUAL_(n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
222  // Load the map contents:
223  in >> n;
224  m_map.resize(n);
225 
226 // Read the note in writeToStream()
227 #if MRPT_IS_BIG_ENDIAN
228  for (uint32_t i = 0; i < n; i++)
229  in >> m_map[i].kf_mean >> m_map[i].dm_mean >>
230  m_map[i].dmv_var_mean;
231 #else
232  // Little endian: just read all at once:
233  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
234 #endif
235 
236  {
237  uint8_t i;
238  in >> i;
239  m_mapType = TMapRepresentation(i);
240 
241  in >> m_cov >> m_stackedCov;
242 
243  in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >>
244  insertionOptions.R_min >> insertionOptions.R_max >>
245  insertionOptions.KF_covSigma >>
246  insertionOptions.KF_initialCellStd >>
247  insertionOptions.KF_observationModelNoise >>
248  insertionOptions.KF_defaultCellMeanValue >>
249  insertionOptions.KF_W_size;
250  }
251 
252  {
253  uint64_t N;
254  in >> m_average_normreadings_mean >>
255  m_average_normreadings_var >> N;
256  m_average_normreadings_count = N;
257  }
258 
259  in >> genericMapParams;
260 
261  m_hasToRecoverMeanAndCov = true;
262  }
263  break;
264  default:
266  };
267 }
268 
269 /*---------------------------------------------------------------
270  TInsertionOptions
271  ---------------------------------------------------------------*/
274  std::ostream& out) const
275 {
276  out << mrpt::format(
277  "\n----------- [CHeightGridMap2D_MRF::TInsertionOptions] ------------ "
278  "\n\n");
279  out << mrpt::format("[TInsertionOptions.Common] ------------ \n\n");
280  internal_dumpToTextStream_common(
281  out); // Common params to all random fields maps:
282 
283  out << mrpt::format("\n");
284 }
285 
286 /*---------------------------------------------------------------
287  loadFromConfigFile
288  ---------------------------------------------------------------*/
290  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
291 {
292  // Common data fields for all random fields maps:
293  internal_loadFromConfigFile_common(iniFile, section);
294 
295  // Specific data fields for this class:
296  // ...
297 }
298 
299 /*---------------------------------------------------------------
300  getAs3DObject
301 ---------------------------------------------------------------*/
303  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
304 {
305  MRPT_START
306  if (!genericMapParams.enableSaveAs3DObject) return;
308  MRPT_END
309 }
310 
311 /*---------------------------------------------------------------
312  getAs3DObject
313 ---------------------------------------------------------------*/
316  mrpt::opengl::CSetOfObjects::Ptr& varObj) const
317 {
318  MRPT_START
319  if (!genericMapParams.enableSaveAs3DObject) return;
320  CRandomFieldGridMap2D::getAs3DObject(meanObj, varObj);
321  MRPT_END
322 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
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...
Extra params for insertIndividualPoint()
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
#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)
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
GLenum GLsizei n
Definition: glext.h:5074
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
STL namespace.
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:48
CHeightGridMap2D_MRF represents digital-elevation-model over a 2D area, with uncertainty, based on a Markov-Random-Field (MRF) estimator.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
void dumpToTextStream_map_specific(std::ostream &out) const override
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
double kf_mean
[KF-methods only] The mean value of this cell
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
The contents of each cell in a CRandomFieldGridMap2D map.
A helper class that can convert an enum value into its textual representation, and viceversa...
double min_x
See CHeightGridMap2D_MRF::CHeightGridMap2D_MRF.
virtual size_t dem_get_size_y() const override
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
virtual size_t dem_get_size_x() const override
GLsizei const GLchar ** string
Definition: glext.h:4101
virtual void dem_update_map() override
Ensure that all observations are reflected in the map estimate.
virtual bool insertIndividualPoint(const double x, const double y, const double z, const CHeightGridMap2D_Base::TPointInsertParams &params=CHeightGridMap2D_Base::TPointInsertParams()) override
Update the DEM with one new point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
virtual bool dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const override
Get cell &#39;z&#39; by (cx,cy) cell indices.
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
#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...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
unsigned __int64 uint64_t
Definition: rptypes.h:50
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.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions insertionOpts
Observations insertion options.
virtual void internal_clear() override
Erase all the contents of the map.
#define MRPT_END
Definition: exceptions.h:266
void internal_clear() override
Erase all the contents of the map.
GLuint in
Definition: glext.h:7274
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
GLenum GLint GLint y
Definition: glext.h:3538
virtual bool dem_get_z(const double x, const double y, double &z_out) const override
Get cell &#39;z&#39; (x,y) by metric coordinates.
virtual double dem_get_resolution() const override
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
GLenum GLint x
Definition: glext.h:3538
unsigned __int32 uint32_t
Definition: rptypes.h:47
Lightweight 2D point.
GLenum const GLfloat * params
Definition: glext.h:3534
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020