Main MRPT website > C++ reference for MRPT 1.9.9
CReflectivityGridMap2D.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/poses/CPose2D.h>
15 #include <mrpt/core/round.h> // round()
16 #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 mrpt::math;
26 using namespace mrpt::system;
27 using namespace mrpt::img;
28 using namespace std;
29 
30 // =========== Begin of Map definition ============
32  "CReflectivityGridMap2D,reflectivityMap",
34 
36  : min_x(-10.0f),
37  max_x(10.0f),
38  min_y(-10.0f),
39  max_y(10.0f),
40  resolution(0.10f)
41 {
42 }
43 
46  const std::string& sectionNamePrefix)
47 {
48  // [<sectionNamePrefix>+"_creationOpts"]
49  const std::string sSectCreation =
50  sectionNamePrefix + string("_creationOpts");
51  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
52  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
53  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
54  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
55  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
56 
57  insertionOpts.loadFromConfigFile(
58  source, sectionNamePrefix + string("_insertOpts"));
59 }
60 
62  std::ostream& out) const
63 {
64  LOADABLEOPTS_DUMP_VAR(min_x, double);
65  LOADABLEOPTS_DUMP_VAR(max_x, double);
66  LOADABLEOPTS_DUMP_VAR(min_y, double);
67  LOADABLEOPTS_DUMP_VAR(max_y, double);
68  LOADABLEOPTS_DUMP_VAR(resolution, double);
69 
70  this->insertionOpts.dumpToTextStream(out);
71 }
72 
76 {
78  *dynamic_cast<const CReflectivityGridMap2D::TMapDefinition*>(&_def);
80  def.min_x, def.max_x, def.min_y, def.max_y, def.resolution);
81  obj->insertionOptions = def.insertionOpts;
82  return obj;
83 }
84 // =========== End of Map definition Block =========
85 
87 
88 // Lookup tables for log-odds
91 
92 /*---------------------------------------------------------------
93  Constructor
94  ---------------------------------------------------------------*/
96  double x_min, double x_max, double y_min, double y_max, double resolution)
97  : CDynamicGrid<int8_t>(x_min, x_max, y_min, y_max, resolution),
98  insertionOptions()
99 {
100  internal_clear();
101 }
102 
103 /*---------------------------------------------------------------
104  clear
105  ---------------------------------------------------------------*/
107 /*---------------------------------------------------------------
108  isEmpty
109  ---------------------------------------------------------------*/
110 bool CReflectivityGridMap2D::isEmpty() const { return false; }
111 /*---------------------------------------------------------------
112  insertObservation
113  ---------------------------------------------------------------*/
115  const CObservation* obs, const CPose3D* robotPose)
116 {
117  MRPT_START
118 
119  CPose2D robotPose2D;
120  CPose3D robotPose3D;
121 
122  if (robotPose)
123  {
124  robotPose2D = CPose2D(*robotPose);
125  robotPose3D = (*robotPose);
126  }
127  else
128  {
129  // Default values are (0,0,0)
130  }
131 
133  {
134  /********************************************************************
135  OBSERVATION TYPE: CObservationReflectivity
136  ********************************************************************/
137  const CObservationReflectivity* o =
138  static_cast<const CObservationReflectivity*>(obs);
139 
140  if (o->channel != -1 && insertionOptions.channel != -1 &&
142  {
143  return false; // Incorrect channel
144  }
145 
146  CPose3D sensor_pose;
147  sensor_pose.composeFrom(robotPose3D, o->sensorPose);
148 
149  // log-odd increment due to the observation:
150  const cell_t logodd_observation =
152 
153  // Update cell, with saturation:
154  cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
155  if (!cell)
156  {
157  // We need to resize the grid!
158  const double new_x_min = std::min(m_x_min, sensor_pose.x());
159  const double new_y_min = std::min(m_y_min, sensor_pose.y());
160  const double new_x_max = std::min(m_x_max, sensor_pose.x());
161  const double new_y_max = std::min(m_y_max, sensor_pose.y());
162 
163  const int8_t default_cell = m_logodd_lut.p2l(0.5);
164  resize(
165  new_x_min, new_x_max, new_y_min, new_y_max, default_cell,
166  2.0 /* addit. margin */);
167 
168  // Now we should get the cell:
169  cell = cellByPos(sensor_pose.x(), sensor_pose.y());
170 
171  ASSERTMSG_(
172  cell != nullptr, "cell==nullptr even after resizing grid!?");
173  }
174 
175  const int cell_old = static_cast<int>(*cell);
176  int cell_new = cell_old + static_cast<int>(logodd_observation);
177  keep_min(cell_new, static_cast<int>(CELLTYPE_MAX) - 1);
178  keep_max(cell_new, static_cast<int>(CELLTYPE_MIN) + 1);
179 
180  *cell = static_cast<cell_t>(cell_new);
181 
182  return true; // Done!
183  } // end if "CObservationGasSensors"
184 
185  return false;
186 
187  MRPT_END
188 }
189 
190 /*---------------------------------------------------------------
191  computeObservationLikelihood
192  ---------------------------------------------------------------*/
194  const CObservation* obs, const CPose3D& takenFrom)
195 {
196  MRPT_START
197 
199  {
200  /********************************************************************
201  OBSERVATION TYPE: CObservationReflectivity
202  ********************************************************************/
203  const CObservationReflectivity* o =
204  static_cast<const CObservationReflectivity*>(obs);
205 
206  if (o->channel != -1 && insertionOptions.channel != -1 &&
208  {
209  return 0; // Incorrect channel
210  }
211 
212  CPose3D sensor_pose;
213  sensor_pose.composeFrom(takenFrom, o->sensorPose);
214 
215  cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
216  if (!cell)
217  return 0; // out of the map..
218  else
219  {
222  return -0.5 * square(
223  (m_logodd_lut.l2p(*cell) - o->reflectivityLevel) /
224  o->sensorStdNoise);
225  }
226  }
227  else
228  return 0;
229 
230  MRPT_END
231 }
232 
236 {
238 
239  // Map cells:
240  const uint32_t n = static_cast<uint32_t>(m_map.size());
241  out << n;
242  if (n) out.WriteBuffer(&m_map[0], n);
243 
244  // Save the insertion options
245  out << insertionOptions.channel; // v3
246 
247  out << genericMapParams; // v1
248 }
249 
252 {
253  switch (version)
254  {
255  case 0:
256  case 1:
257  case 2:
258  case 3:
259  {
260  dyngridcommon_readFromStream(in, version < 2);
261 
262  // Map cells:
263  uint32_t n;
264  in >> n;
265  m_map.resize(n);
266  if (n) in.ReadBuffer(&m_map[0], n);
267 
268  // Load the insertion options:
269  if (version >= 3) in >> insertionOptions.channel;
270 
271  if (version >= 1) in >> genericMapParams;
272  }
273  break;
274  default:
276  };
277 }
278 
279 /*---------------------------------------------------------------
280  TInsertionOptions
281  ---------------------------------------------------------------*/
284  std::ostream& out) const
285 {
286  out << mrpt::format(
287  "\n----------- [CReflectivityGridMap2D::TInsertionOptions] "
288  "------------ \n\n");
289 
290  LOADABLEOPTS_DUMP_VAR(channel, int);
291 
292  out << mrpt::format("\n");
293 }
294 
295 /*---------------------------------------------------------------
296  loadFromConfigFile
297  ---------------------------------------------------------------*/
299  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
300 {
302  MRPT_UNUSED_PARAM(section);
303  MRPT_LOAD_CONFIG_VAR(channel, int, iniFile, section);
304 }
305 
306 /*---------------------------------------------------------------
307  saveMetricMapRepresentationToFile
308  ---------------------------------------------------------------*/
310  const std::string& filNamePrefix) const
311 {
312  // Text matrix:
313  saveToTextFile(filNamePrefix + std::string("_probability.txt"));
314 }
315 
316 /*---------------------------------------------------------------
317  getAsImage
318  ---------------------------------------------------------------*/
320  CImage& img, bool verticalFlip, bool forceRGB) const
321 {
322  if (!forceRGB)
323  { // 8bit gray-scale
324  img.resize(m_size_x, m_size_y, 1, true);
325  const cell_t* srcPtr = &m_map[0];
326  unsigned char* destPtr;
327  for (unsigned int y = 0; y < m_size_y; y++)
328  {
329  if (!verticalFlip)
330  destPtr = img(0, m_size_y - 1 - y);
331  else
332  destPtr = img(0, y);
333  for (unsigned int x = 0; x < m_size_x; x++)
334  {
335  *destPtr++ = m_logodd_lut.l2p_255(*srcPtr++);
336  }
337  }
338  }
339  else
340  { // 24bit RGB:
341  img.resize(m_size_x, m_size_y, 3, true);
342  const cell_t* srcPtr = &m_map[0];
343  unsigned char* destPtr;
344  for (unsigned int y = 0; y < m_size_y; y++)
345  {
346  if (!verticalFlip)
347  destPtr = img(0, m_size_y - 1 - y);
348  else
349  destPtr = img(0, y);
350  for (unsigned int x = 0; x < m_size_x; x++)
351  {
352  uint8_t c = m_logodd_lut.l2p_255(*srcPtr++);
353  *destPtr++ = c;
354  *destPtr++ = c;
355  *destPtr++ = c;
356  }
357  }
358  }
359 }
360 
361 /*---------------------------------------------------------------
362  getAs3DObject
363 ---------------------------------------------------------------*/
365  mrpt::opengl::CSetOfObjects::Ptr& outSetOfObj) const
366 {
368 
369  MRPT_START
370 
372  mrpt::make_aligned_shared<opengl::CTexturedPlane>();
373 
374  outObj->setPlaneCorners(m_x_min, m_x_max, m_y_min, m_y_max);
375 
376  // Create the color & transparecy (alpha) images:
377  CImage imgColor(m_size_x, m_size_y, 1);
378  CImage imgTrans(m_size_x, m_size_y, 1);
379 
380  const cell_t* srcPtr = &m_map[0];
381  unsigned char* destPtr_color;
382  unsigned char* destPtr_trans;
383 
384  for (unsigned int y = 0; y < m_size_y; y++)
385  {
386  destPtr_color = imgColor(0, y);
387  destPtr_trans = imgTrans(0, y);
388  for (unsigned int x = 0; x < m_size_x; x++)
389  {
390  uint8_t cell255 = m_logodd_lut.l2p_255(*srcPtr++);
391  *destPtr_color++ = cell255;
392 
393  int8_t auxC = (int8_t)((signed short)cell255) - 128;
394  *destPtr_trans++ = auxC > 0 ? (auxC << 1) : ((-auxC) << 1);
395  }
396  }
397 
398  outObj->assignImage_fast(imgColor, imgTrans);
399  outSetOfObj->insert(outObj);
400 
401  MRPT_END
402 }
403 
405  const mrpt::maps::CMetricMap* otherMap,
406  const mrpt::poses::CPose3D& otherMapPose,
407  const TMatchingRatioParams& params) const
408 {
409  MRPT_UNUSED_PARAM(otherMap);
410  MRPT_UNUSED_PARAM(otherMapPose);
412  return 0;
413 }
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...
Parameters for CMetricMap::compute3DMatchingRatio()
std::vector< int8_t > m_map
The cells.
Definition: CDynamicGrid.h:42
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
See docs in base class: in this class this always returns 0.
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.
#define MRPT_START
Definition: exceptions.h:262
#define min(a, b)
void fill(const int8_t &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:117
CReflectivityGridMap2D(double x_min=-2, double x_max=2, double y_min=-2, double y_max=2, double resolution=0.1)
Constructor.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void dumpToTextStream_map_specific(std::ostream &out) const override
GLenum GLsizei n
Definition: glext.h:5074
float reflectivityLevel
The read reflectivity level, in the range [0,1] (0=black, 1=white).
signed char int8_t
Definition: rptypes.h:40
Declares a class derived from "CObservation" that encapsules a single short-range reflectivity measur...
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:282
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:48
void dyngridcommon_writeToStream(STREAM &out) const
Definition: CDynamicGrid.h:343
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
float sensorStdNoise
1-sigma of the sensor Gaussian noise (in the same normalized units than reflectivityLevel) ...
A 2D grid map representing the reflectivity of the environment (for example, measured with an IR prox...
mrpt::poses::CPose3D sensorPose
The pose of this sensor in robot&#39;s local coordinates.
unsigned char uint8_t
Definition: rptypes.h:41
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#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.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
cell_t p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:565
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
const GLubyte * c
Definition: glext.h:6313
GLint GLvoid * img
Definition: glext.h:3763
int8_t * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or nullptr if it is out of the ...
Definition: CDynamicGrid.h:211
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const int8_t &defaultValueNewCells, double additionalMarginMeters=2.0)
Changes the size of the grid, maintaining previous contents.
Definition: CDynamicGrid.h:127
This namespace contains representation of robot actions and observations.
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.
string iniFile(myDataDir+string("benchmark-options.ini"))
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
void dyngridcommon_readFromStream(STREAM &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:351
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#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...
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
mrpt::maps::CReflectivityGridMap2D::TInsertionOptions insertionOptions
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.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
uint8_t l2p_255(const cell_t l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void internal_clear() override
Internal method called by clear()
#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:103
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:323
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
int16_t channel
The reflectivity channel for this map.
int16_t channel
The channel for this observation.
GLenum GLint GLint y
Definition: glext.h:3538
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:177
GLenum GLint x
Definition: glext.h:3538
static CLogOddsGridMapLUT< cell_t > m_logodd_lut
Lookup tables for log-odds.
mrpt::maps::CReflectivityGridMap2D::TInsertionOptions insertionOpts
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
void getAsImage(mrpt::img::CImage &img, bool verticalFlip=false, bool forceRGB=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0...
unsigned __int32 uint32_t
Definition: rptypes.h:47
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
GLenum const GLfloat * params
Definition: glext.h:3534
double min_x
See CReflectivityGridMap2DOptions::CReflectivityGridMap2DOptions.
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
#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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019