Main MRPT website > C++ reference for MRPT 1.5.7
COccupancyGridMap2D_common.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 
12 // Force size_x being a multiple of 16 cells
13 //#define ROWSIZE_MULTIPLE_16
14 
17 #include <mrpt/utils/CStream.h>
18 #include <mrpt/poses/CPose3D.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::math;
22 using namespace mrpt::maps;
23 using namespace mrpt::obs;
24 using namespace mrpt::utils;
25 using namespace mrpt::poses;
26 using namespace std;
27 
28 // =========== Begin of Map definition ============
29 MAP_DEFINITION_REGISTER("COccupancyGridMap2D,occupancyGrid", mrpt::maps::COccupancyGridMap2D)
30 
32  min_x(-10.0f),
33  max_x(10.0f),
34  min_y(-10.0f),
35  max_y(10.0f),
36  resolution(0.10f)
37 {
38 }
39 
40 void COccupancyGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
41 {
42  // [<sectionNamePrefix>+"_creationOpts"]
43  const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
44  MRPT_LOAD_CONFIG_VAR(min_x, float, source,sSectCreation);
45  MRPT_LOAD_CONFIG_VAR(max_x, float, source,sSectCreation);
46  MRPT_LOAD_CONFIG_VAR(min_y, float, source,sSectCreation);
47  MRPT_LOAD_CONFIG_VAR(max_y, float, source,sSectCreation);
48  MRPT_LOAD_CONFIG_VAR(resolution, float, source,sSectCreation);
49 
50  // [<sectionName>+"_occupancyGrid_##_insertOpts"]
51  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
52 
53  // [<sectionName>+"_occupancyGrid_##_likelihoodOpts"]
54  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix+string("_likelihoodOpts") );
55 
56 }
57 
58 void COccupancyGridMap2D::TMapDefinition::dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
59 {
60  LOADABLEOPTS_DUMP_VAR(min_x , float);
61  LOADABLEOPTS_DUMP_VAR(max_x , float);
62  LOADABLEOPTS_DUMP_VAR(min_y , float);
63  LOADABLEOPTS_DUMP_VAR(max_y , float);
64  LOADABLEOPTS_DUMP_VAR(resolution , float);
65 
66  this->insertionOpts.dumpToTextStream(out);
67  this->likelihoodOpts.dumpToTextStream(out);
68 }
69 
70 mrpt::maps::CMetricMap* COccupancyGridMap2D::internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &_def)
71 {
72  const COccupancyGridMap2D::TMapDefinition &def = *dynamic_cast<const COccupancyGridMap2D::TMapDefinition*>(&_def);
74  obj->insertionOptions = def.insertionOpts;
75  obj->likelihoodOptions = def.likelihoodOpts;
76  return obj;
77 }
78 // =========== End of Map definition Block =========
79 
81 
82 std::vector<float> COccupancyGridMap2D::entropyTable;
83 
84 static const float MAX_H = 0.69314718055994531f; // ln(2)
85 
86 // Static lookup tables for log-odds
87 CLogOddsGridMapLUT<COccupancyGridMap2D::cellType> COccupancyGridMap2D::m_logodd_lut;
88 
89 /*---------------------------------------------------------------
90  Constructor
91  ---------------------------------------------------------------*/
92 COccupancyGridMap2D::COccupancyGridMap2D(
93  float min_x,
94  float max_x,
95  float min_y,
96  float max_y,
97  float resolution) :
98  map(),
99  size_x(0),size_y(0),
100  x_min(),x_max(),y_min(),y_max(), resolution(),
101  precomputedLikelihood(),
102  precomputedLikelihoodToBeRecomputed(true),
103  m_basis_map(),
104  m_voronoi_diagram(),
105  m_is_empty(true),
106  voroni_free_threshold(),
107  updateInfoChangeOnly(),
108  insertionOptions(),
109  likelihoodOptions(),
110  likelihoodOutputs(),
111  CriticalPointsList()
112 {
113  MRPT_START
114  setSize(min_x,max_x,min_y,max_y, resolution,0.5f );
115  MRPT_END
116 }
117 
118 
119 /*---------------------------------------------------------------
120  Destructor
121  ---------------------------------------------------------------*/
123 {
124  freeMap();
125 }
126 
128 {
129  freeMap();
131  x_min = o.x_min;
132  x_max = o.x_max;
133  y_min = o.y_min;
134  y_max = o.y_max;
135  size_x = o.size_x;
136  size_y = o.size_y;
137  map = o.map;
138 
139  m_basis_map.clear();
141 
144 }
145 
146 /*---------------------------------------------------------------
147  setSize
148  ---------------------------------------------------------------*/
150  float x_min,
151  float x_max,
152  float y_min,
153  float y_max,
154  float resolution,
155  float default_value)
156 {
157  MRPT_START
158 
161  ASSERT_(default_value>=0 && default_value<=1)
162 
163  // Liberar primero:
164  freeMap();
165 
166  // For the precomputed likelihood trick:
168 
169  // Adjust sizes to adapt them to full sized cells acording to the resolution:
174 
175  // Set parameters:
176  this->resolution = resolution;
177  this->x_min = x_min;
178  this->x_max = x_max;
179  this->y_min = y_min;
180  this->y_max = y_max;
181 
182  // Now the number of cells should be integers:
185 
186 #ifdef ROWSIZE_MULTIPLE_16
187  // map rows must be 16 bytes aligned:
188  if (0!=(size_x % 16))
189  {
190  size_x = ((size_x >> 4)+1) << 4;
192  }
194  ASSERT_(0==(size_x % 16));
195 #endif
196 
197  // Cells memory:
198  map.resize(size_x*size_y,p2l(default_value));
199 
200  // Free these buffers also:
201  m_basis_map.clear();
203 
204  m_is_empty=true;
205 
206  MRPT_END
207 }
208 
209 /*---------------------------------------------------------------
210  ResizeGrid
211  ---------------------------------------------------------------*/
212 void COccupancyGridMap2D::resizeGrid(float new_x_min,float new_x_max,float new_y_min,float new_y_max,float new_cells_default_value, bool additionalMargin) MRPT_NO_THROWS
213 {
214  unsigned int extra_x_izq=0,extra_y_arr=0,new_size_x=0,new_size_y=0;
215  std::vector<cellType> new_map;
216 
217  if( new_x_min > new_x_max )
218  {
219  printf("[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: x_min=%f x_max=%f\n", new_x_min, new_x_max);
220  return;
221  }
222  if( new_y_min > new_y_max )
223  {
224  printf("[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: y_min=%f y_max=%f\n", new_y_min, new_y_max);
225  return;
226  }
227 
228  // Required?
229  if (new_x_min>=x_min &&
230  new_y_min>=y_min &&
231  new_x_max<=x_max &&
232  new_y_max<=y_max) return;
233 
234  // For the precomputed likelihood trick:
235  precomputedLikelihoodToBeRecomputed = true;
236 
237  // Add an additional margin:
238  if (additionalMargin)
239  {
240  if (new_x_min<x_min) new_x_min= floor(new_x_min-4);
241  if (new_x_max>x_max) new_x_max= ceil(new_x_max+4);
242  if (new_y_min<y_min) new_y_min= floor(new_y_min-4);
243  if (new_y_max>y_max) new_y_max= ceil(new_y_max+4);
244  }
245 
246  // We do not support grid shrinking... at least stay the same:
247  new_x_min = min( new_x_min, x_min);
248  new_x_max = max( new_x_max, x_max);
249  new_y_min = min( new_y_min, y_min);
250  new_y_max = max( new_y_max, y_max);
251 
252 
253  // Adjust sizes to adapt them to full sized cells acording to the resolution:
254  if (fabs(new_x_min/resolution - round(new_x_min/resolution))>0.05f )
255  new_x_min = resolution*round(new_x_min/resolution);
256  if (fabs(new_y_min/resolution - round(new_y_min/resolution))>0.05f )
257  new_y_min = resolution*round(new_y_min/resolution);
258  if (fabs(new_x_max/resolution - round(new_x_max/resolution))>0.05f )
259  new_x_max = resolution*round(new_x_max/resolution);
260  if (fabs(new_y_max/resolution - round(new_y_max/resolution))>0.05f )
261  new_y_max = resolution*round(new_y_max/resolution);
262 
263  // Change size: 4 sides extensions:
264  extra_x_izq = round((x_min-new_x_min) / resolution);
265  extra_y_arr = round((y_min-new_y_min) / resolution);
266 
267  new_size_x = round((new_x_max-new_x_min) / resolution);
268  new_size_y = round((new_y_max-new_y_min) / resolution);
269 
270  assert( new_size_x>=size_x+extra_x_izq );
271 
272 #ifdef ROWSIZE_MULTIPLE_16
273  // map rows must be 16 bytes aligned:
274  size_t old_new_size_x = new_size_x; // Debug
275  if (0!=(new_size_x % 16))
276  {
277  int size_x_incr = 16 - (new_size_x % 16);
278  //new_x_max = new_x_min + new_size_x * resolution;
279  new_x_max += size_x_incr * resolution;
280  }
281  new_size_x = round((new_x_max-new_x_min)/resolution);
282  assert(0==(new_size_x % 16));
283 #endif
284 
285  // Reserve new mem block
286  new_map.resize(new_size_x*new_size_y, p2l(new_cells_default_value));
287 
288  // Copy all the old map rows into the new map:
289  {
290  cellType *dest_ptr = &new_map[extra_x_izq + extra_y_arr*new_size_x];
291  cellType *src_ptr = &map[0];
292  size_t row_size = size_x*sizeof(cellType);
293 
294  for (size_t y = 0;y<size_y;y++)
295  {
296 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
297  assert( dest_ptr+row_size-1 <= &new_map[new_map.size()-1] );
298  assert( src_ptr+row_size-1 <= &map[map.size()-1] );
299 #endif
300  memcpy( dest_ptr, src_ptr, row_size );
301  dest_ptr += new_size_x;
302  src_ptr += size_x;
303  }
304  }
305 
306  // Move new values into the new map:
307  x_min = new_x_min;
308  x_max = new_x_max;
309  y_min = new_y_min;
310  y_max = new_y_max;
311 
312  size_x = new_size_x;
313  size_y = new_size_y;
314 
315  // Free old map, replace by new one:
316  map.swap( new_map );
317 
318  // Free the other buffers:
319  m_basis_map.clear();
320  m_voronoi_diagram.clear();
321 }
322 
323 /*---------------------------------------------------------------
324  freeMap
325  ---------------------------------------------------------------*/
327 {
328  MRPT_START
329 
330  // Free map and sectors
331  map.clear();
332 
333  m_basis_map.clear();
335 
336  size_x=size_y=0;
337 
338  // For the precomputed likelihood trick:
340 
341  m_is_empty=true;
342 
343  MRPT_END
344 }
345 
346 
347 
348 /*---------------------------------------------------------------
349  Computes the entropy and related values of this grid map.
350  out_H The target variable for absolute entropy, computed as:<br><center>H(map)=Sum<sub>x,y</sub>{ -p(x,y)ln(p(x,y)) -(1-p(x,y))ln(1-p(x,y)) }</center><br><br>
351  out_I The target variable for absolute "information", defining I(x) = 1 - H(x)
352  out_mean_H The target variable for mean entropy, defined as entropy per square meter: mean_H(map) = H(map) / (Map length x (meters))(Map length y (meters))
353  out_mean_I The target variable for mean information, defined as information per square meter: mean_I(map) = I(map) / (Map length x (meters))(Map length y (meters))
354  ---------------------------------------------------------------*/
356 {
357  unsigned long i;
358  float h,p;
359 
360 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
361  unsigned int N = 256;
362 #else
363  unsigned int N = 65536;
364 #endif
365 
366  // Compute the entropy table: The entropy for each posible cell value
367  // ----------------------------------------------------------------------
368  if (entropyTable.size()!=N)
369  {
370  entropyTable.resize(N,0);
371  for (i=0;i<N;i++)
372  {
373  p = l2p(static_cast<cellType>(i));
374  h = H(p)+H(1-p);
375 
376  // Cell's probabilities rounding problem fixing:
377  if (i==0 || i==(N-1)) h=0;
378  if (h>(MAX_H - 1e-4)) h=MAX_H;
379 
380  entropyTable[i] = h;
381  }
382  }
383 
384  // Initialize the global results:
385  info.H = info.I = 0;
386  info.effectiveMappedCells = 0;
387 
388 
389  info.H = info.I = 0;
390  info.effectiveMappedCells = 0;
391  for ( std::vector<cellType>::const_iterator it=map.begin();it!=map.end();++it)
392  {
393  cellTypeUnsigned i = static_cast<cellTypeUnsigned>(*it);
394  h = entropyTable[ i ];
395  info.H+= h;
396  if (h<(MAX_H-0.001f))
397  {
398  info.effectiveMappedCells++;
399  info.I-=h;
400  }
401  }
402 
403  // The info: (See ref. paper EMMI in IROS 2006)
404  info.I /= MAX_H;
405  info.I += info.effectiveMappedCells;
406 
407  // Mean values:
408  // ------------------------------------------
409  info.effectiveMappedArea = info.effectiveMappedCells * resolution*resolution;
410  if (info.effectiveMappedCells)
411  {
412  info.mean_H = info.H / info.effectiveMappedCells;
413  info.mean_I = info.I / info.effectiveMappedCells;
414  }
415  else
416  {
417  info.mean_H = 0;
418  info.mean_I = 0;
419  }
420 }
421 
422 /*---------------------------------------------------------------
423  Entropy aux. function
424  ---------------------------------------------------------------*/
425 double COccupancyGridMap2D::H(double p)
426 {
427  if (p==0 || p==1) return 0;
428  else return -p*log(p);
429 }
430 
431 
432 /*---------------------------------------------------------------
433  clear
434  ---------------------------------------------------------------*/
436 {
437  setSize( -10,10,-10,10,getResolution());
438  //resetFeaturesCache();
439  // For the precomputed likelihood trick:
441 }
442 
443 /*---------------------------------------------------------------
444  fill
445  ---------------------------------------------------------------*/
446 void COccupancyGridMap2D::fill(float default_value)
447 {
448  cellType defValue = p2l( default_value );
449  for (std::vector<cellType>::iterator it=map.begin();it<map.end();++it)
450  *it = defValue;
451  // For the precomputed likelihood trick:
453  //resetFeaturesCache();
454 }
455 
456 /*---------------------------------------------------------------
457  updateCell
458  ---------------------------------------------------------------*/
459 void COccupancyGridMap2D::updateCell(int x,int y, float v)
460 {
461  // Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
462  if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
463  return;
464 
465  // Get the current contents of the cell:
466  cellType &theCell = map[x+y*size_x];
467 
468  // Compute the new Bayesian-fused value of the cell:
470  {
471  float old = l2p(theCell);
472  float new_v = 1 / ( 1 + (1-v)*(1-old)/(old*v) );
474  updateInfoChangeOnly.I_change+= 1-(H(new_v)+H(1-new_v))/MAX_H;
475  }
476  else
477  {
478  cellType obs = p2l(v); // The observation: will be >0 for free, <0 for occupied.
479  if (obs>0)
480  {
481  if ( theCell>(OCCGRID_CELLTYPE_MAX-obs) )
482  theCell = OCCGRID_CELLTYPE_MAX; // Saturate
483  else theCell += obs;
484  }
485  else
486  {
487  if ( theCell<(OCCGRID_CELLTYPE_MIN-obs) )
488  theCell = OCCGRID_CELLTYPE_MIN; // Saturate
489  else theCell += obs;
490  }
491  }
492 }
493 
494 
495 /*---------------------------------------------------------------
496  subSample
497  ---------------------------------------------------------------*/
498 void COccupancyGridMap2D::subSample( int downRatio )
499 {
500  std::vector<cellType> newMap;
501 
502  ASSERT_(downRatio>0);
503 
504  resolution*=downRatio;
505 
506  int newSizeX = round((x_max-x_min)/resolution);
507  int newSizeY = round((y_max-y_min)/resolution);
508 
509  newMap.resize(newSizeX*newSizeY);
510 
511  for (int x=0;x<newSizeX;x++)
512  {
513  for (int y=0;y<newSizeY;y++)
514  {
515  float newCell = 0;
516 
517  for (int xx=0;xx<downRatio;xx++)
518  for (int yy=0;yy<downRatio;yy++)
519  newCell+= getCell(x*downRatio+xx, y*downRatio+yy);
520 
521  newCell /= (downRatio*downRatio);
522 
523  newMap[ x + y*newSizeX ] = p2l(newCell);
524  }
525  }
526 
527 
529  map = newMap;
530 
531 
532 }
533 
534 
535 
536 /*---------------------------------------------------------------
537  computeMatchingWith
538  ---------------------------------------------------------------*/
540  const mrpt::maps::CMetricMap * otherMap2,
541  const CPose2D & otherMapPose_,
542  TMatchingPairList & correspondences,
543  const TMatchingParams & params,
544  TMatchingExtraResults & extraResults ) const
545 {
546  MRPT_START
547 
548  extraResults = TMatchingExtraResults();
549 
550  ASSERT_ABOVE_(params.decimation_other_map_points,0)
551  ASSERT_BELOW_(params.offset_other_map_points, params.decimation_other_map_points)
552 
554  const CPointsMap *otherMap = static_cast<const CPointsMap*>(otherMap2);
555 
556  const TPose2D otherMapPose = TPose2D(otherMapPose_);
557 
558  const size_t nLocalPoints = otherMap->size();
559  std::vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),z_locals(nLocalPoints);
560 
561  const float sin_phi = sin(otherMapPose.phi);
562  const float cos_phi = cos(otherMapPose.phi);
563 
564  size_t nOtherMapPointsWithCorrespondence = 0; // Number of points with one corrs. at least
565  size_t nTotalCorrespondences = 0; // Total number of corrs
566  float _sumSqrDist=0;
567 
568  // The number of cells to look around each point:
569  const int cellsSearchRange = round( params.maxDistForCorrespondence / resolution );
570 
571  // Initially there are no correspondences:
572  correspondences.clear();
573 
574  // Hay mapa local?
575  if (!nLocalPoints) return; // No
576 
577  // Solo hacer matching si existe alguna posibilidad de que
578  // los dos mapas se toquen:
579  // -----------------------------------------------------------
580  float local_x_min = std::numeric_limits<float>::max();
581  float local_x_max = -std::numeric_limits<float>::max();
582  float local_y_min = std::numeric_limits<float>::max();
583  float local_y_max = -std::numeric_limits<float>::max();
584 
585  const std::vector<float> & otherMap_pxs = otherMap->getPointsBufferRef_x();
586  const std::vector<float> & otherMap_pys = otherMap->getPointsBufferRef_y();
587  const std::vector<float> & otherMap_pzs = otherMap->getPointsBufferRef_z();
588 
589  // Translate all local map points:
590  for (unsigned int localIdx=params.offset_other_map_points;localIdx<nLocalPoints;localIdx+=params.decimation_other_map_points)
591  {
592  // Girar y desplazar cada uno de los puntos del local map:
593  const float xx = x_locals[localIdx] = otherMapPose.x + cos_phi* otherMap_pxs[localIdx] - sin_phi* otherMap_pys[localIdx] ;
594  const float yy = y_locals[localIdx] = otherMapPose.y + sin_phi* otherMap_pxs[localIdx] + cos_phi* otherMap_pys[localIdx] ;
595  z_locals[localIdx] = /* otherMapPose.z +*/ otherMap_pzs[localIdx];
596 
597  // mantener el max/min de los puntos:
598  local_x_min = min(local_x_min,xx);
599  local_x_max = max(local_x_max,xx);
600  local_y_min = min(local_y_min,yy);
601  local_y_max = max(local_y_max,yy);
602  }
603 
604  // If the local map is entirely out of the grid,
605  // do not even try to match them!!
606  if (local_x_min> x_max ||
607  local_x_max< x_min ||
608  local_y_min> y_max ||
609  local_y_max< y_min) return; // Matching is NULL!
610 
611 
612  const cellType thresholdCellValue = p2l(0.5f);
613 
614  // For each point in the other map:
615  for (unsigned int localIdx=params.offset_other_map_points;localIdx<nLocalPoints;localIdx+=params.decimation_other_map_points)
616  {
617  // Starting value:
618  float maxDistForCorrespondenceSquared = square( params.maxDistForCorrespondence );
619 
620  // For speed-up:
621  const float x_local = x_locals[localIdx];
622  const float y_local = y_locals[localIdx];
623  const float z_local = z_locals[localIdx];
624 
625  // Look for the occupied cell closest from the map point:
626  float min_dist = 1e6;
627  TMatchingPair closestCorr;
628 
629  // Get the indexes of cell where the point falls:
630  const int cx0=x2idx(x_local);
631  const int cy0=y2idx(y_local);
632 
633  // Get the rectangle to look for into:
634  const int cx_min = max(0, cx0 - cellsSearchRange );
635  const int cx_max = min(static_cast<int>(size_x)-1, cx0 + cellsSearchRange );
636  const int cy_min = max(0, cy0 - cellsSearchRange );
637  const int cy_max = min(static_cast<int>(size_y)-1, cy0 + cellsSearchRange );
638 
639  // Will be set to true if a corrs. is found:
640  bool thisLocalHasCorr = false;
641 
642 
643  // Look in nearby cells:
644  for (int cx=cx_min;cx<=cx_max;cx++)
645  {
646  for (int cy=cy_min;cy<=cy_max;cy++)
647  {
648  // Is an occupied cell?
649  if ( map[cx+cy*size_x] < thresholdCellValue )// getCell(cx,cy)<0.49)
650  {
651  const float residual_x = idx2x(cx)- x_local;
652  const float residual_y = idx2y(cy)- y_local;
653 
654  // Compute max. allowed distance:
655  maxDistForCorrespondenceSquared = square(
656  params.maxAngularDistForCorrespondence * params.angularDistPivotPoint.distanceTo(TPoint3D(x_local,y_local,0) ) +
657  params.maxDistForCorrespondence );
658 
659  // Square distance to the point:
660  const float this_dist = square( residual_x ) + square( residual_y );
661 
662  if (this_dist<maxDistForCorrespondenceSquared)
663  {
664  if (!params.onlyKeepTheClosest)
665  {
666  // save the correspondence:
667  nTotalCorrespondences++;
668  TMatchingPair mp;
669  mp.this_idx = cx+cy*size_x;
670  mp.this_x = idx2x(cx);
671  mp.this_y = idx2y(cy);
672  mp.this_z = z_local;
673  mp.other_idx = localIdx;
674  mp.other_x = otherMap_pxs[localIdx];
675  mp.other_y = otherMap_pys[localIdx];
676  mp.other_z = otherMap_pzs[localIdx];
677  correspondences.push_back( mp );
678  }
679  else
680  {
681  // save the closest only:
682  if (this_dist<min_dist)
683  {
684  min_dist = this_dist;
685 
686  closestCorr.this_idx = cx+cy*size_x;
687  closestCorr.this_x = idx2x(cx);
688  closestCorr.this_y = idx2y(cy);
689  closestCorr.this_z = z_local;
690  closestCorr.other_idx = localIdx;
691  closestCorr.other_x = otherMap_pxs[localIdx];
692  closestCorr.other_y = otherMap_pys[localIdx];
693  closestCorr.other_z = otherMap_pzs[localIdx];
694  }
695  }
696 
697  // At least one:
698  thisLocalHasCorr = true;
699  }
700  }
701  }
702  } // End of find closest nearby cell
703 
704  // save the closest correspondence:
705  if (params.onlyKeepTheClosest && (min_dist<maxDistForCorrespondenceSquared))
706  {
707  nTotalCorrespondences++;
708  correspondences.push_back( closestCorr );
709  }
710 
711  // At least one corr:
712  if (thisLocalHasCorr)
713  {
714  nOtherMapPointsWithCorrespondence++;
715 
716  // Accumulate the MSE:
717  _sumSqrDist+= min_dist;
718  }
719 
720  } // End "for each local point"...
721 
722  extraResults.correspondencesRatio = nOtherMapPointsWithCorrespondence / static_cast<float>(nLocalPoints/params.decimation_other_map_points);
723  extraResults.sumSqrDist = _sumSqrDist;
724 
725 
726  MRPT_END
727 }
728 
729 
730 /*---------------------------------------------------------------
731  isEmpty
732  ---------------------------------------------------------------*/
734 {
735  return m_is_empty;
736 }
737 
738 
739 
740 /*---------------------------------------------------------------
741  operator <
742  ---------------------------------------------------------------*/
744 {
745  return e1.first > e2.first;
746 }
747 
748 
749 /*---------------------------------------------------------------
750  computePathCost
751  ---------------------------------------------------------------*/
752 float COccupancyGridMap2D::computePathCost( float x1, float y1, float x2, float y2 ) const
753 {
754  float sumCost = 0;
755 
756  float dist = sqrt( square(x1-x2)+square(y1-y2) );
757  int nSteps = round(1.5f * dist / resolution);
758 
759  for (int i=0;i<nSteps;i++)
760  {
761  float x = x1 + (x2-x1)*i/static_cast<float>(nSteps);
762  float y = y1 + (y2-y1)*i/static_cast<float>(nSteps);
763  sumCost += getPos( x,y );
764  }
765 
766  if (nSteps)
767  return sumCost/static_cast<float>(nSteps);
768  else return 0;
769 }
770 
772 {
773  MRPT_UNUSED_PARAM(otherMap); MRPT_UNUSED_PARAM(otherMapPose);
775  return 0;
776 }
777 
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
float y_max
The limits of the grid in "units" (meters)
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Parameters for CMetricMap::compute3DMatchingRatio()
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:94
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define min(a, b)
float getResolution() const
Returns the resolution of the grid map.
#define ASSERT_ABOVE_(__A, __B)
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
mrpt::utils::CDynamicGrid< uint8_t > m_basis_map
Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
float resolution
See COccupancyGridMap2D::COccupancyGridMap2D.
float resolution
Cell size, i.e. resolution of the grid map.
static double H(double p)
Entropy computation internal function:
#define ASSERT_BELOW_(__A, __B)
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:28
Scalar * iterator
Definition: eigen_plugins.h:23
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
#define MRPT_NO_THROWS
C++11 noexcept: Used after member declarations.
void updateCell(int x, int y, float v)
Performs the Bayesian fusion of a new observation of a cell.
static const float MAX_H
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
void freeMap()
Frees the dynamic memory buffers of map.
std::vector< cellType > map
Store of cell occupancy values. Order: row by row, from left to right.
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
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
struct MAPS_IMPEXP mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly updateInfoChangeOnly
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
void copyMapContentFrom(const COccupancyGridMap2D &otherMap)
copy the gridmap contents, but not all the options, from another map instance
This namespace contains representation of robot actions and observations.
void subSample(int downRatio)
Performs a downsampling of the gridmap, by a given factor: resolution/=ratio.
double I_change
The cummulative change in Information: This is updated only from the "updateCell" method...
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double y
X,Y coordinates.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
bool isEmpty() const MRPT_OVERRIDE
Returns true upon map construction or after calling clear(), the return changes to false upon success...
void resizeGrid(float new_x_min, float new_x_max, float new_y_min, float new_y_max, float new_cells_default_value=0.5f, bool additionalMargin=true) MRPT_NO_THROWS
Change the size of gridmap, maintaining previous contents.
A class for storing an occupancy grid map.
#define MRPT_START
static const cellType OCCGRID_CELLTYPE_MIN
Discrete to float conversion factors: The min/max values of the integer cell type, eg.
#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...
const GLdouble * v
Definition: glext.h:3603
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.
static cellType 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...
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
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
backing_store_ptr info
Definition: jmemsys.h:170
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const MRPT_OVERRIDE
See the base class for more details: In this class it is implemented as correspondences of the passed...
bool m_is_empty
True upon construction; used by isEmpty()
mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Lightweight 2D pose.
uint32_t size_y
The size of the grid in cells.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#define ASSERT_(f)
float idx2y(const size_t cy) const
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
GLenum GLint GLint y
Definition: glext.h:3516
void computeEntropy(TEntropyInfo &info) const
Computes the entropy and related values of this grid map.
void fill(float default_value=0.5f)
Fills all the cells with a default value.
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
void setSize(float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
GLenum GLint x
Definition: glext.h:3516
Lightweight 3D point.
float computePathCost(float x1, float y1, float x2, float y2) const
Compute the &#39;cost&#39; of traversing a segment of the map according to the occupancy of traversed cells...
Parameters for the determination of matchings between point clouds, etc.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
Used for returning entropy related information.
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
GLfloat GLfloat p
Definition: glext.h:5587
bool MAPS_IMPEXP operator<(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2)
GLenum const GLfloat * params
Definition: glext.h:3514
int16_t cellType
The type of the map cells:
int x2idx(float x) const
Transform a coordinate value into a cell index.
double phi
Orientation (rads)
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
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.
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0...
virtual void internal_clear() MRPT_OVERRIDE
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resoluti...
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
bool enabled
If set to false (default), this struct is not used. Set to true only when measuring the info of an ob...
mrpt::utils::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019