MRPT  2.0.4
CWirelessPowerGridMap2D.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 
12 #include <mrpt/core/round.h>
13 #include <mrpt/img/color_maps.h>
18 #include <mrpt/system/CTicTac.h>
19 #include <mrpt/system/os.h>
20 
21 using namespace mrpt;
22 using namespace mrpt::maps;
23 using namespace mrpt::obs;
24 using namespace mrpt::poses;
25 using namespace std;
26 
27 // =========== Begin of Map definition ============
29  "mrpt::maps::CWirelessPowerGridMap2D,wifiGrid",
31 
33 
35  const mrpt::config::CConfigFileBase& source,
36  const std::string& sectionNamePrefix)
37 {
38  // [<sectionNamePrefix>+"_creationOpts"]
39  const std::string sSectCreation =
40  sectionNamePrefix + string("_creationOpts");
41  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
42  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
43  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
44  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
45  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
47  sSectCreation, "mapType", mapType);
48 
49  insertionOpts.loadFromConfigFile(
50  source, sectionNamePrefix + string("_insertOpts"));
51 }
52 
54  std::ostream& out) const
55 {
56  out << mrpt::format(
57  "MAP TYPE = %s\n",
60  .c_str());
61  LOADABLEOPTS_DUMP_VAR(min_x, double);
62  LOADABLEOPTS_DUMP_VAR(max_x, double);
63  LOADABLEOPTS_DUMP_VAR(min_y, double);
64  LOADABLEOPTS_DUMP_VAR(max_y, double);
65  LOADABLEOPTS_DUMP_VAR(resolution, double);
66 
67  this->insertionOpts.dumpToTextStream(out);
68 }
69 
73 {
75  *dynamic_cast<const CWirelessPowerGridMap2D::TMapDefinition*>(&_def);
76  auto* obj = new CWirelessPowerGridMap2D(
77  def.mapType, def.min_x, def.max_x, def.min_y, def.max_y,
78  def.resolution);
79  obj->insertionOptions = def.insertionOpts;
80  return obj;
81 }
82 // =========== End of Map definition Block =========
83 
86 
87 /*---------------------------------------------------------------
88  Constructor
89  ---------------------------------------------------------------*/
91  TMapRepresentation mapType, double x_min, double x_max, double y_min,
92  double y_max, double resolution)
93  : CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution),
94  insertionOptions()
95 {
96  // Set the grid to initial values (and adjusts the KF covariance matrix!)
97  // Also, calling clear() is mandatory to end initialization of our base
98  // class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
100 }
101 
102 CWirelessPowerGridMap2D::~CWirelessPowerGridMap2D() = default;
103 /*---------------------------------------------------------------
104  clear
105  ---------------------------------------------------------------*/
106 void CWirelessPowerGridMap2D::internal_clear()
107 {
108  // Just do the generic clear:
110 
111  // Anything else special for this derived class?
112  // ...
113 }
114 
115 /*---------------------------------------------------------------
116  insertObservation
117  ---------------------------------------------------------------*/
119  const CObservation& obs, const CPose3D* robotPose)
120 {
121  MRPT_START
122 
123  CPose2D robotPose2D;
124  CPose3D robotPose3D;
125 
126  if (robotPose)
127  {
128  robotPose2D = CPose2D(*robotPose);
129  robotPose3D = (*robotPose);
130  }
131  else
132  {
133  // Default values are (0,0,0)
134  }
135 
137  {
138  /********************************************************************
139  OBSERVATION TYPE: CObservationWirelessPower
140  ********************************************************************/
141  const auto& o = dynamic_cast<const CObservationWirelessPower&>(obs);
142  float sensorReading;
143 
144  // Compute the 3D sensor pose in world coordinates:
145  CPose2D sensorPose = CPose2D(robotPose3D + o.sensorPoseOnRobot);
146 
147  sensorReading = o.power;
148 
149  // Normalization:
150  sensorReading = (sensorReading - insertionOptions.R_min) /
151  (insertionOptions.R_max - insertionOptions.R_min);
152 
153  // Update the gross estimates of mean/vars for the whole reading history
154  // (see IROS2009 paper):
156  (sensorReading +
160  (square(sensorReading - m_average_normreadings_mean) +
164 
165  // Finally, do the actual map update with that value:
167  sensorReading,
168  mrpt::math::TPoint2D(sensorPose.x(), sensorPose.y()));
169 
170  return true; // Done!
171 
172  } // end if "CObservationWirelessPower"
173 
174  return false;
175 
176  MRPT_END
177 }
178 
179 /*---------------------------------------------------------------
180  computeObservationLikelihood
181  ---------------------------------------------------------------*/
183  [[maybe_unused]] const CObservation& obs,
184  [[maybe_unused]] const CPose3D& takenFrom)
185 {
186  THROW_EXCEPTION("Not implemented yet!");
187 }
188 
189 uint8_t CWirelessPowerGridMap2D::serializeGetVersion() const { return 5; }
192 {
194 
195  // To ensure compatibility: The size of each cell:
196  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
197  out << n;
198 
199  // Save the map contents:
200  n = static_cast<uint32_t>(m_map.size());
201  out << n;
202 
203 // Save the "m_map": This requires special handling for big endian systems:
204 #if MRPT_IS_BIG_ENDIAN
205  for (uint32_t i = 0; i < n; i++)
206  {
207  out << m_map[i].kf_mean() << m_map[i].dm_mean()
208  << m_map[i].dmv_var_mean;
209  }
210 #else
211  // Little endian: just write all at once:
212  out.WriteBuffer(
213  &m_map[0],
214  sizeof(m_map[0]) * m_map.size()); // TODO: Do this endianness safe!!
215 #endif
216 
217  // Version 1: Save the insertion options:
218  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
219 
220  out << insertionOptions.sigma << insertionOptions.cutoffRadius
221  << insertionOptions.R_min << insertionOptions.R_max
222  << insertionOptions.KF_covSigma << insertionOptions.KF_initialCellStd
223  << insertionOptions.KF_observationModelNoise
224  << insertionOptions.KF_defaultCellMeanValue
225  << insertionOptions.KF_W_size;
226 
227  // New in v3:
229  << uint64_t(m_average_normreadings_count);
230 
231  out << genericMapParams; // v4
232 }
233 
234 // Aux struct used below (must be at global scope for STL):
236 {
237  float mean, std;
238  float w, wr;
239 };
240 
242  mrpt::serialization::CArchive& in, uint8_t version)
243 {
244  switch (version)
245  {
246  case 0:
247  case 1:
248  case 2:
249  case 3:
250  case 4:
251  case 5:
252  {
253  dyngridcommon_readFromStream(in, version < 5);
254 
255  // To ensure compatibility: The size of each cell:
256  uint32_t n;
257  in >> n;
258 
259  if (version < 2)
260  { // Converter from old versions <=1
261  ASSERT_(
262  n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
263  // Load the map contents in an aux struct:
264  in >> n;
265  vector<TOldCellTypeInVersion1> old_map(n);
266  in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());
267 
268  // Convert to newer format:
269  m_map.resize(n);
270  for (size_t k = 0; k < n; k++)
271  {
272  m_map[k].kf_mean() =
273  (old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
274  m_map[k].kf_std() =
275  (old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
276  }
277  }
278  else
279  {
281  n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
282  // Load the map contents:
283  in >> n;
284  m_map.resize(n);
285 
286 // Read the note in writeToStream()
287 #if MRPT_IS_BIG_ENDIAN
288  for (uint32_t i = 0; i < n; i++)
289  in >> m_map[i].kf_mean() >> m_map[i].dm_mean() >>
290  m_map[i].dmv_var_mean;
291 #else
292  // Little endian: just read all at once:
293  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
294 #endif
295  }
296 
297  // Version 1: Insertion options:
298  if (version >= 1)
299  {
300  uint8_t i;
301  in >> i;
303 
304  in >> m_cov >> m_stackedCov;
305 
306  in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >>
307  insertionOptions.R_min >> insertionOptions.R_max >>
308  insertionOptions.KF_covSigma >>
309  insertionOptions.KF_initialCellStd >>
310  insertionOptions.KF_observationModelNoise >>
311  insertionOptions.KF_defaultCellMeanValue >>
312  insertionOptions.KF_W_size;
313  }
314 
315  if (version >= 3)
316  {
317  uint64_t N;
321  }
322 
323  if (version >= 4) in >> genericMapParams;
324 
326  }
327  break;
328  default:
330  };
331 }
332 
335  std::ostream& out) const
336 {
337  out << "\n----------- [CWirelessPowerGridMap2D::TInsertionOptions] "
338  "------------ \n\n";
339  internal_dumpToTextStream_common(
340  out); // Common params to all random fields maps:
341 
342  out << "\n";
343 }
344 
345 /*---------------------------------------------------------------
346  loadFromConfigFile
347  ---------------------------------------------------------------*/
349  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
350 {
351  // Common data fields for all random fields maps:
352  internal_loadFromConfigFile_common(iniFile, section);
353 }
354 
355 /*---------------------------------------------------------------
356  getAs3DObject
357 ---------------------------------------------------------------*/
359  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
360 {
361  MRPT_START
364  MRPT_END
365 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:42
IMPLEMENTS_SERIALIZABLE(CWirelessPowerGridMap2D, CRandomFieldGridMap2D, mrpt::maps) CWirelessPowerGridMap2D
CWirelessPowerGridMap2D(TMapRepresentation mapType=mrKernelDM, double x_min=-2, double x_max=2, double y_min=-2, double y_max=2, double resolution=0.1)
Constructor.
#define MRPT_START
Definition: exceptions.h:241
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions insertionOptions
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:274
void dumpToTextStream_map_specific(std::ostream &out) const override
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
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.
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...
CWirelessPowerGridMap2D represents a PDF of wifi concentrations over a 2D area.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
This class allows loading and storing values and vectors of different types from a configuration text...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
The contents of each cell in a CRandomFieldGridMap2D map.
A helper class that can convert an enum value into its textual representation, and viceversa...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
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.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void dyngridcommon_readFromStream(STREAM &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:342
This represents a measurement of the wireless strength perceived by the robot.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions insertionOpts
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
return_t square(const num_t x)
Inline function for the square of a number.
#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...
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:54
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void internal_clear() override
Erase all the contents of the map.
#define MRPT_END
Definition: exceptions.h:245
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
mrpt::maps::CWirelessPowerGridMap2D::TMapRepresentation mapType
The kind of map representation (see CWirelessPowerGridMap2D::CWirelessPowerGridMap2D) ...
double mean(const CONTAINER &v)
Computes the mean value of a vector.
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
double min_x
See CWirelessPowerGridMap2D::CWirelessPowerGridMap2D.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
void insertIndividualReading(const double sensorReading, const mrpt::math::TPoint2D &point, const bool update_map=true, const bool time_invariant=true, const double reading_stddev=.0)
Direct update of the map with a reading in a given position of the map, using the appropriate method ...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 3e8ecd670 Mon May 18 14:23:19 2020 +0200 at lun may 18 14:30:11 CEST 2020