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  "CWirelessPowerGridMap2D,wifiGrid", mrpt::maps::CWirelessPowerGridMap2D)
30 
32 
33  = default;
34 
37  const std::string& sectionNamePrefix)
38 {
39  // [<sectionNamePrefix>+"_creationOpts"]
40  const std::string sSectCreation =
41  sectionNamePrefix + string("_creationOpts");
42  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
43  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
44  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
45  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
46  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
48  sSectCreation, "mapType", mapType);
49 
50  insertionOpts.loadFromConfigFile(
51  source, sectionNamePrefix + string("_insertOpts"));
52 }
53 
55  std::ostream& out) const
56 {
57  out << mrpt::format(
58  "MAP TYPE = %s\n",
61  .c_str());
62  LOADABLEOPTS_DUMP_VAR(min_x, double);
63  LOADABLEOPTS_DUMP_VAR(max_x, double);
64  LOADABLEOPTS_DUMP_VAR(min_y, double);
65  LOADABLEOPTS_DUMP_VAR(max_y, double);
66  LOADABLEOPTS_DUMP_VAR(resolution, double);
67 
68  this->insertionOpts.dumpToTextStream(out);
69 }
70 
74 {
76  *dynamic_cast<const CWirelessPowerGridMap2D::TMapDefinition*>(&_def);
77  auto* obj = new CWirelessPowerGridMap2D(
78  def.mapType, def.min_x, def.max_x, def.min_y, def.max_y,
79  def.resolution);
80  obj->insertionOptions = def.insertionOpts;
81  return obj;
82 }
83 // =========== End of Map definition Block =========
84 
87 
88 /*---------------------------------------------------------------
89  Constructor
90  ---------------------------------------------------------------*/
92  TMapRepresentation mapType, double x_min, double x_max, double y_min,
93  double y_max, double resolution)
94  : CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution),
95  insertionOptions()
96 {
97  // Set the grid to initial values (and adjusts the KF covariance matrix!)
98  // Also, calling clear() is mandatory to end initialization of our base
99  // class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
101 }
102 
103 CWirelessPowerGridMap2D::~CWirelessPowerGridMap2D() = default;
104 /*---------------------------------------------------------------
105  clear
106  ---------------------------------------------------------------*/
107 void CWirelessPowerGridMap2D::internal_clear()
108 {
109  // Just do the generic clear:
111 
112  // Anything else special for this derived class?
113  // ...
114 }
115 
116 /*---------------------------------------------------------------
117  insertObservation
118  ---------------------------------------------------------------*/
120  const CObservation& obs, 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 auto& o = static_cast<const CObservationWirelessPower&>(obs);
143  float sensorReading;
144 
145  // Compute the 3D sensor pose in world coordinates:
146  CPose2D sensorPose = CPose2D(robotPose3D + o.sensorPoseOnRobot);
147 
148  sensorReading = o.power;
149 
150  // Normalization:
151  sensorReading = (sensorReading - insertionOptions.R_min) /
152  (insertionOptions.R_max - insertionOptions.R_min);
153 
154  // Update the gross estimates of mean/vars for the whole reading history
155  // (see IROS2009 paper):
157  (sensorReading +
161  (square(sensorReading - m_average_normreadings_mean) +
165 
166  // Finally, do the actual map update with that value:
168  sensorReading,
169  mrpt::math::TPoint2D(sensorPose.x(), sensorPose.y()));
170 
171  return true; // Done!
172 
173  } // end if "CObservationWirelessPower"
174 
175  return false;
176 
177  MRPT_END
178 }
179 
180 /*---------------------------------------------------------------
181  computeObservationLikelihood
182  ---------------------------------------------------------------*/
184  const CObservation& obs, const CPose3D& takenFrom)
185 {
186  MRPT_UNUSED_PARAM(obs);
187  MRPT_UNUSED_PARAM(takenFrom);
188 
189  THROW_EXCEPTION("Not implemented yet!");
190 }
191 
195 {
197 
198  // To ensure compatibility: The size of each cell:
199  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
200  out << n;
201 
202  // Save the map contents:
203  n = static_cast<uint32_t>(m_map.size());
204  out << n;
205 
206 // Save the "m_map": This requires special handling for big endian systems:
207 #if MRPT_IS_BIG_ENDIAN
208  for (uint32_t i = 0; i < n; i++)
209  {
210  out << m_map[i].kf_mean << m_map[i].dm_mean << m_map[i].dmv_var_mean;
211  }
212 #else
213  // Little endian: just write all at once:
214  out.WriteBuffer(
215  &m_map[0],
216  sizeof(m_map[0]) * m_map.size()); // TODO: Do this endianness safe!!
217 #endif
218 
219  // Version 1: Save the insertion options:
220  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
221 
222  out << insertionOptions.sigma << insertionOptions.cutoffRadius
223  << insertionOptions.R_min << insertionOptions.R_max
224  << insertionOptions.KF_covSigma << insertionOptions.KF_initialCellStd
225  << insertionOptions.KF_observationModelNoise
226  << insertionOptions.KF_defaultCellMeanValue
227  << insertionOptions.KF_W_size;
228 
229  // New in v3:
232 
233  out << genericMapParams; // v4
234 }
235 
236 // Aux struct used below (must be at global scope for STL):
238 {
239  float mean, std;
240  float w, wr;
241 };
242 
245 {
246  switch (version)
247  {
248  case 0:
249  case 1:
250  case 2:
251  case 3:
252  case 4:
253  case 5:
254  {
255  dyngridcommon_readFromStream(in, version < 5);
256 
257  // To ensure compatibility: The size of each cell:
258  uint32_t n;
259  in >> n;
260 
261  if (version < 2)
262  { // Converter from old versions <=1
263  ASSERT_(
264  n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
265  // Load the map contents in an aux struct:
266  in >> n;
267  vector<TOldCellTypeInVersion1> old_map(n);
268  in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());
269 
270  // Convert to newer format:
271  m_map.resize(n);
272  for (size_t k = 0; k < n; k++)
273  {
274  m_map[k].kf_mean =
275  (old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
276  m_map[k].kf_std =
277  (old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
278  }
279  }
280  else
281  {
283  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 >>
292  m_map[i].dmv_var_mean;
293 #else
294  // Little endian: just read all at once:
295  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
296 #endif
297  }
298 
299  // Version 1: Insertion options:
300  if (version >= 1)
301  {
302  uint8_t i;
303  in >> i;
305 
306  in >> m_cov >> m_stackedCov;
307 
308  in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >>
309  insertionOptions.R_min >> insertionOptions.R_max >>
310  insertionOptions.KF_covSigma >>
311  insertionOptions.KF_initialCellStd >>
312  insertionOptions.KF_observationModelNoise >>
313  insertionOptions.KF_defaultCellMeanValue >>
314  insertionOptions.KF_W_size;
315  }
316 
317  if (version >= 3)
318  {
319  uint64_t N;
323  }
324 
325  if (version >= 4) in >> genericMapParams;
326 
328  }
329  break;
330  default:
332  };
333 }
334 
337  std::ostream& out) const
338 {
339  out << mrpt::format(
340  "\n----------- [CWirelessPowerGridMap2D::TInsertionOptions] "
341  "------------ \n\n");
342  internal_dumpToTextStream_common(
343  out); // Common params to all random fields maps:
344 
345  out << mrpt::format("\n");
346 }
347 
348 /*---------------------------------------------------------------
349  loadFromConfigFile
350  ---------------------------------------------------------------*/
352  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
353 {
354  // Common data fields for all random fields maps:
355  internal_loadFromConfigFile_common(iniFile, section);
356 }
357 
358 /*---------------------------------------------------------------
359  getAs3DObject
360 ---------------------------------------------------------------*/
362  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
363 {
364  MRPT_START
367  MRPT_END
368 }
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
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
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:49
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.
unsigned char uint8_t
Definition: rptypes.h:44
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
T square(const T x)
Inline function for the square of a number.
#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:133
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...
unsigned __int64 uint64_t
Definition: rptypes.h:53
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:53
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:84
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
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()
unsigned __int32 uint32_t
Definition: rptypes.h:50
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:182
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: c26d46ba6 Thu Jul 18 13:03:42 2019 +0200 at jue jul 18 13:10:17 CEST 2019