MRPT  2.0.1
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-2020, 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  "mrpt::maps::CReflectivityGridMap2D,reflectivityMap",
34 
36 
37  = default;
38 
40  const mrpt::config::CConfigFileBase& source,
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 = dynamic_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 = dynamic_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 
225 uint8_t CReflectivityGridMap2D::serializeGetVersion() const { return 3; }
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 
243  mrpt::serialization::CArchive& in, uint8_t version)
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 << "\n----------- [CReflectivityGridMap2D::TInsertionOptions] "
279  "------------ \n\n";
280 
282 
283  out << "\n";
284 }
285 
286 /*---------------------------------------------------------------
287  loadFromConfigFile
288  ---------------------------------------------------------------*/
290  [[maybe_unused]] const mrpt::config::CConfigFileBase& iniFile,
291  [[maybe_unused]] const std::string& section)
292 {
293  MRPT_LOAD_CONFIG_VAR(channel, int, iniFile, section);
294 }
295 
296 /*---------------------------------------------------------------
297  saveMetricMapRepresentationToFile
298  ---------------------------------------------------------------*/
300  const std::string& filNamePrefix) const
301 {
302  // Text matrix:
303  saveToTextFile(filNamePrefix + std::string("_probability.txt"));
304 }
305 
306 /*---------------------------------------------------------------
307  getAsImage
308  ---------------------------------------------------------------*/
310  CImage& img, bool verticalFlip, bool forceRGB) const
311 {
312  if (!forceRGB)
313  { // 8bit gray-scale
315  const cell_t* srcPtr = &m_map[0];
316  unsigned char* destPtr;
317  for (unsigned int y = 0; y < m_size_y; y++)
318  {
319  if (!verticalFlip)
320  destPtr = img(0, m_size_y - 1 - y);
321  else
322  destPtr = img(0, y);
323  for (unsigned int x = 0; x < m_size_x; x++)
324  {
325  *destPtr++ = m_logodd_lut.l2p_255(*srcPtr++);
326  }
327  }
328  }
329  else
330  { // 24bit RGB:
332  const cell_t* srcPtr = &m_map[0];
333  unsigned char* destPtr;
334  for (unsigned int y = 0; y < m_size_y; y++)
335  {
336  if (!verticalFlip)
337  destPtr = img(0, m_size_y - 1 - y);
338  else
339  destPtr = img(0, y);
340  for (unsigned int x = 0; x < m_size_x; x++)
341  {
342  uint8_t c = m_logodd_lut.l2p_255(*srcPtr++);
343  *destPtr++ = c;
344  *destPtr++ = c;
345  *destPtr++ = c;
346  }
347  }
348  }
349 }
350 
351 /*---------------------------------------------------------------
352  getAs3DObject
353 ---------------------------------------------------------------*/
355  mrpt::opengl::CSetOfObjects::Ptr& outSetOfObj) const
356 {
358 
359  MRPT_START
360 
362  std::make_shared<opengl::CTexturedPlane>();
363 
364  outObj->setPlaneCorners(m_x_min, m_x_max, m_y_min, m_y_max);
365 
366  // Create the color & transparecy (alpha) images:
367  CImage imgColor(m_size_x, m_size_y, CH_GRAY);
368  CImage imgTrans(m_size_x, m_size_y, CH_GRAY);
369 
370  const cell_t* srcPtr = &m_map[0];
371  unsigned char* destPtr_color;
372  unsigned char* destPtr_trans;
373 
374  for (unsigned int y = 0; y < m_size_y; y++)
375  {
376  destPtr_color = imgColor(0, y);
377  destPtr_trans = imgTrans(0, y);
378  for (unsigned int x = 0; x < m_size_x; x++)
379  {
380  uint8_t cell255 = m_logodd_lut.l2p_255(*srcPtr++);
381  *destPtr_color++ = cell255;
382 
383  int8_t auxC = (int8_t)((signed short)cell255) - 128;
384  *destPtr_trans++ = auxC > 0 ? (auxC << 1) : ((-auxC) << 1);
385  }
386  }
387 
388  outObj->assignImage(imgColor, imgTrans);
389  outSetOfObj->insert(outObj);
390 
391  MRPT_END
392 }
393 
395  [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
396  [[maybe_unused]] const mrpt::poses::CPose3D& otherMapPose,
397  [[maybe_unused]] const TMatchingRatioParams& params) const
398 {
399  return 0;
400 }
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
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...
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
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
mrpt::vision::TStereoCalibParams params
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:274
void dyngridcommon_writeToStream(STREAM &out) const
Definition: CDynamicGrid.h:334
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...
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
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.
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 resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
Definition: CImage.cpp:247
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:556
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:146
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
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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...
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
return_t square(const num_t x)
Inline function for the square of a number.
#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:54
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:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void internal_clear() override
Internal method called by clear()
#define MRPT_END
Definition: exceptions.h:245
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:313
int16_t channel
The reflectivity channel for this map.
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
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
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...
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
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:148



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020