MRPT  1.9.9
CReflectivityGridMap2D.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> // round()
17 #include <mrpt/poses/CPose2D.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 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 
37  = default;
38 
41  const std::string& sectionNamePrefix)
42 {
43  // [<sectionNamePrefix>+"_creationOpts"]
44  const std::string sSectCreation =
45  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 
52  insertionOpts.loadFromConfigFile(
53  source, sectionNamePrefix + string("_insertOpts"));
54 }
55 
57  std::ostream& out) const
58 {
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 
71 {
73  *dynamic_cast<const CReflectivityGridMap2D::TMapDefinition*>(&_def);
74  auto* obj = new CReflectivityGridMap2D(
75  def.min_x, def.max_x, def.min_y, def.max_y, def.resolution);
76  obj->insertionOptions = def.insertionOpts;
77  return obj;
78 }
79 // =========== End of Map definition Block =========
80 
82 
83 // Lookup tables for log-odds
86 
87 /*---------------------------------------------------------------
88  Constructor
89  ---------------------------------------------------------------*/
91  double x_min, double x_max, double y_min, double y_max, double resolution)
92  : CDynamicGrid<int8_t>(x_min, x_max, y_min, y_max, resolution),
93  insertionOptions()
94 {
96 }
97 
98 /*---------------------------------------------------------------
99  clear
100  ---------------------------------------------------------------*/
102 /*---------------------------------------------------------------
103  isEmpty
104  ---------------------------------------------------------------*/
105 bool CReflectivityGridMap2D::isEmpty() const { return false; }
106 /*---------------------------------------------------------------
107  insertObservation
108  ---------------------------------------------------------------*/
110  const CObservation* obs, const CPose3D* robotPose)
111 {
112  MRPT_START
113 
114  CPose2D robotPose2D;
115  CPose3D robotPose3D;
116 
117  if (robotPose)
118  {
119  robotPose2D = CPose2D(*robotPose);
120  robotPose3D = (*robotPose);
121  }
122  else
123  {
124  // Default values are (0,0,0)
125  }
126 
128  {
129  /********************************************************************
130  OBSERVATION TYPE: CObservationReflectivity
131  ********************************************************************/
132  const auto* o = static_cast<const CObservationReflectivity*>(obs);
133 
134  if (o->channel != -1 && insertionOptions.channel != -1 &&
135  o->channel != insertionOptions.channel)
136  {
137  return false; // Incorrect channel
138  }
139 
140  CPose3D sensor_pose;
141  sensor_pose.composeFrom(robotPose3D, o->sensorPose);
142 
143  // log-odd increment due to the observation:
144  const cell_t logodd_observation =
145  m_logodd_lut.p2l(o->reflectivityLevel);
146 
147  // Update cell, with saturation:
148  cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
149  if (!cell)
150  {
151  // We need to resize the grid!
152  const double new_x_min = std::min(m_x_min, sensor_pose.x());
153  const double new_y_min = std::min(m_y_min, sensor_pose.y());
154  const double new_x_max = std::min(m_x_max, sensor_pose.x());
155  const double new_y_max = std::min(m_y_max, sensor_pose.y());
156 
157  const int8_t default_cell = m_logodd_lut.p2l(0.5);
158  resize(
159  new_x_min, new_x_max, new_y_min, new_y_max, default_cell,
160  2.0 /* addit. margin */);
161 
162  // Now we should get the cell:
163  cell = cellByPos(sensor_pose.x(), sensor_pose.y());
164 
165  ASSERTMSG_(
166  cell != nullptr, "cell==nullptr even after resizing grid!?");
167  }
168 
169  const int cell_old = static_cast<int>(*cell);
170  int cell_new = cell_old + static_cast<int>(logodd_observation);
171  keep_min(cell_new, static_cast<int>(CELLTYPE_MAX) - 1);
172  keep_max(cell_new, static_cast<int>(CELLTYPE_MIN) + 1);
173 
174  *cell = static_cast<cell_t>(cell_new);
175 
176  return true; // Done!
177  } // end if "CObservationGasSensors"
178 
179  return false;
180 
181  MRPT_END
182 }
183 
184 /*---------------------------------------------------------------
185  computeObservationLikelihood
186  ---------------------------------------------------------------*/
188  const CObservation* obs, const CPose3D& takenFrom)
189 {
190  MRPT_START
191 
193  {
194  /********************************************************************
195  OBSERVATION TYPE: CObservationReflectivity
196  ********************************************************************/
197  const auto* o = static_cast<const CObservationReflectivity*>(obs);
198 
199  if (o->channel != -1 && insertionOptions.channel != -1 &&
200  o->channel != insertionOptions.channel)
201  {
202  return 0; // Incorrect channel
203  }
204 
205  CPose3D sensor_pose;
206  sensor_pose.composeFrom(takenFrom, o->sensorPose);
207 
208  cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
209  if (!cell)
210  return 0; // out of the map..
211  else
212  {
213  ASSERT_ABOVEEQ_(o->reflectivityLevel, 0);
214  ASSERT_BELOWEQ_(o->reflectivityLevel, 1);
215  return -0.5 * square(
216  (m_logodd_lut.l2p(*cell) - o->reflectivityLevel) /
217  o->sensorStdNoise);
218  }
219  }
220  else
221  return 0;
222 
223  MRPT_END
224 }
225 
229 {
231 
232  // Map cells:
233  const auto n = static_cast<uint32_t>(m_map.size());
234  out << n;
235  if (n) out.WriteBuffer(&m_map[0], n);
236 
237  // Save the insertion options
238  out << insertionOptions.channel; // v3
239 
240  out << genericMapParams; // v1
241 }
242 
245 {
246  switch (version)
247  {
248  case 0:
249  case 1:
250  case 2:
251  case 3:
252  {
253  dyngridcommon_readFromStream(in, version < 2);
254 
255  // Map cells:
256  uint32_t n;
257  in >> n;
258  m_map.resize(n);
259  if (n) in.ReadBuffer(&m_map[0], n);
260 
261  // Load the insertion options:
262  if (version >= 3) in >> insertionOptions.channel;
263 
264  if (version >= 1) in >> genericMapParams;
265  }
266  break;
267  default:
269  };
270 }
271 
272 /*---------------------------------------------------------------
273  TInsertionOptions
274  ---------------------------------------------------------------*/
277  std::ostream& out) const
278 {
279  out << mrpt::format(
280  "\n----------- [CReflectivityGridMap2D::TInsertionOptions] "
281  "------------ \n\n");
282 
284 
285  out << mrpt::format("\n");
286 }
287 
288 /*---------------------------------------------------------------
289  loadFromConfigFile
290  ---------------------------------------------------------------*/
292  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
293 {
295  MRPT_UNUSED_PARAM(section);
296  MRPT_LOAD_CONFIG_VAR(channel, int, iniFile, section);
297 }
298 
299 /*---------------------------------------------------------------
300  saveMetricMapRepresentationToFile
301  ---------------------------------------------------------------*/
303  const std::string& filNamePrefix) const
304 {
305  // Text matrix:
306  saveToTextFile(filNamePrefix + std::string("_probability.txt"));
307 }
308 
309 /*---------------------------------------------------------------
310  getAsImage
311  ---------------------------------------------------------------*/
313  CImage& img, bool verticalFlip, bool forceRGB) const
314 {
315  if (!forceRGB)
316  { // 8bit gray-scale
317  img.resize(m_size_x, m_size_y, CH_GRAY);
318  const cell_t* srcPtr = &m_map[0];
319  unsigned char* destPtr;
320  for (unsigned int y = 0; y < m_size_y; y++)
321  {
322  if (!verticalFlip)
323  destPtr = img(0, m_size_y - 1 - y);
324  else
325  destPtr = img(0, y);
326  for (unsigned int x = 0; x < m_size_x; x++)
327  {
328  *destPtr++ = m_logodd_lut.l2p_255(*srcPtr++);
329  }
330  }
331  }
332  else
333  { // 24bit RGB:
334  img.resize(m_size_x, m_size_y, CH_RGB);
335  const cell_t* srcPtr = &m_map[0];
336  unsigned char* destPtr;
337  for (unsigned int y = 0; y < m_size_y; y++)
338  {
339  if (!verticalFlip)
340  destPtr = img(0, m_size_y - 1 - y);
341  else
342  destPtr = img(0, y);
343  for (unsigned int x = 0; x < m_size_x; x++)
344  {
345  uint8_t c = m_logodd_lut.l2p_255(*srcPtr++);
346  *destPtr++ = c;
347  *destPtr++ = c;
348  *destPtr++ = c;
349  }
350  }
351  }
352 }
353 
354 /*---------------------------------------------------------------
355  getAs3DObject
356 ---------------------------------------------------------------*/
358  mrpt::opengl::CSetOfObjects::Ptr& outSetOfObj) const
359 {
361 
362  MRPT_START
363 
365  mrpt::make_aligned_shared<opengl::CTexturedPlane>();
366 
367  outObj->setPlaneCorners(m_x_min, m_x_max, m_y_min, m_y_max);
368 
369  // Create the color & transparecy (alpha) images:
370  CImage imgColor(m_size_x, m_size_y, CH_GRAY);
371  CImage imgTrans(m_size_x, m_size_y, CH_GRAY);
372 
373  const cell_t* srcPtr = &m_map[0];
374  unsigned char* destPtr_color;
375  unsigned char* destPtr_trans;
376 
377  for (unsigned int y = 0; y < m_size_y; y++)
378  {
379  destPtr_color = imgColor(0, y);
380  destPtr_trans = imgTrans(0, y);
381  for (unsigned int x = 0; x < m_size_x; x++)
382  {
383  uint8_t cell255 = m_logodd_lut.l2p_255(*srcPtr++);
384  *destPtr_color++ = cell255;
385 
386  int8_t auxC = (int8_t)((signed short)cell255) - 128;
387  *destPtr_trans++ = auxC > 0 ? (auxC << 1) : ((-auxC) << 1);
388  }
389  }
390 
391  outObj->assignImage_fast(imgColor, imgTrans);
392  outSetOfObj->insert(outObj);
393 
394  MRPT_END
395 }
396 
398  const mrpt::maps::CMetricMap* otherMap,
399  const mrpt::poses::CPose3D& otherMapPose,
400  const TMatchingRatioParams& params) const
401 {
402  MRPT_UNUSED_PARAM(otherMap);
403  MRPT_UNUSED_PARAM(otherMapPose);
405  return 0;
406 }
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:282
#define min(a, b)
void fill(const int8_t &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:109
CReflectivityGridMap2D(double x_min=-2, double x_max=2, double y_min=-2, double y_max=2, double resolution=0.1)
Constructor.
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:5136
signed char int8_t
Definition: rptypes.h:43
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:280
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:49
void dyngridcommon_writeToStream(STREAM &out) const
Definition: CDynamicGrid.h:334
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:4085
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
A 2D grid map representing the reflectivity of the environment (for example, measured with an IR prox...
unsigned char uint8_t
Definition: rptypes.h:44
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:138
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:558
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:6406
GLint GLvoid * img
Definition: glext.h:3769
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:201
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:117
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"))
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:149
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:208
void dyngridcommon_readFromStream(STREAM &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:342
GLsizei const GLchar ** string
Definition: glext.h:4116
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:52
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:53
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:82
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:42
void internal_clear() override
Internal method called by clear()
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
#define MRPT_END
Definition: exceptions.h:286
GLuint in
Definition: glext.h:7391
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:313
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
int16_t channel
The reflectivity channel for this map.
GLenum GLint GLint y
Definition: glext.h:3542
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:202
GLenum GLint x
Definition: glext.h:3542
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:50
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
GLenum const GLfloat * params
Definition: glext.h:3538
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:147
#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: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019