MRPT  1.9.9
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-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 #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 
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  const CObservation& obs, const CPose3D& takenFrom)
184 {
185  MRPT_UNUSED_PARAM(obs);
186  MRPT_UNUSED_PARAM(takenFrom);
187 
188  THROW_EXCEPTION("Not implemented yet!");
189 }
190 
191 uint8_t CWirelessPowerGridMap2D::serializeGetVersion() const { return 5; }
194 {
196 
197  // To ensure compatibility: The size of each cell:
198  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
199  out << n;
200 
201  // Save the map contents:
202  n = static_cast<uint32_t>(m_map.size());
203  out << n;
204 
205 // Save the "m_map": This requires special handling for big endian systems:
206 #if MRPT_IS_BIG_ENDIAN
207  for (uint32_t i = 0; i < n; i++)
208  {
209  out << m_map[i].kf_mean << m_map[i].dm_mean << m_map[i].dmv_var_mean;
210  }
211 #else
212  // Little endian: just write all at once:
213  out.WriteBuffer(
214  &m_map[0],
215  sizeof(m_map[0]) * m_map.size()); // TODO: Do this endianness safe!!
216 #endif
217 
218  // Version 1: Save the insertion options:
219  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
220 
221  out << insertionOptions.sigma << insertionOptions.cutoffRadius
222  << insertionOptions.R_min << insertionOptions.R_max
223  << insertionOptions.KF_covSigma << insertionOptions.KF_initialCellStd
224  << insertionOptions.KF_observationModelNoise
225  << insertionOptions.KF_defaultCellMeanValue
226  << insertionOptions.KF_W_size;
227 
228  // New in v3:
231 
232  out << genericMapParams; // v4
233 }
234 
235 // Aux struct used below (must be at global scope for STL):
237 {
238  float mean, std;
239  float w, wr;
240 };
241 
243  mrpt::serialization::CArchive& in, uint8_t version)
244 {
245  switch (version)
246  {
247  case 0:
248  case 1:
249  case 2:
250  case 3:
251  case 4:
252  case 5:
253  {
254  dyngridcommon_readFromStream(in, version < 5);
255 
256  // To ensure compatibility: The size of each cell:
257  uint32_t n;
258  in >> n;
259 
260  if (version < 2)
261  { // Converter from old versions <=1
262  ASSERT_(
263  n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
264  // Load the map contents in an aux struct:
265  in >> n;
266  vector<TOldCellTypeInVersion1> old_map(n);
267  in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());
268 
269  // Convert to newer format:
270  m_map.resize(n);
271  for (size_t k = 0; k < n; k++)
272  {
273  m_map[k].kf_mean =
274  (old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
275  m_map[k].kf_std =
276  (old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
277  }
278  }
279  else
280  {
282  n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
283  // Load the map contents:
284  in >> n;
285  m_map.resize(n);
286 
287 // Read the note in writeToStream()
288 #if MRPT_IS_BIG_ENDIAN
289  for (uint32_t i = 0; i < n; i++)
290  in >> m_map[i].kf_mean >> m_map[i].dm_mean >>
291  m_map[i].dmv_var_mean;
292 #else
293  // Little endian: just read all at once:
294  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
295 #endif
296  }
297 
298  // Version 1: Insertion options:
299  if (version >= 1)
300  {
301  uint8_t i;
302  in >> i;
304 
305  in >> m_cov >> m_stackedCov;
306 
307  in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >>
308  insertionOptions.R_min >> insertionOptions.R_max >>
309  insertionOptions.KF_covSigma >>
310  insertionOptions.KF_initialCellStd >>
311  insertionOptions.KF_observationModelNoise >>
312  insertionOptions.KF_defaultCellMeanValue >>
313  insertionOptions.KF_W_size;
314  }
315 
316  if (version >= 3)
317  {
318  uint64_t N;
322  }
323 
324  if (version >= 4) in >> genericMapParams;
325 
327  }
328  break;
329  default:
331  };
332 }
333 
336  std::ostream& out) const
337 {
338  out << "\n----------- [CWirelessPowerGridMap2D::TInsertionOptions] "
339  "------------ \n\n";
340  internal_dumpToTextStream_common(
341  out); // Common params to all random fields maps:
342 
343  out << "\n";
344 }
345 
346 /*---------------------------------------------------------------
347  loadFromConfigFile
348  ---------------------------------------------------------------*/
350  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
351 {
352  // Common data fields for all random fields maps:
353  internal_loadFromConfigFile_common(iniFile, section);
354 }
355 
356 /*---------------------------------------------------------------
357  getAs3DObject
358 ---------------------------------------------------------------*/
360  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
361 {
362  MRPT_START
365  MRPT_END
366 }
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
GLenum GLsizei n
Definition: glext.h:5136
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:275
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
void dumpToTextStream_map_specific(std::ostream &out) const override
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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.
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
T square(const T x)
Inline function for the square of a number.
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
GLsizei const GLchar ** string
Definition: glext.h:4116
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...
#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
unsigned __int64 uint64_t
Definition: glext.h:3457
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) ...
GLuint in
Definition: glext.h:7391
double mean(const CONTAINER &v)
Computes the mean value of a vector.
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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()
Lightweight 2D point.
Definition: TPoint2D.h:31
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 ...
#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: cfb2df9d3 Fri Nov 15 06:57:14 2019 +0100 at vie nov 15 07:00:11 CET 2019