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 = m_logodd_lut.p2l(o.reflectivityLevel);
145 
146  // Update cell, with saturation:
147  cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
148  if (!cell)
149  {
150  // We need to resize the grid!
151  const double new_x_min = std::min(m_x_min, sensor_pose.x());
152  const double new_y_min = std::min(m_y_min, sensor_pose.y());
153  const double new_x_max = std::min(m_x_max, sensor_pose.x());
154  const double new_y_max = std::min(m_y_max, sensor_pose.y());
155 
156  const int8_t default_cell = m_logodd_lut.p2l(0.5);
157  resize(
158  new_x_min, new_x_max, new_y_min, new_y_max, default_cell,
159  2.0 /* addit. margin */);
160 
161  // Now we should get the cell:
162  cell = cellByPos(sensor_pose.x(), sensor_pose.y());
163 
164  ASSERTMSG_(
165  cell != nullptr, "cell==nullptr even after resizing grid!?");
166  }
167 
168  const int cell_old = static_cast<int>(*cell);
169  int cell_new = cell_old + static_cast<int>(logodd_observation);
170  keep_min(cell_new, static_cast<int>(CELLTYPE_MAX) - 1);
171  keep_max(cell_new, static_cast<int>(CELLTYPE_MIN) + 1);
172 
173  *cell = static_cast<cell_t>(cell_new);
174 
175  return true; // Done!
176  } // end if "CObservationGasSensors"
177 
178  return false;
179 
180  MRPT_END
181 }
182 
183 /*---------------------------------------------------------------
184  computeObservationLikelihood
185  ---------------------------------------------------------------*/
187  const CObservation& obs, const CPose3D& takenFrom)
188 {
189  MRPT_START
190 
192  {
193  /********************************************************************
194  OBSERVATION TYPE: CObservationReflectivity
195  ********************************************************************/
196  const auto& o = static_cast<const CObservationReflectivity&>(obs);
197 
198  if (o.channel != -1 && insertionOptions.channel != -1 &&
199  o.channel != insertionOptions.channel)
200  {
201  return 0; // Incorrect channel
202  }
203 
204  CPose3D sensor_pose;
205  sensor_pose.composeFrom(takenFrom, o.sensorPose);
206 
207  cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
208  if (!cell)
209  return 0; // out of the map..
210  else
211  {
212  ASSERT_ABOVEEQ_(o.reflectivityLevel, 0);
213  ASSERT_BELOWEQ_(o.reflectivityLevel, 1);
214  return -0.5 * square(
215  (m_logodd_lut.l2p(*cell) - o.reflectivityLevel) /
216  o.sensorStdNoise);
217  }
218  }
219  else
220  return 0;
221 
222  MRPT_END
223 }
224 
228 {
230 
231  // Map cells:
232  const auto n = static_cast<uint32_t>(m_map.size());
233  out << n;
234  if (n) out.WriteBuffer(&m_map[0], n);
235 
236  // Save the insertion options
237  out << insertionOptions.channel; // v3
238 
239  out << genericMapParams; // v1
240 }
241 
244 {
245  switch (version)
246  {
247  case 0:
248  case 1:
249  case 2:
250  case 3:
251  {
252  dyngridcommon_readFromStream(in, version < 2);
253 
254  // Map cells:
255  uint32_t n;
256  in >> n;
257  m_map.resize(n);
258  if (n) in.ReadBuffer(&m_map[0], n);
259 
260  // Load the insertion options:
261  if (version >= 3) in >> insertionOptions.channel;
262 
263  if (version >= 1) in >> genericMapParams;
264  }
265  break;
266  default:
268  };
269 }
270 
271 /*---------------------------------------------------------------
272  TInsertionOptions
273  ---------------------------------------------------------------*/
276  std::ostream& out) const
277 {
278  out << mrpt::format(
279  "\n----------- [CReflectivityGridMap2D::TInsertionOptions] "
280  "------------ \n\n");
281 
283 
284  out << mrpt::format("\n");
285 }
286 
287 /*---------------------------------------------------------------
288  loadFromConfigFile
289  ---------------------------------------------------------------*/
291  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
292 {
294  MRPT_UNUSED_PARAM(section);
295  MRPT_LOAD_CONFIG_VAR(channel, int, iniFile, section);
296 }
297 
298 /*---------------------------------------------------------------
299  saveMetricMapRepresentationToFile
300  ---------------------------------------------------------------*/
302  const std::string& filNamePrefix) const
303 {
304  // Text matrix:
305  saveToTextFile(filNamePrefix + std::string("_probability.txt"));
306 }
307 
308 /*---------------------------------------------------------------
309  getAsImage
310  ---------------------------------------------------------------*/
312  CImage& img, bool verticalFlip, bool forceRGB) const
313 {
314  if (!forceRGB)
315  { // 8bit gray-scale
316  img.resize(m_size_x, m_size_y, CH_GRAY);
317  const cell_t* srcPtr = &m_map[0];
318  unsigned char* destPtr;
319  for (unsigned int y = 0; y < m_size_y; y++)
320  {
321  if (!verticalFlip)
322  destPtr = img(0, m_size_y - 1 - y);
323  else
324  destPtr = img(0, y);
325  for (unsigned int x = 0; x < m_size_x; x++)
326  {
327  *destPtr++ = m_logodd_lut.l2p_255(*srcPtr++);
328  }
329  }
330  }
331  else
332  { // 24bit RGB:
333  img.resize(m_size_x, m_size_y, CH_RGB);
334  const cell_t* srcPtr = &m_map[0];
335  unsigned char* destPtr;
336  for (unsigned int y = 0; y < m_size_y; y++)
337  {
338  if (!verticalFlip)
339  destPtr = img(0, m_size_y - 1 - y);
340  else
341  destPtr = img(0, y);
342  for (unsigned int x = 0; x < m_size_x; x++)
343  {
344  uint8_t c = m_logodd_lut.l2p_255(*srcPtr++);
345  *destPtr++ = c;
346  *destPtr++ = c;
347  *destPtr++ = c;
348  }
349  }
350  }
351 }
352 
353 /*---------------------------------------------------------------
354  getAs3DObject
355 ---------------------------------------------------------------*/
357  mrpt::opengl::CSetOfObjects::Ptr& outSetOfObj) const
358 {
360 
361  MRPT_START
362 
364  std::make_shared<opengl::CTexturedPlane>();
365 
366  outObj->setPlaneCorners(m_x_min, m_x_max, m_y_min, m_y_max);
367 
368  // Create the color & transparecy (alpha) images:
369  CImage imgColor(m_size_x, m_size_y, CH_GRAY);
370  CImage imgTrans(m_size_x, m_size_y, CH_GRAY);
371 
372  const cell_t* srcPtr = &m_map[0];
373  unsigned char* destPtr_color;
374  unsigned char* destPtr_trans;
375 
376  for (unsigned int y = 0; y < m_size_y; y++)
377  {
378  destPtr_color = imgColor(0, y);
379  destPtr_trans = imgTrans(0, y);
380  for (unsigned int x = 0; x < m_size_x; x++)
381  {
382  uint8_t cell255 = m_logodd_lut.l2p_255(*srcPtr++);
383  *destPtr_color++ = cell255;
384 
385  int8_t auxC = (int8_t)((signed short)cell255) - 128;
386  *destPtr_trans++ = auxC > 0 ? (auxC << 1) : ((-auxC) << 1);
387  }
388  }
389 
390  outObj->assignImage_fast(imgColor, imgTrans);
391  outSetOfObj->insert(outObj);
392 
393  MRPT_END
394 }
395 
397  const mrpt::maps::CMetricMap* otherMap,
398  const mrpt::poses::CPose3D& otherMapPose,
399  const TMatchingRatioParams& params) const
400 {
401  MRPT_UNUSED_PARAM(otherMap);
402  MRPT_UNUSED_PARAM(otherMapPose);
404  return 0;
405 }
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:241
#define min(a, b)
void fill(const int8_t &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:109
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
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)
To be added to 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:275
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:97
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:563
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 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
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
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:53
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: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
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 MRPT_END
Definition: exceptions.h:245
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:161
GLenum GLint x
Definition: glext.h:3542
static CLogOddsGridMapLUT< cell_t > m_logodd_lut
Lookup tables for log-odds.
mrpt::maps::CReflectivityGridMap2D::TInsertionOptions insertionOpts
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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019