Main MRPT website > C++ reference for MRPT 1.5.6
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 {
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 {
92  internal_clear();
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  {
225  dyngridcommon_writeToStream(out);
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  {
248  dyngridcommon_readFromStream(in, version<2);
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 {
360  if (!genericMapParams.enableSaveAs3DObject) return;
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);
399  MRPT_UNUSED_PARAM(params);
400  return 0;
401 }
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer On any error, or if ZERO bytes are read...
Definition: CStream.cpp:45
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
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)
#define min(a, b)
EIGEN_STRONG_INLINE void fill(const Scalar v)
Definition: eigen_plugins.h:38
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
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.
const GLfloat * c
Definition: glew.h:10088
void resize(unsigned int width, unsigned int height, TImageChannels nChannels, bool originTopLeft)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
Definition: CImage.h:209
#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).
signed char int8_t
Definition: rptypes.h:42
Declares a class derived from "CObservation" that encapsules a single short-range reflectivity measur...
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
Load all map-specific params.
GLuint in
Definition: glew.h:7146
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's local coordinates.
unsigned char uint8_t
Definition: rptypes.h:43
GLint GLvoid * img
Definition: glew.h:1290
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLsizei n
Definition: glew.h:5051
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()
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
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 ...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
GLhandleARB obj
Definition: glew.h:3276
int version
Definition: mrpt_jpeglib.h:898
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
double resolution
See CReflectivityGridMap2DOptions::CReflectivityGridMap2DOptions.
mrpt::maps::CReflectivityGridMap2D::TInsertionOptions insertionOptions
#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...
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
#define ASSERT_ABOVEEQ_(__A, __B)
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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'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:103
GLfloat * params
Definition: glew.h:1436
void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
mrpt::maps::CReflectivityGridMap2D::TInsertionOptions insertionOpts
unsigned __int32 uint32_t
Definition: rptypes.h:49
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)
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.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018