Main MRPT website > C++ reference for MRPT 1.5.6
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);
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
88 
89 /*---------------------------------------------------------------
90  Constructor
91  ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
122 COccupancyGridMap2D::~COccupancyGridMap2D()
123 {
124  freeMap();
125 }
126 
127 void COccupancyGridMap2D::copyMapContentFrom(const COccupancyGridMap2D &o)
128 {
129  freeMap();
130  resolution = o.resolution;
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();
140  m_voronoi_diagram.clear();
141 
142  precomputedLikelihoodToBeRecomputed = true;
143  m_is_empty=o.m_is_empty;
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 
159  ASSERT_(resolution>0)
160  ASSERT_(x_max>x_min && y_max>y_min)
161  ASSERT_(default_value>=0 && default_value<=1)
162 
163  // Liberar primero:
164  freeMap();
165 
166  // For the precomputed likelihood trick:
167  precomputedLikelihoodToBeRecomputed = true;
168 
169  // Adjust sizes to adapt them to full sized cells acording to the resolution:
170  x_min = resolution*round(x_min/resolution);
171  y_min = resolution*round(y_min/resolution);
172  x_max = resolution*round(x_max/resolution);
173  y_max = resolution*round(y_max/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:
183  size_x = round((x_max-x_min)/resolution);
184  size_y = round((y_max-y_min)/resolution);
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;
191  x_max = x_min + size_x * resolution;
192  }
193  size_x = round((x_max-x_min)/resolution);
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();
202  m_voronoi_diagram.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  ---------------------------------------------------------------*/
326 void COccupancyGridMap2D::freeMap()
327 {
328  MRPT_START
329 
330  // Free map and sectors
331  map.clear();
332 
333  m_basis_map.clear();
334  m_voronoi_diagram.clear();
335 
336  size_x=size_y=0;
337 
338  // For the precomputed likelihood trick:
339  precomputedLikelihoodToBeRecomputed = true;
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  ---------------------------------------------------------------*/
355 void COccupancyGridMap2D::computeEntropy( TEntropyInfo &info ) const
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  ---------------------------------------------------------------*/
435 void COccupancyGridMap2D::internal_clear()
436 {
437  setSize( -10,10,-10,10,getResolution());
438  //resetFeaturesCache();
439  // For the precomputed likelihood trick:
440  precomputedLikelihoodToBeRecomputed = true;
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:
452  precomputedLikelihoodToBeRecomputed = true;
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:
469  if ( updateInfoChangeOnly.enabled )
470  {
471  float old = l2p(theCell);
472  float new_v = 1 / ( 1 + (1-v)*(1-old)/(old*v) );
473  updateInfoChangeOnly.cellsUpdated++;
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 
528  setSize(x_min,x_max,y_min,y_max,resolution);
529  map = newMap;
530 
531 
532 }
533 
534 
535 
536 /*---------------------------------------------------------------
537  computeMatchingWith
538  ---------------------------------------------------------------*/
539 void COccupancyGridMap2D::determineMatching2D(
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 
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  ---------------------------------------------------------------*/
733 bool COccupancyGridMap2D::isEmpty() const
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 
771 float COccupancyGridMap2D::compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const
772 {
773  MRPT_UNUSED_PARAM(otherMap); MRPT_UNUSED_PARAM(otherMapPose);
774  MRPT_UNUSED_PARAM(params);
775  return 0;
776 }
777 
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
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)
mrpt::math::TPoint3D angularDistPivotPoint
The point used to calculate angular distances: e.g. the coordinates of the sensor for a 2D laser scan...
const GLdouble * v
Definition: glew.h:1296
float maxAngularDistForCorrespondence
Allowed "angular error" (in radians): this permits larger pairing threshold distances to more distant...
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 min(a, b)
EIGEN_STRONG_INLINE void fill(const Scalar v)
Definition: eigen_plugins.h:38
#define ASSERT_ABOVE_(__A, __B)
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
size_t decimation_other_map_points
(Default=1) Only consider 1 out of this number of points from the "other" map.
double effectiveMappedArea
The target variable for the area of cells with information, i.e. p(x)!=0.5.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
float resolution
See COccupancyGridMap2D::COccupancyGridMap2D.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
float resolution
Cell size, i.e. resolution of the grid map.
#define ASSERT_BELOW_(__A, __B)
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.
static const float MAX_H
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
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...
double H
The target variable for absolute entropy, computed as: H(map)=Sumx,y{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }
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.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
GLhandleARB obj
Definition: glew.h:3276
float maxDistForCorrespondence
Maximum linear distance between two points to be paired (meters)
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:28
GLfloat GLfloat p
Definition: glew.h:10113
mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOptions
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells) ...
double y
X,Y coordinates.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
A class for storing an occupancy grid map.
#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...
bool onlyKeepTheClosest
If set to true (default), only the closest correspondence will be returned. If false all are returned...
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
size_t offset_other_map_points
Index of the first point in the "other" map to start checking for correspondences (Default=0) ...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
unsigned long effectiveMappedCells
The mapped area in cells.
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
backing_store_ptr info
Definition: jmemsys.h:170
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.
#define ASSERT_(f)
GLfloat * params
Definition: glew.h:1436
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
Lightweight 3D point.
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
Parameters for the determination of matchings between point clouds, etc.
Used for returning entropy related information.
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
bool MAPS_IMPEXP operator<(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2)
int16_t cellType
The type of the map cells:
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
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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