MRPT  1.9.9
CWirelessPowerGridMap2D.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
11 
14 #include <mrpt/system/os.h>
15 #include <mrpt/core/round.h>
16 #include <mrpt/system/CTicTac.h>
17 #include <mrpt/img/color_maps.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  "CWirelessPowerGridMap2D,wifiGrid", mrpt::maps::CWirelessPowerGridMap2D)
30 
32  : min_x(-2),
33  max_x(2),
34  min_y(-2),
35  max_y(2),
36  resolution(0.10f),
37  mapType(CWirelessPowerGridMap2D::mrKernelDM)
38 {
39 }
40 
43  const std::string& sectionNamePrefix)
44 {
45  // [<sectionNamePrefix>+"_creationOpts"]
46  const std::string sSectCreation =
47  sectionNamePrefix + string("_creationOpts");
48  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
49  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
50  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
51  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
52  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
54  sSectCreation, "mapType", mapType);
55 
56  insertionOpts.loadFromConfigFile(
57  source, sectionNamePrefix + string("_insertOpts"));
58 }
59 
61  std::ostream& out) const
62 {
63  out << mrpt::format(
64  "MAP TYPE = %s\n",
67  .c_str());
68  LOADABLEOPTS_DUMP_VAR(min_x, double);
69  LOADABLEOPTS_DUMP_VAR(max_x, double);
70  LOADABLEOPTS_DUMP_VAR(min_y, double);
71  LOADABLEOPTS_DUMP_VAR(max_y, double);
72  LOADABLEOPTS_DUMP_VAR(resolution, double);
73 
74  this->insertionOpts.dumpToTextStream(out);
75 }
76 
80 {
82  *dynamic_cast<const CWirelessPowerGridMap2D::TMapDefinition*>(&_def);
84  def.mapType, def.min_x, def.max_x, def.min_y, def.max_y,
85  def.resolution);
86  obj->insertionOptions = def.insertionOpts;
87  return obj;
88 }
89 // =========== End of Map definition Block =========
90 
93 
94 /*---------------------------------------------------------------
95  Constructor
96  ---------------------------------------------------------------*/
98  TMapRepresentation mapType, double x_min, double x_max, double y_min,
99  double y_max, double resolution)
100  : CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution),
101  insertionOptions()
102 {
103  // Set the grid to initial values (and adjusts the KF covariance matrix!)
104  // Also, calling clear() is mandatory to end initialization of our base
105  // class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
107 }
108 
109 CWirelessPowerGridMap2D::~CWirelessPowerGridMap2D() {}
110 /*---------------------------------------------------------------
111  clear
112  ---------------------------------------------------------------*/
114 {
115  // Just do the generic clear:
117 
118  // Anything else special for this derived class?
119  // ...
120 }
121 
122 /*---------------------------------------------------------------
123  insertObservation
124  ---------------------------------------------------------------*/
126  const CObservation* obs, const CPose3D* robotPose)
127 {
128  MRPT_START
129 
130  CPose2D robotPose2D;
131  CPose3D robotPose3D;
132 
133  if (robotPose)
134  {
135  robotPose2D = CPose2D(*robotPose);
136  robotPose3D = (*robotPose);
137  }
138  else
139  {
140  // Default values are (0,0,0)
141  }
142 
144  {
145  /********************************************************************
146  OBSERVATION TYPE: CObservationWirelessPower
147  ********************************************************************/
148  const CObservationWirelessPower* o =
149  static_cast<const CObservationWirelessPower*>(obs);
150  float sensorReading;
151 
152  // Compute the 3D sensor pose in world coordinates:
153  CPose2D sensorPose = CPose2D(robotPose3D + o->sensorPoseOnRobot);
154 
155  sensorReading = o->power;
156 
157  // Normalization:
158  sensorReading = (sensorReading - insertionOptions.R_min) /
159  (insertionOptions.R_max - insertionOptions.R_min);
160 
161  // Update the gross estimates of mean/vars for the whole reading history
162  // (see IROS2009 paper):
164  (sensorReading +
168  (square(sensorReading - m_average_normreadings_mean) +
172 
173  // Finally, do the actual map update with that value:
175  sensorReading,
176  mrpt::math::TPoint2D(sensorPose.x(), sensorPose.y()));
177 
178  return true; // Done!
179 
180  } // end if "CObservationWirelessPower"
181 
182  return false;
183 
184  MRPT_END
185 }
186 
187 /*---------------------------------------------------------------
188  computeObservationLikelihood
189  ---------------------------------------------------------------*/
191  const CObservation* obs, const CPose3D& takenFrom)
192 {
193  MRPT_UNUSED_PARAM(obs);
194  MRPT_UNUSED_PARAM(takenFrom);
195 
196  THROW_EXCEPTION("Not implemented yet!");
197 }
198 
202 {
204 
205  // To ensure compatibility: The size of each cell:
206  uint32_t n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
207  out << n;
208 
209  // Save the map contents:
210  n = static_cast<uint32_t>(m_map.size());
211  out << n;
212 
213 // Save the "m_map": This requires special handling for big endian systems:
214 #if MRPT_IS_BIG_ENDIAN
215  for (uint32_t i = 0; i < n; i++)
216  {
217  out << m_map[i].kf_mean << m_map[i].dm_mean << m_map[i].dmv_var_mean;
218  }
219 #else
220  // Little endian: just write all at once:
221  out.WriteBuffer(
222  &m_map[0],
223  sizeof(m_map[0]) * m_map.size()); // TODO: Do this endianness safe!!
224 #endif
225 
226  // Version 1: Save the insertion options:
227  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
228 
229  out << insertionOptions.sigma << insertionOptions.cutoffRadius
230  << insertionOptions.R_min << insertionOptions.R_max
231  << insertionOptions.KF_covSigma << insertionOptions.KF_initialCellStd
232  << insertionOptions.KF_observationModelNoise
233  << insertionOptions.KF_defaultCellMeanValue
234  << insertionOptions.KF_W_size;
235 
236  // New in v3:
239 
240  out << genericMapParams; // v4
241 }
242 
243 // Aux struct used below (must be at global scope for STL):
245 {
246  float mean, std;
247  float w, wr;
248 };
249 
252 {
253  switch (version)
254  {
255  case 0:
256  case 1:
257  case 2:
258  case 3:
259  case 4:
260  case 5:
261  {
262  dyngridcommon_readFromStream(in, version < 5);
263 
264  // To ensure compatibility: The size of each cell:
265  uint32_t n;
266  in >> n;
267 
268  if (version < 2)
269  { // Converter from old versions <=1
270  ASSERT_(
271  n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
272  // Load the map contents in an aux struct:
273  in >> n;
274  vector<TOldCellTypeInVersion1> old_map(n);
275  in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());
276 
277  // Convert to newer format:
278  m_map.resize(n);
279  for (size_t k = 0; k < n; k++)
280  {
281  m_map[k].kf_mean =
282  (old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
283  m_map[k].kf_std =
284  (old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
285  }
286  }
287  else
288  {
290  n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
291  // Load the map contents:
292  in >> n;
293  m_map.resize(n);
294 
295 // Read the note in writeToStream()
296 #if MRPT_IS_BIG_ENDIAN
297  for (uint32_t i = 0; i < n; i++)
298  in >> m_map[i].kf_mean >> m_map[i].dm_mean >>
299  m_map[i].dmv_var_mean;
300 #else
301  // Little endian: just read all at once:
302  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
303 #endif
304  }
305 
306  // Version 1: Insertion options:
307  if (version >= 1)
308  {
309  uint8_t i;
310  in >> i;
312 
313  in >> m_cov >> m_stackedCov;
314 
315  in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >>
316  insertionOptions.R_min >> insertionOptions.R_max >>
317  insertionOptions.KF_covSigma >>
318  insertionOptions.KF_initialCellStd >>
319  insertionOptions.KF_observationModelNoise >>
320  insertionOptions.KF_defaultCellMeanValue >>
321  insertionOptions.KF_W_size;
322  }
323 
324  if (version >= 3)
325  {
326  uint64_t N;
330  }
331 
332  if (version >= 4) in >> genericMapParams;
333 
335  }
336  break;
337  default:
339  };
340 }
341 
344  std::ostream& out) const
345 {
346  out << mrpt::format(
347  "\n----------- [CWirelessPowerGridMap2D::TInsertionOptions] "
348  "------------ \n\n");
349  internal_dumpToTextStream_common(
350  out); // Common params to all random fields maps:
351 
352  out << mrpt::format("\n");
353 }
354 
355 /*---------------------------------------------------------------
356  loadFromConfigFile
357  ---------------------------------------------------------------*/
359  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
360 {
361  // Common data fields for all random fields maps:
362  internal_loadFromConfigFile_common(iniFile, section);
363 }
364 
365 /*---------------------------------------------------------------
366  getAs3DObject
367 ---------------------------------------------------------------*/
369  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
370 {
371  MRPT_START
374  MRPT_END
375 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
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:262
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions insertionOptions
GLenum GLsizei n
Definition: glext.h:5074
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
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:280
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:48
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
void dumpToTextStream_map_specific(std::ostream &out) const override
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
virtual 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.
unsigned char uint8_t
Definition: rptypes.h:41
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:90
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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: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...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
double power
The power or signal strength as sensed by the Wifi receiver (In percentage: [0-100]) ...
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
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.
void dyngridcommon_readFromStream(STREAM &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:351
GLsizei const GLchar ** string
Definition: glext.h:4101
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...
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
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:54
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
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
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:102
virtual void internal_clear() override
Erase all the contents of the map.
#define MRPT_END
Definition: exceptions.h:266
mrpt::maps::CWirelessPowerGridMap2D::TMapRepresentation mapType
The kind of map representation (see CWirelessPowerGridMap2D::CWirelessPowerGridMap2D) ...
GLuint in
Definition: glext.h:7274
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::poses::CPose3D sensorPoseOnRobot
The location of the sensing antenna on the robot coordinate framework.
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
unsigned __int32 uint32_t
Definition: rptypes.h:47
Lightweight 2D point.
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:186
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void internal_clear() override
Erase all the contents of the map.
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020