MRPT  2.0.4
CHeightGridMap2D_MRF.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 #include "maps-precomp.h" // Precomp header
11 
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/poses/CPose3D.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::maps;
18 using namespace mrpt::obs;
19 using namespace mrpt::poses;
20 using namespace std;
21 using namespace mrpt::math;
22 
23 // =========== Begin of Map definition ============
25  "mrpt::maps::CHeightGridMap2D_MRF,dem_mrf",
27 
29 
31  const mrpt::config::CConfigFileBase& source,
32  const std::string& sectionNamePrefix)
33 {
34  // [<sectionNamePrefix>+"_creationOpts"]
35  const std::string sSectCreation =
36  sectionNamePrefix + string("_creationOpts");
38  run_map_estimation_at_ctor, bool, source, sSectCreation);
39  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
40  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
41  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
42  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
43  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
45  sSectCreation, "mapType", mapType);
46 
47  insertionOpts.loadFromConfigFile(
48  source, sectionNamePrefix + string("_insertOpts"));
49 }
50 
52  std::ostream& out) const
53 {
54  out << mrpt::format(
55  "MAP TYPE = %s\n",
57  CHeightGridMap2D_MRF::TMapRepresentation>::value2name(mapType)
58  .c_str());
59  LOADABLEOPTS_DUMP_VAR(run_map_estimation_at_ctor, bool);
60  LOADABLEOPTS_DUMP_VAR(min_x, double);
61  LOADABLEOPTS_DUMP_VAR(max_x, double);
62  LOADABLEOPTS_DUMP_VAR(min_y, double);
63  LOADABLEOPTS_DUMP_VAR(max_y, double);
64  LOADABLEOPTS_DUMP_VAR(resolution, double);
65 
66  this->insertionOpts.dumpToTextStream(out);
67 }
68 
71 {
73  *dynamic_cast<const CHeightGridMap2D_MRF::TMapDefinition*>(&_def);
74  auto* obj = new CHeightGridMap2D_MRF(
75  def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution,
77  obj->insertionOptions = def.insertionOpts;
78  return obj;
79 }
80 // =========== End of Map definition Block =========
81 
83 
84 // Constructor
86  TMapRepresentation mapType, double x_min, double x_max, double y_min,
87  double y_max, double resolution, bool run_first_map_estimation_now)
88  : CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution),
89  insertionOptions()
90 {
91  m_rfgm_run_update_upon_clear = run_first_map_estimation_now;
92  // Set the grid to initial values (and adjusts the KF covariance matrix!)
93  // Also, calling clear() is mandatory to end initialization of our base
94  // class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
96 }
97 
99  const double x, const double y, const double z,
101 {
102  const TRandomFieldCell* cell = cellByPos(x, y);
103  if (!cell) return false;
104  this->insertIndividualReading(
105  z, mrpt::math::TPoint2D(x, y), params.update_map_after_insertion,
106  true /*time invariant*/, params.pt_z_std);
107  return true;
108 }
109 double CHeightGridMap2D_MRF::dem_get_resolution() const { return m_resolution; }
110 size_t CHeightGridMap2D_MRF::dem_get_size_x() const { return m_size_x; }
111 size_t CHeightGridMap2D_MRF::dem_get_size_y() const { return m_size_y; }
113  const size_t cx, const size_t cy, double& z_out) const
114 {
115  const TRandomFieldCell* cell = cellByIndex(cx, cy);
116  if (cell && cell->kf_mean())
117  {
118  z_out = cell->kf_mean();
119  return true;
120  }
121  else
122  return false;
123 }
125  const double x, const double y, double& z_out) const
126 {
127  const TRandomFieldCell* cell = cellByPos(x, y);
128  if (cell && cell->kf_mean())
129  {
130  z_out = cell->kf_mean();
131  return true;
132  }
133  else
134  return false;
135 }
136 void CHeightGridMap2D_MRF::dem_update_map() { this->updateMapEstimation(); }
138 {
139  // Just do the generic clear:
141  // Anything else special for this derived class?
142 }
143 
145  const CObservation& obs, const CPose3D* robotPose)
146 {
147  return dem_internal_insertObservation(obs, robotPose);
148 }
149 
150 /*---------------------------------------------------------------
151  computeObservationLikelihood
152  ---------------------------------------------------------------*/
154  [[maybe_unused]] const CObservation& obs,
155  [[maybe_unused]] const CPose3D& takenFrom)
156 {
157  THROW_EXCEPTION("Not implemented yet!");
158 }
159 
160 uint8_t CHeightGridMap2D_MRF::serializeGetVersion() const { return 0; }
162 {
163  dyngridcommon_writeToStream(out);
164 
165  // To assure compatibility: The size of each cell:
166  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
167  out << n;
168 
169  // Save the map contents:
170  n = static_cast<uint32_t>(m_map.size());
171  out << n;
172 
173 // Save the "m_map": This requires special handling for big endian systems:
174 #if MRPT_IS_BIG_ENDIAN
175  for (uint32_t i = 0; i < n; i++)
176  {
177  out << m_map[i].kf_mean() << m_map[i].dm_mean()
178  << m_map[i].dmv_var_mean;
179  }
180 #else
181  // Little endian: just write all at once:
182  out.WriteBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
183 #endif
184 
185  // Save the insertion options:
186  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
187 
188  out << insertionOptions.sigma << insertionOptions.cutoffRadius
189  << insertionOptions.R_min << insertionOptions.R_max
190  << insertionOptions.KF_covSigma << insertionOptions.KF_initialCellStd
191  << insertionOptions.KF_observationModelNoise
192  << insertionOptions.KF_defaultCellMeanValue
193  << insertionOptions.KF_W_size;
194 
195  out << m_average_normreadings_mean << m_average_normreadings_var
196  << uint64_t(m_average_normreadings_count);
197 
198  out << genericMapParams;
199 }
200 
202  mrpt::serialization::CArchive& in, uint8_t version)
203 {
204  switch (version)
205  {
206  case 0:
207  {
208  dyngridcommon_readFromStream(in);
209 
210  // To assure compatibility: The size of each cell:
211  uint32_t n;
212  in >> n;
213 
214  ASSERT_EQUAL_(n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
215  // Load the map contents:
216  in >> n;
217  m_map.resize(n);
218 
219 // Read the note in writeToStream()
220 #if MRPT_IS_BIG_ENDIAN
221  for (uint32_t i = 0; i < n; i++)
222  in >> m_map[i].kf_mean() >> m_map[i].dm_mean() >>
223  m_map[i].dmv_var_mean;
224 #else
225  // Little endian: just read all at once:
226  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
227 #endif
228 
229  {
230  uint8_t i;
231  in >> i;
232  m_mapType = TMapRepresentation(i);
233 
234  in >> m_cov >> m_stackedCov;
235 
236  in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >>
237  insertionOptions.R_min >> insertionOptions.R_max >>
238  insertionOptions.KF_covSigma >>
239  insertionOptions.KF_initialCellStd >>
240  insertionOptions.KF_observationModelNoise >>
241  insertionOptions.KF_defaultCellMeanValue >>
242  insertionOptions.KF_W_size;
243  }
244 
245  {
246  uint64_t N;
247  in >> m_average_normreadings_mean >>
248  m_average_normreadings_var >> N;
249  m_average_normreadings_count = N;
250  }
251 
252  in >> genericMapParams;
253 
254  m_hasToRecoverMeanAndCov = true;
255  }
256  break;
257  default:
259  };
260 }
261 
262 /*---------------------------------------------------------------
263  TInsertionOptions
264  ---------------------------------------------------------------*/
267  std::ostream& out) const
268 {
269  out << "\n----------- [CHeightGridMap2D_MRF::TInsertionOptions] "
270  "------------ "
271  "\n\n";
272  out << "[TInsertionOptions.Common] ------------ \n\n";
273  internal_dumpToTextStream_common(
274  out); // Common params to all random fields maps:
275 
276  out << "\n";
277 }
278 
279 /*---------------------------------------------------------------
280  loadFromConfigFile
281  ---------------------------------------------------------------*/
283  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
284 {
285  // Common data fields for all random fields maps:
286  internal_loadFromConfigFile_common(iniFile, section);
287 
288  // Specific data fields for this class:
289  // ...
290 }
291 
292 /*---------------------------------------------------------------
293  getAs3DObject
294 ---------------------------------------------------------------*/
296  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
297 {
298  MRPT_START
299  if (!genericMapParams.enableSaveAs3DObject) return;
301  MRPT_END
302 }
303 
304 /*---------------------------------------------------------------
305  getAs3DObject
306 ---------------------------------------------------------------*/
309  mrpt::opengl::CSetOfObjects::Ptr& varObj) const
310 {
311  MRPT_START
312  if (!genericMapParams.enableSaveAs3DObject) return;
313  CRandomFieldGridMap2D::getAs3DObject(meanObj, varObj);
314  MRPT_END
315 }
mrpt::maps::CRandomFieldGridMap2D::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
Definition: CRandomFieldGridMap2D.cpp:1586
mrpt::maps::CHeightGridMap2D_MRF::dem_get_z_by_cell
bool dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const override
Get cell 'z' by (cx,cy) cell indices.
Definition: CHeightGridMap2D_MRF.cpp:112
mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions::loadFromConfigFile
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.
Definition: CHeightGridMap2D_MRF.cpp:282
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
mrpt::maps::CHeightGridMap2D_MRF::internal_CreateFromMapDefinition
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
Definition: CHeightGridMap2D_MRF.cpp:69
mrpt::math::TPoint2D_< double >
mrpt::maps::CHeightGridMap2D_Base::TPointInsertParams
Extra params for insertIndividualPoint()
Definition: CHeightGridMap2D_Base.h:40
mrpt::maps::TRandomFieldCell::kf_mean
double & kf_mean()
[KF-methods only] The mean value of this cell
Definition: CRandomFieldGridMap2D.h:59
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:91
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::dumpToTextStream_map_specific
void dumpToTextStream_map_specific(std::ostream &out) const override
Definition: CHeightGridMap2D_MRF.cpp:51
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::max_y
double max_y
Definition: CHeightGridMap2D_MRF.h:103
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::max_x
double max_x
Definition: CHeightGridMap2D_MRF.h:103
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::maps::CHeightGridMap2D_MRF::dem_get_size_x
size_t dem_get_size_x() const override
Definition: CHeightGridMap2D_MRF.cpp:110
mrpt::serialization::CArchive::ReadBuffer
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:32
mrpt::maps::CHeightGridMap2D_MRF::insertIndividualPoint
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.
Definition: CHeightGridMap2D_MRF.cpp:98
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
CPose2D.h
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::maps::CHeightGridMap2D_MRF
CHeightGridMap2D_MRF represents digital-elevation-model over a 2D area, with uncertainty,...
Definition: CHeightGridMap2D_MRF.h:33
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::resolution
double resolution
Definition: CHeightGridMap2D_MRF.h:103
mrpt::maps::CRandomFieldGridMap2D::internal_clear
void internal_clear() override
Erase all the contents of the map.
Definition: CRandomFieldGridMap2D.cpp:83
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::run_map_estimation_at_ctor
bool run_map_estimation_at_ctor
Runs map estimation at start up (Default:true)
Definition: CHeightGridMap2D_MRF.h:101
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::insertionOpts
mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions insertionOpts
Observations insertion options.
Definition: CHeightGridMap2D_MRF.h:108
mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
Definition: CRandomFieldGridMap2D.h:175
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::TMapDefinition
TMapDefinition()
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions::TInsertionOptions
TInsertionOptions()
Default values loader.
mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CHeightGridMap2D_MRF.cpp:266
mrpt::typemeta::TEnumType
A helper class that can convert an enum value into its textual representation, and viceversa.
Definition: config/CConfigFileBase.h:24
mrpt::maps::CHeightGridMap2D_MRF::internal_insertObservation
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
Definition: CHeightGridMap2D_MRF.cpp:144
LOADABLEOPTS_DUMP_VAR
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
Definition: config/CLoadableOptions.h:101
mrpt::config::CConfigFileBase::read_enum
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
Definition: config/CConfigFileBase.h:269
mrpt::maps::CHeightGridMap2D_MRF::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CHeightGridMap2D_MRF.cpp:295
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::maps::CHeightGridMap2D_MRF::internal_computeObservationLikelihood
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
Definition: CHeightGridMap2D_MRF.cpp:153
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::maps::CHeightGridMap2D_MRF::dem_get_size_y
size_t dem_get_size_y() const override
Definition: CHeightGridMap2D_MRF.cpp:111
CPose3D.h
mrpt::maps::CHeightGridMap2D_MRF::dem_get_resolution
double dem_get_resolution() const override
Definition: CHeightGridMap2D_MRF.cpp:109
mrpt::maps::CHeightGridMap2D_MRF::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CHeightGridMap2D_MRF.cpp:201
mrpt::maps::CHeightGridMap2D_MRF::dem_update_map
void dem_update_map() override
Ensure that all observations are reflected in the map estimate.
Definition: CHeightGridMap2D_MRF.cpp:136
maps-precomp.h
mrpt::maps::CRandomFieldGridMap2D
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
Definition: CRandomFieldGridMap2D.h:152
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CHeightGridMap2D_MRF::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CHeightGridMap2D_MRF.cpp:160
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::min_x
double min_x
See CHeightGridMap2D_MRF::CHeightGridMap2D_MRF.
Definition: CHeightGridMap2D_MRF.h:103
CHeightGridMap2D_MRF.h
mrpt::maps::CHeightGridMap2D_MRF::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CHeightGridMap2D_MRF.cpp:161
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::maps::CHeightGridMap2D_MRF::internal_clear
void internal_clear() override
Internal method called by clear()
Definition: CHeightGridMap2D_MRF.cpp:137
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::mapType
mrpt::maps::CHeightGridMap2D_MRF::TMapRepresentation mapType
The kind of map representation (see CHeightGridMap2D_MRF::CHeightGridMap2D_MRF)
Definition: CHeightGridMap2D_MRF.h:106
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::loadFromConfigFile_map_specific
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
Definition: CHeightGridMap2D_MRF.cpp:30
mrpt::maps::TRandomFieldCell
The contents of each cell in a CRandomFieldGridMap2D map.
Definition: CRandomFieldGridMap2D.h:36
mrpt::maps::CHeightGridMap2D_MRF::dem_get_z
bool dem_get_z(const double x, const double y, double &z_out) const override
Get cell 'z' (x,y) by metric coordinates.
Definition: CHeightGridMap2D_MRF.cpp:124
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::min_y
double min_y
Definition: CHeightGridMap2D_MRF.h:103
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition
Definition: CHeightGridMap2D_MRF.h:99
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020