Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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/utils/round.h> // round()
16 #include <mrpt/system/os.h>
19 #include <mrpt/utils/CStream.h>
20 
21 
22 using namespace mrpt;
23 using namespace mrpt::maps;
24 using namespace mrpt::obs;
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27 using namespace mrpt::utils;
28 using namespace mrpt::system;
29 using namespace std;
30 
31 // =========== Begin of Map definition ============
32 MAP_DEFINITION_REGISTER("CReflectivityGridMap2D,reflectivityMap", mrpt::maps::CReflectivityGridMap2D)
33 
35  min_x(-10.0f),
36  max_x(10.0f),
37  min_y(-10.0f),
38  max_y(10.0f),
39  resolution(0.10f)
40 {
41 }
42 
44 {
45  // [<sectionNamePrefix>+"_creationOpts"]
46  const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
47  MRPT_LOAD_CONFIG_VAR(min_x, double, source,sSectCreation);
48  MRPT_LOAD_CONFIG_VAR(max_x, double, source,sSectCreation);
49  MRPT_LOAD_CONFIG_VAR(min_y, double, source,sSectCreation);
50  MRPT_LOAD_CONFIG_VAR(max_y, double, source,sSectCreation);
51  MRPT_LOAD_CONFIG_VAR(resolution, double, source,sSectCreation);
52 
53  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
54 }
55 
57 {
58  LOADABLEOPTS_DUMP_VAR(min_x , double);
59  LOADABLEOPTS_DUMP_VAR(max_x , double);
60  LOADABLEOPTS_DUMP_VAR(min_y , double);
61  LOADABLEOPTS_DUMP_VAR(max_y , double);
62  LOADABLEOPTS_DUMP_VAR(resolution , double);
63 
64  this->insertionOpts.dumpToTextStream(out);
65 }
66 
68 {
71  obj->insertionOptions = def.insertionOpts;
72  return obj;
73 }
74 // =========== End of Map definition Block =========
75 
77 
78 
79 // Lookup tables for log-odds
81 
82 /*---------------------------------------------------------------
83  Constructor
84  ---------------------------------------------------------------*/
86  double x_min, double x_max,
87  double y_min, double y_max,
88  double resolution ) :
89  CDynamicGrid<int8_t>( x_min,x_max,y_min,y_max,resolution ),
90  insertionOptions()
91 {
93 }
94 
95 /*---------------------------------------------------------------
96  clear
97  ---------------------------------------------------------------*/
99 {
100  fill( m_logodd_lut.p2l(0.5) );
101 }
102 
103 /*---------------------------------------------------------------
104  isEmpty
105  ---------------------------------------------------------------*/
107 {
108  return false;
109 }
110 
111 /*---------------------------------------------------------------
112  insertObservation
113  ---------------------------------------------------------------*/
115  const CObservation *obs,
116  const CPose3D *robotPose )
117 {
118  MRPT_START
119 
120  CPose2D robotPose2D;
121  CPose3D robotPose3D;
122 
123  if (robotPose)
124  {
125  robotPose2D = CPose2D(*robotPose);
126  robotPose3D = (*robotPose);
127  }
128  else
129  {
130  // Default values are (0,0,0)
131  }
132 
133  if ( IS_CLASS(obs, CObservationReflectivity ) )
134  {
135  /********************************************************************
136  OBSERVATION TYPE: CObservationReflectivity
137  ********************************************************************/
138  const CObservationReflectivity *o = static_cast<const CObservationReflectivity*>( obs );
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(new_x_min,new_x_max, new_y_min,new_y_max, default_cell , 2.0 /* addit. margin */ );
158 
159  // Now we should get the cell:
160  cell = cellByPos(sensor_pose.x(),sensor_pose.y());
161 
162  ASSERTMSG_(cell!=NULL,"cell==NULL even after resizing grid!?")
163  }
164 
165  const int cell_old = static_cast<int>(*cell);
166  int cell_new = cell_old + static_cast<int>(logodd_observation);
167  keep_min(cell_new, static_cast<int>( CELLTYPE_MAX)-1);
168  keep_max(cell_new, static_cast<int>( CELLTYPE_MIN)+1);
169 
170  *cell = static_cast<cell_t>(cell_new);
171 
172  return true; // Done!
173  } // end if "CObservationGasSensors"
174 
175  return false;
176 
177  MRPT_END
178 }
179 
180 
181 /*---------------------------------------------------------------
182  computeObservationLikelihood
183  ---------------------------------------------------------------*/
185  const CObservation *obs,
186  const CPose3D &takenFrom )
187 {
188  MRPT_START
189 
190  if ( IS_CLASS(obs, CObservationReflectivity ) )
191  {
192  /********************************************************************
193  OBSERVATION TYPE: CObservationReflectivity
194  ********************************************************************/
195  const CObservationReflectivity *o = static_cast<const CObservationReflectivity*>( obs );
196 
197  CPose3D sensor_pose;
198  sensor_pose.composeFrom(takenFrom, o->sensorPose);
199 
200  cell_t *cell = cellByPos(sensor_pose.x(),sensor_pose.y());
201  if (!cell)
202  return 0; // out of the map..
203  else
204  {
207  return -0.5*square( ( m_logodd_lut.l2p(*cell) - o->reflectivityLevel )/o->sensorStdNoise);
208  }
209  }
210  else
211  return 0;
212 
213  MRPT_END
214 }
215 
216 /*---------------------------------------------------------------
217  Implements the writing to a CStream capability of CSerializable objects
218  ---------------------------------------------------------------*/
220 {
221  if (version)
222  *version = 2;
223  else
224  {
226 
227  // Map cells:
228  const uint32_t n = static_cast<uint32_t>(m_map.size());
229  out << n;
230  if (n)
231  out.WriteBuffer(&m_map[0],n);
232 
233  out << genericMapParams; // v1
234  }
235 }
236 
237 /*---------------------------------------------------------------
238  Implements the reading from a CStream capability of CSerializable objects
239  ---------------------------------------------------------------*/
241 {
242  switch(version)
243  {
244  case 0:
245  case 1:
246  case 2:
247  {
249 
250  // Map cells:
251  uint32_t n;
252  in >> n;
253  m_map.resize(n);
254  if (n)
255  in.ReadBuffer(&m_map[0],n);
256 
257  if (version>=1)
258  in >> genericMapParams;
259 
260  } break;
261  default:
263 
264  };
265 
266 }
267 
268 /*---------------------------------------------------------------
269  TInsertionOptions
270  ---------------------------------------------------------------*/
272 {
273 }
274 
275 /*---------------------------------------------------------------
276  dumpToTextStream
277  ---------------------------------------------------------------*/
279 {
280  out.printf("\n----------- [CReflectivityGridMap2D::TInsertionOptions] ------------ \n\n");
281 
282  //LOADABLEOPTS_DUMP_VAR(maxOccupancyUpdateCertainty, float)
283 
284  out.printf("\n");
285 }
286 
287 /*---------------------------------------------------------------
288  loadFromConfigFile
289  ---------------------------------------------------------------*/
291  const mrpt::utils::CConfigFileBase &iniFile,
292  const std::string &section)
293 {
294  MRPT_UNUSED_PARAM(iniFile);
295  MRPT_UNUSED_PARAM(section);
296  //MRPT_LOAD_CONFIG_VAR( maxOccupancyUpdateCertainty, float, 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  ---------------------------------------------------------------*/
314  bool verticalFlip,
315  bool forceRGB ) const
316 {
317  if (!forceRGB)
318  { // 8bit gray-scale
319  img.resize(m_size_x,m_size_y,1,true);
320  const cell_t *srcPtr = &m_map[0];
321  unsigned char *destPtr;
322  for (unsigned int y=0;y<m_size_y;y++)
323  {
324  if (!verticalFlip)
325  destPtr = img(0,m_size_y-1-y);
326  else destPtr = img(0,y);
327  for (unsigned int x=0;x<m_size_x;x++)
328  {
329  *destPtr++ = m_logodd_lut.l2p_255(*srcPtr++);
330  }
331  }
332  }
333  else
334  { // 24bit RGB:
335  img.resize(m_size_x,m_size_y,3,true);
336  const cell_t *srcPtr = &m_map[0];
337  unsigned char *destPtr;
338  for (unsigned int y=0;y<m_size_y;y++)
339  {
340  if (!verticalFlip)
341  destPtr = img(0,m_size_y-1-y);
342  else 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 /*---------------------------------------------------------------
356  getAs3DObject
357 ---------------------------------------------------------------*/
358 void CReflectivityGridMap2D::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outSetOfObj ) const
359 {
361 
362  MRPT_START
363 
364  opengl::CTexturedPlanePtr outObj = opengl::CTexturedPlane::Create();
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,1);
370  CImage imgTrans(m_size_x,m_size_y,1);
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 {
398  MRPT_UNUSED_PARAM(otherMap); MRPT_UNUSED_PARAM(otherMapPose);
400  return 0;
401 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Parameters for CMetricMap::compute3DMatchingRatio()
#define ASSERT_BELOWEQ_(__A, __B)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define min(a, b)
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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:29
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:4618
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
static CTexturedPlanePtr Create()
float reflectivityLevel
The read reflectivity level, in the range [0,1] (0=black, 1=white).
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:109
signed char int8_t
Definition: rptypes.h:42
Declares a class derived from "CObservation" that encapsules a single short-range reflectivity measur...
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
Load all map-specific params.
void fill(const int8_t &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:101
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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...
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::poses::CPose3D sensorPose
The pose of this sensor in robot&#39;s local coordinates.
unsigned char uint8_t
Definition: rptypes.h:43
int8_t * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map...
Definition: CDynamicGrid.h:183
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
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...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
See docs in base class: in this class this always returns 0.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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:595
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Internal method called by insertObservation()
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLint GLvoid * img
Definition: glext.h:3645
std::vector< int8_t > m_map
The cells.
Definition: CDynamicGrid.h:43
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
This namespace contains representation of robot actions and observations.
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
int version
Definition: mrpt_jpeglib.h:898
GLsizei const GLchar ** string
Definition: glext.h:3919
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...
Definition: bits.h:176
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double resolution
See CReflectivityGridMap2DOptions::CReflectivityGridMap2DOptions.
#define MRPT_START
#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...
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.
#define ASSERT_ABOVEEQ_(__A, __B)
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)...
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...
Definition: bits.h:171
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:280
GLuint in
Definition: glext.h:6301
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
Definition: CDynamicGrid.h:295
GLenum GLint GLint y
Definition: glext.h:3516
void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
void getAsImage(utils::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 ...
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
GLenum GLint x
Definition: glext.h:3516
static CLogOddsGridMapLUT< cell_t > m_logodd_lut
Lookup tables for log-odds.
mrpt::maps::CReflectivityGridMap2D::TInsertionOptions insertionOpts
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:49
void dyngridcommon_readFromStream(mrpt::utils::CStream &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:300
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
#define ASSERTMSG_(f, __ERROR_MSG)
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
GLenum const GLfloat * params
Definition: glext.h:3514
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019