Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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/utils/round.h>
16 #include <mrpt/utils/CTicTac.h>
17 #include <mrpt/utils/color_maps.h>
19 #include <mrpt/utils/CStream.h>
20 
21 using namespace mrpt;
22 using namespace mrpt::maps;
23 using namespace mrpt::obs;
24 using namespace mrpt::utils;
25 using namespace mrpt::poses;
26 using namespace std;
27 
28 
29 // =========== Begin of Map definition ============
31 
33  min_x(-2),
34  max_x(2),
35  min_y(-2),
36  max_y(2),
37  resolution(0.10f),
38  mapType(CWirelessPowerGridMap2D::mrKernelDM)
39 {
40 }
41 
43 {
44  // [<sectionNamePrefix>+"_creationOpts"]
45  const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
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);
51  mapType = source.read_enum<CWirelessPowerGridMap2D::TMapRepresentation>(sSectCreation,"mapType",mapType);
52 
53  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
54 }
55 
57 {
59  LOADABLEOPTS_DUMP_VAR(min_x , double );
60  LOADABLEOPTS_DUMP_VAR(max_x , double );
61  LOADABLEOPTS_DUMP_VAR(min_y , double );
62  LOADABLEOPTS_DUMP_VAR(max_y , double );
63  LOADABLEOPTS_DUMP_VAR(resolution , double );
64 
65  this->insertionOpts.dumpToTextStream(out);
66 }
67 
69 {
73  return obj;
74 }
75 // =========== End of Map definition Block =========
76 
77 
79 
80 /*---------------------------------------------------------------
81  Constructor
82  ---------------------------------------------------------------*/
84  TMapRepresentation mapType,
85  double x_min,
86  double x_max,
87  double y_min,
88  double y_max,
89  double resolution ) :
90  CRandomFieldGridMap2D(mapType, x_min,x_max,y_min,y_max,resolution ),
91  insertionOptions()
92 {
93  // Set the grid to initial values (and adjusts the KF covariance matrix!)
94  // Also, calling clear() is mandatory to end initialization of our base class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
96 }
97 
99 {
100 }
101 
102 /*---------------------------------------------------------------
103  clear
104  ---------------------------------------------------------------*/
106 {
107  // Just do the generic clear:
109 
110  // Anything else special for this derived class?
111  // ...
112 }
113 
114 
115 /*---------------------------------------------------------------
116  insertObservation
117  ---------------------------------------------------------------*/
119  const CObservation *obs,
120  const CPose3D *robotPose )
121 {
122  MRPT_START
123 
124  CPose2D robotPose2D;
125  CPose3D robotPose3D;
126 
127  if (robotPose)
128  {
129  robotPose2D = CPose2D(*robotPose);
130  robotPose3D = (*robotPose);
131  }
132  else
133  {
134  // Default values are (0,0,0)
135  }
136 
138  {
139  /********************************************************************
140  OBSERVATION TYPE: CObservationWirelessPower
141  ********************************************************************/
142  const CObservationWirelessPower *o = static_cast<const CObservationWirelessPower*>( obs );
143  float sensorReading;
144 
145 
146  // Compute the 3D sensor pose in world coordinates:
147  CPose2D sensorPose = CPose2D(robotPose3D + o->sensorPoseOnRobot );
148 
149  sensorReading = o->power;
150 
151  // Normalization:
152  sensorReading = (sensorReading - insertionOptions.R_min) /( insertionOptions.R_max - insertionOptions.R_min );
153 
154  // Update the gross estimates of mean/vars for the whole reading history (see IROS2009 paper):
155  m_average_normreadings_mean = (sensorReading + m_average_normreadings_count*m_average_normreadings_mean)/(1+m_average_normreadings_count);
156  m_average_normreadings_var = (square(sensorReading - m_average_normreadings_mean) + m_average_normreadings_count*m_average_normreadings_var) /(1+m_average_normreadings_count);
157  m_average_normreadings_count++;
158 
159  // Finally, do the actual map update with that value:
160  this->insertIndividualReading(sensorReading, mrpt::math::TPoint2D(sensorPose.x(),sensorPose.y()) );
161 
162  return true; // Done!
163 
164  } // end if "CObservationWirelessPower"
165 
166  return false;
167 
168  MRPT_END
169 }
170 
171 
172 /*---------------------------------------------------------------
173  computeObservationLikelihood
174  ---------------------------------------------------------------*/
176  const CObservation *obs,
177  const CPose3D &takenFrom )
178 {
179  MRPT_UNUSED_PARAM(obs);MRPT_UNUSED_PARAM(takenFrom);
180 
181  THROW_EXCEPTION("Not implemented yet!");
182 }
183 
184 /*---------------------------------------------------------------
185  Implements the writing to a CStream capability of CSerializable objects
186  ---------------------------------------------------------------*/
188 {
189  if (version)
190  *version = 5;
191  else
192  {
193  dyngridcommon_writeToStream(out);
194 
195  // To ensure compatibility: The size of each cell:
196  uint32_t 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 << m_map[i].dmv_var_mean;
208  }
209 #else
210  // Little endian: just write all at once:
211  out.WriteBuffer( &m_map[0], sizeof(m_map[0])*m_map.size() ); // TODO: Do this endianness safe!!
212 #endif
213 
214 
215  // Version 1: Save the insertion options:
216  out << uint8_t(m_mapType)
217  << m_cov
218  << m_stackedCov;
219 
220  out << insertionOptions.sigma
221  << insertionOptions.cutoffRadius
222  << insertionOptions.R_min
223  << insertionOptions.R_max
224  << insertionOptions.KF_covSigma
225  << insertionOptions.KF_initialCellStd
226  << insertionOptions.KF_observationModelNoise
227  << insertionOptions.KF_defaultCellMeanValue
228  << insertionOptions.KF_W_size;
229 
230  // New in v3:
231  out << m_average_normreadings_mean << m_average_normreadings_var << uint64_t(m_average_normreadings_count);
232 
233  out << genericMapParams; // v4
234 
235  }
236 }
237 
238 // Aux struct used below (must be at global scope for STL):
240 {
241  float mean, std;
242  float w,wr;
243 };
244 
245 /*---------------------------------------------------------------
246  Implements the reading from a CStream capability of CSerializable objects
247  ---------------------------------------------------------------*/
249 {
250  switch(version)
251  {
252  case 0:
253  case 1:
254  case 2:
255  case 3:
256  case 4:
257  case 5:
258  {
259  dyngridcommon_readFromStream(in, version<5);
260 
261  // To ensure compatibility: The size of each cell:
262  uint32_t n;
263  in >> n;
264 
265  if (version<2)
266  { // Converter from old versions <=1
267  ASSERT_( n == static_cast<uint32_t>( sizeof( TOldCellTypeInVersion1 ) ));
268  // Load the map contents in an aux struct:
269  in >> n;
270  vector<TOldCellTypeInVersion1> old_map(n);
271  in.ReadBuffer( &old_map[0], sizeof(old_map[0])*old_map.size() );
272 
273  // Convert to newer format:
274  m_map.resize(n);
275  for (size_t k=0;k<n;k++)
276  {
277  m_map[k].kf_mean = (old_map[k].w!=0) ? old_map[k].wr : old_map[k].mean;
278  m_map[k].kf_std = (old_map[k].w!=0) ? old_map[k].w : old_map[k].std;
279  }
280  }
281  else
282  {
283  ASSERT_EQUAL_( n , static_cast<uint32_t>( sizeof( TRandomFieldCell ) ));
284  // Load the map contents:
285  in >> n;
286  m_map.resize(n);
287 
288  // Read the note in writeToStream()
289 #if MRPT_IS_BIG_ENDIAN
290  for (uint32_t i=0;i<n;i++)
291  in >> m_map[i].kf_mean >> m_map[i].dm_mean >> 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;
303  m_mapType = TMapRepresentation(i);
304 
305  in >> m_cov
306  >> m_stackedCov;
307 
308  in >> insertionOptions.sigma
309  >> insertionOptions.cutoffRadius
310  >> insertionOptions.R_min
311  >> insertionOptions.R_max
312  >> insertionOptions.KF_covSigma
313  >> insertionOptions.KF_initialCellStd
314  >> insertionOptions.KF_observationModelNoise
315  >> insertionOptions.KF_defaultCellMeanValue
316  >> insertionOptions.KF_W_size;
317  }
318 
319  if (version>=3)
320  {
321  uint64_t N;
322  in >> m_average_normreadings_mean >> m_average_normreadings_var >> N;
323  m_average_normreadings_count = N;
324  }
325 
326  if (version>=4)
327  in >> genericMapParams;
328 
329  m_hasToRecoverMeanAndCov = true;
330  } break;
331  default:
333 
334  };
335 
336 }
337 
338 /*---------------------------------------------------------------
339  TInsertionOptions
340  ---------------------------------------------------------------*/
342 {
343 }
344 
345 /*---------------------------------------------------------------
346  dumpToTextStream
347  ---------------------------------------------------------------*/
349 {
350  out.printf("\n----------- [CWirelessPowerGridMap2D::TInsertionOptions] ------------ \n\n");
351  internal_dumpToTextStream_common(out); // Common params to all random fields maps:
352 
353  out.printf("\n");
354 }
355 
356 /*---------------------------------------------------------------
357  loadFromConfigFile
358  ---------------------------------------------------------------*/
360  const mrpt::utils::CConfigFileBase &iniFile,
361  const std::string &section)
362 {
363  // Common data fields for all random fields maps:
364  internal_loadFromConfigFile_common(iniFile,section);
365 }
366 
367 /*---------------------------------------------------------------
368  getAs3DObject
369 ---------------------------------------------------------------*/
370 void CWirelessPowerGridMap2D::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
371 {
372  MRPT_START
373  if (!genericMapParams.enableSaveAs3DObject) return;
375  MRPT_END
376 }
#define ASSERT_EQUAL_(__A, __B)
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer On any error, or if ZERO bytes are read...
Definition: CStream.cpp:45
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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...
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions insertionOptions
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
#define THROW_EXCEPTION(msg)
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
GLuint in
Definition: glew.h:7146
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
unsigned char uint8_t
Definition: rptypes.h:43
CWirelessPowerGridMap2D represents a PDF of wifi concentrations over a 2D area.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLsizei n
Definition: glew.h:5051
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
The contents of each cell in a CRandomFieldGridMap2D map.
A helper class that can convert an enum value into its textual representation, and viceversa...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
double power
The power or signal strength as sensed by the Wifi receiver (In percentage: [0-100]) ...
GLhandleARB obj
Definition: glew.h:3276
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map (mean)
int version
Definition: mrpt_jpeglib.h:898
double resolution
See CWirelessPowerGridMap2D::CWirelessPowerGridMap2D.
This represents a measurement of the wireless strength perceived by the robot.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions insertionOpts
#define MRPT_START
unsigned __int64 uint64_t
Definition: rptypes.h:52
#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...
#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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot's observation.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
mrpt::maps::CWirelessPowerGridMap2D::TMapRepresentation mapType
The kind of map representation (see CWirelessPowerGridMap2D::CWirelessPowerGridMap2D) ...
#define ASSERT_(f)
mrpt::poses::CPose3D sensorPoseOnRobot
The location of the sensing antenna on the robot coordinate framework.
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
Load all map-specific params.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
unsigned __int32 uint32_t
Definition: rptypes.h:49
Lightweight 2D point.
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Internal method called by insertObservation()
void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
virtual void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018