Main MRPT website > C++ reference for MRPT 1.5.6
COccupancyGridMap2D_insert.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 #include <mrpt/maps/COccupancyGridMap2D.h>
13 #include <mrpt/obs/CObservation2DRangeScan.h>
14 #include <mrpt/obs/CObservationRange.h>
15 #include <mrpt/utils/CStream.h>
16 #include <mrpt/utils/round.h> // round()
17 
18 #if HAVE_ALLOCA_H
19 # include <alloca.h>
20 #endif
21 
22 using namespace mrpt;
23 using namespace mrpt::maps;
24 using namespace mrpt::obs;
25 using namespace mrpt::utils;
26 using namespace mrpt::poses;
27 using namespace std;
28 
29 /** Local stucture used in the next method (must be here for usage within STL stuff) */
30 struct TLocalPoint
31 {
32  float x,y; int cx, cy;
33 };
34 
35 /*---------------------------------------------------------------
36  insertObservation
37 
38 Insert the observation information into this map.
39  ---------------------------------------------------------------*/
40 bool COccupancyGridMap2D::internal_insertObservation(
41  const CObservation *obs,
42  const CPose3D *robotPose)
43 {
44 // MRPT_START // Avoid "try" since we use "alloca"
45 
46 #define FRBITS 9
47 
48  CPose2D robotPose2D;
49  CPose3D robotPose3D;
50 
51  // This is required to indicate the grid map has changed!
52  //resetFeaturesCache();
53  // For the precomputed likelihood trick:
54  precomputedLikelihoodToBeRecomputed = true;
55 
56  if (robotPose)
57  {
58  robotPose2D = CPose2D(*robotPose);
59  robotPose3D = (*robotPose);
60  }
61  else
62  {
63  // Default values are (0,0,0)
64  }
65 
66  // the occupied and free probabilities:
67  const float maxCertainty = insertionOptions.maxOccupancyUpdateCertainty;
68  float maxFreeCertainty = insertionOptions.maxFreenessUpdateCertainty;
69  if (maxFreeCertainty==.0f) maxFreeCertainty = maxCertainty;
70  float maxFreeCertaintyNoEcho = insertionOptions.maxFreenessInvalidRanges;
71  if (maxFreeCertaintyNoEcho==.0f) maxFreeCertaintyNoEcho = maxCertainty;
72 
73  cellType logodd_observation_free = std::max<cellType>(1, p2l(maxFreeCertainty));
74  cellType logodd_observation_occupied = 3*std::max<cellType>(1, p2l(maxCertainty));
75  cellType logodd_noecho_free = std::max<cellType>(1, p2l(maxFreeCertaintyNoEcho));
76 
77  // saturation limits:
78  cellType logodd_thres_occupied = OCCGRID_CELLTYPE_MIN+logodd_observation_occupied;
79  cellType logodd_thres_free = OCCGRID_CELLTYPE_MAX-std::max(logodd_noecho_free, logodd_observation_free);
80 
81 
82  if ( CLASS_ID(CObservation2DRangeScan)==obs->GetRuntimeClass())
83  {
84  /********************************************************************
85 
86  OBSERVATION TYPE: CObservation2DRangeScan
87 
88  ********************************************************************/
89  const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
90  CPose3D sensorPose3D = robotPose3D + o->sensorPose;
91  CPose2D laserPose( sensorPose3D );
92 
93  // Insert only HORIZONTAL scans, since the grid is supposed to
94  // be a horizontal representation of space.
95  bool reallyInsert = o->isPlanarScan( insertionOptions.horizontalTolerance );
96  unsigned int decimation = insertionOptions.decimation;
97 
98  // Check the altitude of the map (if feature enabled!)
99  if ( insertionOptions.useMapAltitude &&
100  fabs(insertionOptions.mapAltitude - sensorPose3D.z() ) > 0.001 )
101  {
102  reallyInsert = false;
103  }
104 
105  // Manage horizontal scans, but with the sensor bottom-up:
106  // Use the z-axis direction of the transformed Z axis of the sensor coordinates:
107  bool sensorIsBottomwards = sensorPose3D.getHomogeneousMatrixVal().get_unsafe(2,2) < 0;
108 
109  if ( reallyInsert )
110  {
111  // ---------------------------------------------
112  // Insert the scan as simple rays:
113  // ---------------------------------------------
114  int cx,cy,N = o->scan.size();
115  float px,py;
116  double A, dAK;
117 
118  // Parameters values:
119  const float maxDistanceInsertion = insertionOptions.maxDistanceInsertion;
120  const bool invalidAsFree = insertionOptions.considerInvalidRangesAsFreeSpace;
121  float new_x_max, new_x_min;
122  float new_y_max, new_y_min;
123  float last_valid_range = maxDistanceInsertion;
124 
125  int K = updateInfoChangeOnly.enabled ? updateInfoChangeOnly.laserRaysSkip : decimation;
126  size_t idx,nRanges = o->scan.size();
127  float curRange=0;
128 
129  // Start position:
130  px = laserPose.x();
131  py = laserPose.y();
132 
133 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
134  MRPT_CHECK_NORMAL_NUMBER(px);
135  MRPT_CHECK_NORMAL_NUMBER(py);
136 #endif
137 
138  // Here we go! Now really insert changes in the grid:
139  if ( !insertionOptions.wideningBeamsWithDistance )
140  {
141  // Method: Simple rays:
142  // -------------------------------------
143 
144  // Reserve a temporary block of memory on the stack with "alloca": this memory has NOT to be deallocated,
145  // so it's ideal for an efficient, small buffer:
146  float *scanPoints_x = (float*) mrpt_alloca( sizeof(float) * nRanges );
147  float *scanPoints_y = (float*) mrpt_alloca( sizeof(float) * nRanges );
148 
149  float *scanPoint_x,*scanPoint_y;
150 
151 
152  if (o->rightToLeft ^ sensorIsBottomwards )
153  {
154  A = laserPose.phi() - 0.5 * o->aperture;
155  dAK = K* o->aperture / N;
156  }
157  else
158  {
159  A = laserPose.phi() + 0.5 * o->aperture;
160  dAK = - K*o->aperture / N;
161  }
162 
163 
164  new_x_max = -(numeric_limits<float>::max)();
165  new_x_min = (numeric_limits<float>::max)();
166  new_y_max = -(numeric_limits<float>::max)();
167  new_y_min = (numeric_limits<float>::max)();
168 
169  for (idx=0, scanPoint_x=scanPoints_x,scanPoint_y=scanPoints_y;idx<nRanges;idx+=K,scanPoint_x++,scanPoint_y++)
170  {
171  if ( o->validRange[idx] )
172  {
173  curRange = o->scan[idx];
174  float R = min(maxDistanceInsertion,curRange);
175 
176  *scanPoint_x = px + cos(A)* R;
177  *scanPoint_y = py + sin(A)* R;
178  last_valid_range = curRange;
179  }
180  else
181  {
182  if (invalidAsFree)
183  {
184  // Invalid range:
185  float R = min(maxDistanceInsertion,0.5f*last_valid_range);
186  *scanPoint_x = px + cos(A)* R;
187  *scanPoint_y = py + sin(A)* R;
188  }
189  else
190  {
191  *scanPoint_x = px;
192  *scanPoint_y = py;
193  }
194  }
195  A+=dAK;
196 
197  // Asjust size (will not change if not required):
198  new_x_max = max( new_x_max, *scanPoint_x );
199  new_x_min = min( new_x_min, *scanPoint_x );
200  new_y_max = max( new_y_max, *scanPoint_y );
201  new_y_min = min( new_y_min, *scanPoint_y );
202  }
203 
204  // Add an extra margin:
205  float securMargen = 15*resolution;
206 
207  if (new_x_max>x_max-securMargen)
208  new_x_max+= 2*securMargen;
209  else new_x_max = x_max;
210  if (new_x_min<x_min+securMargen)
211  new_x_min-= 2;
212  else new_x_min = x_min;
213 
214  if (new_y_max>y_max-securMargen)
215  new_y_max+= 2*securMargen;
216  else new_y_max = y_max;
217  if (new_y_min<y_min+securMargen)
218  new_y_min-= 2;
219  else new_y_min = y_min;
220 
221  // -----------------------
222  // Resize to make room:
223  // -----------------------
224  resizeGrid(new_x_min,new_x_max, new_y_min,new_y_max,0.5);
225 
226  // For updateCell_fast methods:
227  cellType *theMapArray = &map[0];
228  unsigned theMapSize_x = size_x;
229 
230  int cx0 = x2idx(px); // Remember: This must be after the resizeGrid!!
231  int cy0 = y2idx(py);
232 
233 
234  // Insert rays:
235  for (idx=0;idx<nRanges;idx+=K)
236  {
237  if ( !o->validRange[idx] && !invalidAsFree ) continue;
238 
239  // Starting position: Laser position
240  cx = cx0;
241  cy = cy0;
242 
243  // Target, in cell indexes:
244  int trg_cx = x2idx(scanPoints_x[idx]);
245  int trg_cy = y2idx(scanPoints_y[idx]);
246 
247  // The x> comparison implicitly holds if x<0
248  ASSERT_(
249  static_cast<unsigned int>(trg_cx) < size_x &&
250  static_cast<unsigned int>(trg_cy) < size_y);
251 
252  // Use "fractional integers" to approximate float operations
253  // during the ray tracing:
254  int Acx = trg_cx - cx;
255  int Acy = trg_cy - cy;
256 
257  int Acx_ = abs(Acx);
258  int Acy_ = abs(Acy);
259 
260  int nStepsRay = max( Acx_, Acy_ );
261  if (!nStepsRay) continue; // May be...
262 
263  // Integers store "float values * 128"
264  float N_1 = 1.0f / nStepsRay; // Avoid division twice.
265 
266  // Increments at each raytracing step:
267  int frAcx = (Acx<0 ? -1:+1)*round((Acx_ << FRBITS) * N_1);
268  int frAcy = (Acy<0 ? -1:+1)*round((Acy_ << FRBITS) * N_1);
269 
270  int frCX = cx << FRBITS;
271  int frCY = cy << FRBITS;
272  const auto logodd_free = o->validRange[idx] ? logodd_observation_free : logodd_noecho_free;
273 
274  for (int nStep = 0;nStep<nStepsRay;nStep++)
275  {
276  updateCell_fast_free(cx,cy, logodd_free, logodd_thres_free, theMapArray, theMapSize_x );
277 
278  frCX += frAcx;
279  frCY += frAcy;
280 
281  cx = frCX >> FRBITS;
282  cy = frCY >> FRBITS;
283  }
284 
285  // And finally, the occupied cell at the end:
286  // Only if:
287  // - It was a valid ray, and
288  // - The ray was not truncated
289  if ( o->validRange[idx] && o->scan[idx]<maxDistanceInsertion )
290  updateCell_fast_occupied(trg_cx,trg_cy, logodd_observation_occupied, logodd_thres_occupied, theMapArray, theMapSize_x );
291 
292  } // End of each range
293 
294  mrpt_alloca_free( scanPoints_x );
295  mrpt_alloca_free( scanPoints_y );
296 
297  } // end insert with simple rays
298  else
299  {
300  // ---------------------------------
301  // Widen rays
302  // Algorithm in: http://www.mrpt.org/Occupancy_Grids
303  // ---------------------------------
304  if (o->rightToLeft ^ sensorIsBottomwards )
305  {
306  A = laserPose.phi() - 0.5 * o->aperture;
307  dAK = K* o->aperture / N;
308  }
309  else
310  {
311  A = laserPose.phi() + 0.5 * o->aperture;
312  dAK = - K*o->aperture / N;
313  }
314 
315  new_x_max = -(numeric_limits<float>::max)();
316  new_x_min = (numeric_limits<float>::max)();
317  new_y_max = -(numeric_limits<float>::max)();
318  new_y_min = (numeric_limits<float>::max)();
319 
320  last_valid_range = maxDistanceInsertion;
321  for (idx=0;idx<nRanges;idx+=K)
322  {
323  float scanPoint_x,scanPoint_y;
324  if ( o->validRange[idx] )
325  {
326  curRange = o->scan[idx];
327  float R = min(maxDistanceInsertion,curRange);
328 
329  scanPoint_x = px + cos(A)* R;
330  scanPoint_y = py + sin(A)* R;
331  last_valid_range = curRange;
332  }
333  else
334  {
335  if (invalidAsFree)
336  {
337  // Invalid range:
338  float R = min(maxDistanceInsertion,0.5f*last_valid_range);
339  scanPoint_x = px + cos(A)* R;
340  scanPoint_y = py + sin(A)* R;
341  }
342  else
343  {
344  scanPoint_x = px;
345  scanPoint_y = py;
346  }
347  }
348  A+=dAK;
349 
350  // Asjust size (will not change if not required):
351  new_x_max = max( new_x_max, scanPoint_x );
352  new_x_min = min( new_x_min, scanPoint_x );
353  new_y_max = max( new_y_max, scanPoint_y );
354  new_y_min = min( new_y_min, scanPoint_y );
355  }
356 
357  // Add an extra margin:
358  float securMargen = 15*resolution;
359 
360  if (new_x_max>x_max-securMargen)
361  new_x_max+= 2*securMargen;
362  else new_x_max = x_max;
363  if (new_x_min<x_min+securMargen)
364  new_x_min-= 2;
365  else new_x_min = x_min;
366 
367  if (new_y_max>y_max-securMargen)
368  new_y_max+= 2*securMargen;
369  else new_y_max = y_max;
370  if (new_y_min<y_min+securMargen)
371  new_y_min-= 2;
372  else new_y_min = y_min;
373 
374  // -----------------------
375  // Resize to make room:
376  // -----------------------
377  resizeGrid(new_x_min,new_x_max, new_y_min,new_y_max,0.5);
378 
379  // For updateCell_fast methods:
380  cellType *theMapArray = &map[0];
381  unsigned theMapSize_x = size_x;
382 
383  //int cx0 = x2idx(px); // Remember: This must be after the resizeGrid!!
384  //int cy0 = y2idx(py);
385 
386 
387  // Now go and insert the triangles of each beam:
388  // -----------------------------------------------
389  if (o->rightToLeft ^ sensorIsBottomwards )
390  {
391  A = laserPose.phi() - 0.5 * o->aperture;
392  dAK = K* o->aperture / N;
393  }
394  else
395  {
396  A = laserPose.phi() + 0.5 * o->aperture;
397  dAK = - K*o->aperture / N;
398  }
399 
400  // Insert the rays:
401  // ------------------------------------------
402  // Vertices of the triangle: In meters
403  TLocalPoint P0,P1,P2, P1b;
404 
405  last_valid_range = maxDistanceInsertion;
406 
407  const double dA_2 = 0.5 * o->aperture / N;
408  for (idx=0;idx<nRanges; idx+=K, A+=dAK)
409  {
410  float theR; // The range of this beam
411  if ( o->validRange[idx] )
412  {
413  curRange = o->scan[idx];
414  last_valid_range = curRange;
415  theR = min(maxDistanceInsertion,curRange);
416  }
417  else
418  {
419  // Invalid range:
420  if (invalidAsFree)
421  {
422  theR = min(maxDistanceInsertion,0.5f*last_valid_range);
423  }
424  else continue; // Nothing to do
425  }
426  if (theR < resolution) continue; // Range must be larger than a cell...
427  theR -= resolution; // Remove one cell of length, which will be filled with "occupied" later.
428 
429  /* ---------------------------------------------------------
430  Fill one triangle with vertices: P0,P1,P2
431  --------------------------------------------------------- */
432  P0.x = px;
433  P0.y = py;
434 
435  P1.x = px + cos(A-dA_2) * theR;
436  P1.y = py + sin(A-dA_2) * theR;
437 
438  P2.x = px + cos(A+dA_2) * theR;
439  P2.y = py + sin(A+dA_2) * theR;
440 
441  // Order the vertices by the "y": P0->bottom, P2: top
442  if (P2.y<P1.y) std::swap(P2,P1);
443  if (P2.y<P0.y) std::swap(P2,P0);
444  if (P1.y<P0.y) std::swap(P1,P0);
445 
446 
447  // In cell indexes:
448  P0.cx = x2idx( P0.x ); P0.cy = y2idx( P0.y );
449  P1.cx = x2idx( P1.x ); P1.cy = y2idx( P1.y );
450  P2.cx = x2idx( P2.x ); P2.cy = y2idx( P2.y );
451 
452  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
453  // The x> comparison implicitly holds if x<0
454  ASSERT_( static_cast<unsigned int>(P0.cx)<size_x && static_cast<unsigned int>(P0.cy)<size_y );
455  ASSERT_( static_cast<unsigned int>(P1.cx)<size_x && static_cast<unsigned int>(P1.cy)<size_y );
456  ASSERT_( static_cast<unsigned int>(P2.cx)<size_x && static_cast<unsigned int>(P2.cy)<size_y );
457  #endif
458 
459  struct { int frX,frY; int cx,cy; } R1,R2; // Fractional coords of the two rays:
460 
461  // Special case: one single row
462  if (P0.cy==P2.cy && P0.cy==P1.cy)
463  {
464  // Optimized case:
465  int min_cx = min3(P0.cx,P1.cx,P2.cx);
466  int max_cx = max3(P0.cx,P1.cx,P2.cx);
467 
468  for (int ccx=min_cx;ccx<=max_cx;ccx++)
469  updateCell_fast_free(ccx,P0.cy, logodd_observation_free, logodd_thres_free, theMapArray, theMapSize_x );
470  }
471  else
472  {
473  // The intersection point P1b in the segment P0-P2 at the "y" of P1:
474  P1b.y = P1.y;
475  P1b.x = P0.x + (P1.y-P0.y) * (P2.x-P0.x) / (P2.y-P0.y);
476 
477  P1b.cx= x2idx( P1b.x ); P1b.cy= y2idx( P1b.y );
478 
479 
480  // Use "fractional integers" to approximate float operations during the ray tracing:
481  // Integers store "float values * 128"
482  const int Acx01 = P1.cx - P0.cx;
483  const int Acy01 = P1.cy - P0.cy;
484  const int Acx01b = P1b.cx - P0.cx;
485  //const int Acy01b = P1b.cy - P0.cy; // = Acy01
486 
487  // Increments at each raytracing step:
488  const float inv_N_01 = 1.0f / ( max3(abs(Acx01),abs(Acy01),abs(Acx01b)) + 1 ); // Number of steps ^ -1
489  const int frAcx01 = round( (Acx01<< FRBITS) * inv_N_01 ); // Acx*128 / N
490  const int frAcy01 = round( (Acy01<< FRBITS) * inv_N_01 ); // Acy*128 / N
491  const int frAcx01b = round((Acx01b<< FRBITS)* inv_N_01 ); // Acx*128 / N
492 
493  // ------------------------------------
494  // First sub-triangle: P0-P1-P1b
495  // ------------------------------------
496  R1.cx = P0.cx;
497  R1.cy = P0.cy;
498  R1.frX = P0.cx << FRBITS;
499  R1.frY = P0.cy << FRBITS;
500 
501  int frAx_R1=0, frAx_R2=0; //, frAy_R2;
502  int frAy_R1 = frAcy01;
503 
504  // Start R1=R2 = P0... unlesss P0.cy == P1.cy, i.e. there is only one row:
505  if (P0.cy!=P1.cy)
506  {
507  R2 = R1;
508  // R1 & R2 follow the edges: P0->P1 & P0->P1b
509  // R1 is forced to be at the left hand:
510  if (P1.x<P1b.x)
511  {
512  // R1: P0->P1
513  frAx_R1 = frAcx01;
514  frAx_R2 = frAcx01b;
515  }
516  else
517  {
518  // R1: P0->P1b
519  frAx_R1 = frAcx01b;
520  frAx_R2 = frAcx01;
521  }
522  }
523  else
524  {
525  R2.cx = P1.cx;
526  R2.cy = P1.cy;
527  R2.frX = P1.cx << FRBITS;
528  //R2.frY = P1.cy << FRBITS;
529  }
530 
531  int last_insert_cy = -1;
532  //int last_insert_cx = -1;
533  do
534  {
535  if (last_insert_cy!=R1.cy) // || last_insert_cx!=R1.cx)
536  {
537  last_insert_cy = R1.cy;
538  // last_insert_cx = R1.cx;
539 
540  for (int ccx=R1.cx;ccx<=R2.cx;ccx++)
541  updateCell_fast_free(ccx,R1.cy, logodd_observation_free, logodd_thres_free, theMapArray, theMapSize_x );
542  }
543 
544  R1.frX += frAx_R1; R1.frY += frAy_R1;
545  R2.frX += frAx_R2; // R1.frY += frAcy01;
546 
547  R1.cx = R1.frX >> FRBITS;
548  R1.cy = R1.frY >> FRBITS;
549  R2.cx = R2.frX >> FRBITS;
550  } while ( R1.cy < P1.cy );
551 
552  // ------------------------------------
553  // Second sub-triangle: P1-P1b-P2
554  // ------------------------------------
555 
556  // Use "fractional integers" to approximate float operations during the ray tracing:
557  // Integers store "float values * 128"
558  const int Acx12 = P2.cx - P1.cx;
559  const int Acy12 = P2.cy - P1.cy;
560  const int Acx1b2 = P2.cx - P1b.cx;
561  //const int Acy1b2 = Acy12
562 
563  // Increments at each raytracing step:
564  const float inv_N_12 = 1.0f / ( max3(abs(Acx12),abs(Acy12),abs(Acx1b2)) + 1 ); // Number of steps ^ -1
565  const int frAcx12 = round( (Acx12<< FRBITS) * inv_N_12 ); // Acx*128 / N
566  const int frAcy12 = round( (Acy12<< FRBITS) * inv_N_12 ); // Acy*128 / N
567  const int frAcx1b2 = round((Acx1b2<< FRBITS)* inv_N_12 ); // Acx*128 / N
568 
569  //struct { int frX,frY; int cx,cy; } R1,R2; // Fractional coords of the two rays:
570  // R1, R2 follow edges P1->P2 & P1b->P2
571  // R1 forced to be at the left hand
572  frAy_R1 = frAcy12;
573  if (!frAy_R1)
574  frAy_R1 = 2 << FRBITS; // If Ay=0, force it to be >0 so the "do...while" loop below ends in ONE iteration.
575 
576  if (P1.x<P1b.x)
577  {
578  // R1: P1->P2, R2: P1b->P2
579  R1.cx = P1.cx;
580  R1.cy = P1.cy;
581  R2.cx = P1b.cx;
582  R2.cy = P1b.cy;
583  frAx_R1 = frAcx12;
584  frAx_R2 = frAcx1b2;
585  }
586  else
587  {
588  // R1: P1b->P2, R2: P1->P2
589  R1.cx = P1b.cx;
590  R1.cy = P1b.cy;
591  R2.cx = P1.cx;
592  R2.cy = P1.cy;
593  frAx_R1 = frAcx1b2;
594  frAx_R2 = frAcx12;
595  }
596 
597  R1.frX = R1.cx << FRBITS;
598  R1.frY = R1.cy << FRBITS;
599  R2.frX = R2.cx << FRBITS;
600  R2.frY = R2.cy << FRBITS;
601 
602  last_insert_cy=-100;
603  //last_insert_cx=-100;
604 
605  do
606  {
607  if (last_insert_cy!=R1.cy) // || last_insert_cx!=R1.cx)
608  {
609  // last_insert_cx = R1.cx;
610  last_insert_cy = R1.cy;
611  for (int ccx=R1.cx;ccx<=R2.cx;ccx++)
612  updateCell_fast_free(ccx,R1.cy, logodd_observation_free, logodd_thres_free, theMapArray, theMapSize_x );
613  }
614 
615  R1.frX += frAx_R1; R1.frY += frAy_R1;
616  R2.frX += frAx_R2; // R1.frY += frAcy01;
617 
618  R1.cx = R1.frX >> FRBITS;
619  R1.cy = R1.frY >> FRBITS;
620  R2.cx = R2.frX >> FRBITS;
621  } while ( R1.cy <= P2.cy );
622 
623  } // end of free-area normal case (not a single row)
624 
625  // ----------------------------------------------------
626  // The final occupied cells along the edge P1<->P2
627  // Only if:
628  // - It was a valid ray, and
629  // - The ray was not truncated
630  // ----------------------------------------------------
631  if ( o->validRange[idx] && o->scan[idx]<maxDistanceInsertion )
632  {
633  theR += resolution;
634 
635  P1.x = px + cos(A-dA_2) * theR;
636  P1.y = py + sin(A-dA_2) * theR;
637 
638  P2.x = px + cos(A+dA_2) * theR;
639  P2.y = py + sin(A+dA_2) * theR;
640 
641  P1.cx = x2idx( P1.x ); P1.cy = y2idx( P1.y );
642  P2.cx = x2idx( P2.x ); P2.cy = y2idx( P2.y );
643 
644  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
645  // The x> comparison implicitly holds if x<0
646  ASSERT_( static_cast<unsigned int>(P1.cx)<size_x && static_cast<unsigned int>(P1.cy)<size_y );
647  ASSERT_( static_cast<unsigned int>(P2.cx)<size_x && static_cast<unsigned int>(P2.cy)<size_y );
648  #endif
649 
650  // Special case: Only one cell:
651  if (P2.cx==P1.cx && P2.cy==P1.cy)
652  {
653  updateCell_fast_occupied(P1.cx,P1.cy, logodd_observation_occupied, logodd_thres_occupied, theMapArray, theMapSize_x );
654  }
655  else
656  {
657  // Use "fractional integers" to approximate float operations during the ray tracing:
658  // Integers store "float values * 128"
659  const int AcxE = P2.cx - P1.cx;
660  const int AcyE = P2.cy - P1.cy;
661 
662  // Increments at each raytracing step:
663  const int nSteps = ( max(abs(AcxE),abs(AcyE)) + 1 );
664  const float inv_N_12 = 1.0f / nSteps; // Number of steps ^ -1
665  const int frAcxE = round( (AcxE<< FRBITS) * inv_N_12 ); // Acx*128 / N
666  const int frAcyE = round( (AcyE<< FRBITS) * inv_N_12 ); // Acy*128 / N
667 
668  R1.cx = P1.cx;
669  R1.cy = P1.cy;
670  R1.frX = R1.cx << FRBITS;
671  R1.frY = R1.cy << FRBITS;
672 
673  for (int nStep=0;nStep<=nSteps;nStep++)
674  {
675  updateCell_fast_occupied(R1.cx,R1.cy, logodd_observation_occupied, logodd_thres_occupied, theMapArray, theMapSize_x );
676 
677  R1.frX += frAcxE;
678  R1.frY += frAcyE;
679  R1.cx = R1.frX >> FRBITS;
680  R1.cy = R1.frY >> FRBITS;
681  }
682 
683  } // end do a line
684 
685  } // end if we must set occupied cells
686 
687  } // End of each range
688 
689  } // end insert with beam widening
690 
691  // Finished:
692  return true;
693  }
694  else
695  {
696  // A non-horizontal scan:
697  return false;
698  }
699  }
700  else if ( CLASS_ID(CObservationRange)==obs->GetRuntimeClass())
701  {
702  const CObservationRange *o = static_cast<const CObservationRange*>( obs );
703  CPose3D spose;
704  o->getSensorPose(spose);
705  CPose3D sensorPose3D = robotPose3D + spose;
706  CPose2D laserPose( sensorPose3D );
707 
708  // Insert only HORIZONTAL scans, since the grid is supposed to
709  // be a horizontal representation of space.
710  bool reallyInsert = true;
711  unsigned int decimation = insertionOptions.decimation;
712 
713  // Check the altitude of the map (if feature enabled!)
714  if ( insertionOptions.useMapAltitude &&
715  fabs(insertionOptions.mapAltitude - sensorPose3D.z() ) > 0.001 )
716  {
717  reallyInsert = false;
718  }
719  if ( reallyInsert )
720  {
721  // ---------------------------------------------
722  // Insert the scan as simple rays:
723  // ---------------------------------------------
724 
725  //int /*cx,cy,*/ N = o->sensedData.size();
726  float px,py;
727  double A, dAK;
728 
729  // Parameters values:
730  const float maxDistanceInsertion = insertionOptions.maxDistanceInsertion;
731  const bool invalidAsFree = insertionOptions.considerInvalidRangesAsFreeSpace;
732  float new_x_max, new_x_min;
733  float new_y_max, new_y_min;
734  float last_valid_range = maxDistanceInsertion;
735 
736  int K = updateInfoChangeOnly.enabled ? updateInfoChangeOnly.laserRaysSkip : decimation;
737  size_t idx,nRanges = o->sensedData.size();
738  float curRange=0;
739 
740  // Start position:
741  px = laserPose.x();
742  py = laserPose.y();
743 
744 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
745  MRPT_CHECK_NORMAL_NUMBER(px);
746  MRPT_CHECK_NORMAL_NUMBER(py);
747 #endif
748  // ---------------------------------
749  // Widen rays
750  // Algorithm in: http://www.mrpt.org/Occupancy_Grids
751  // FIXME: doesn't support many different poses in one measurement
752  // ---------------------------------
753  A = laserPose.phi();
754  dAK = 0;
755 
756  new_x_max = -(numeric_limits<float>::max)();
757  new_x_min = (numeric_limits<float>::max)();
758  new_y_max = -(numeric_limits<float>::max)();
759  new_y_min = (numeric_limits<float>::max)();
760 
761  last_valid_range = maxDistanceInsertion;
762 
763  for (idx=0;idx<nRanges;idx+=K)
764  {
765  float scanPoint_x,scanPoint_y;
766  if ( o->sensedData[idx].sensedDistance < maxDistanceInsertion )
767  {
768  curRange = o->sensedData[idx].sensedDistance;
769  float R = min(maxDistanceInsertion,curRange);
770 
771  scanPoint_x = px + cos(A)* R;
772  scanPoint_y = py + sin(A)* R;
773  last_valid_range = curRange;
774  }
775  else
776  {
777  if (invalidAsFree)
778  {
779  // Invalid range:
780  float R = min(maxDistanceInsertion,0.5f*last_valid_range);
781  scanPoint_x = px + cos(A)* R;
782  scanPoint_y = py + sin(A)* R;
783  }
784  else
785  {
786  scanPoint_x = px;
787  scanPoint_y = py;
788  }
789  }
790  A+=dAK;
791 
792  // Asjust size (will not change if not required):
793  new_x_max = max( new_x_max, scanPoint_x );
794  new_x_min = min( new_x_min, scanPoint_x );
795  new_y_max = max( new_y_max, scanPoint_y );
796  new_y_min = min( new_y_min, scanPoint_y );
797  }
798 
799  // Add an extra margin:
800  float securMargen = 15*resolution;
801 
802  if (new_x_max>x_max-securMargen)
803  new_x_max+= 2*securMargen;
804  else new_x_max = x_max;
805  if (new_x_min<x_min+securMargen)
806  new_x_min-= 2;
807  else new_x_min = x_min;
808 
809  if (new_y_max>y_max-securMargen)
810  new_y_max+= 2*securMargen;
811  else new_y_max = y_max;
812  if (new_y_min<y_min+securMargen)
813  new_y_min-= 2;
814  else new_y_min = y_min;
815 
816  // -----------------------
817  // Resize to make room:
818  // -----------------------
819  resizeGrid(new_x_min,new_x_max, new_y_min,new_y_max,0.5);
820 
821  // For updateCell_fast methods:
822  cellType *theMapArray = &map[0];
823  unsigned theMapSize_x = size_x;
824 
825  //int cx0 = x2idx(px); // Remember: This must be after the resizeGrid!!
826  //int cy0 = y2idx(py);
827 
828 
829  // Now go and insert the triangles of each beam:
830  // -----------------------------------------------
831  A = laserPose.phi() - 0.5 * o->sensorConeApperture;
832  dAK = 0;
833 
834  // Insert the rays:
835  // ------------------------------------------
836  // Vertices of the triangle: In meters
837  TLocalPoint P0,P1,P2, P1b;
838 
839  last_valid_range = maxDistanceInsertion;
840 
841  const double dA_2 = 0.5 * o->sensorConeApperture;
842  for (idx=0;idx<nRanges; idx+=K, A+=dAK)
843  {
844  float theR; // The range of this beam
845  if ( o->sensedData[idx].sensedDistance < maxDistanceInsertion )
846  {
847  curRange = o->sensedData[idx].sensedDistance;
848  last_valid_range = curRange;
849  theR = min(maxDistanceInsertion,curRange);
850  }
851  else
852  {
853  // Invalid range:
854  if (invalidAsFree)
855  {
856  theR = min(maxDistanceInsertion,0.5f*last_valid_range);
857  }
858  else continue; // Nothing to do
859  }
860  if (theR < resolution) continue; // Range must be larger than a cell...
861  theR -= resolution; // Remove one cell of length, which will be filled with "occupied" later.
862 
863  /* ---------------------------------------------------------
864  Fill one triangle with vertices: P0,P1,P2
865  --------------------------------------------------------- */
866  P0.x = px;
867  P0.y = py;
868 
869  P1.x = px + cos(A-dA_2) * theR;
870  P1.y = py + sin(A-dA_2) * theR;
871 
872  P2.x = px + cos(A+dA_2) * theR;
873  P2.y = py + sin(A+dA_2) * theR;
874 
875  // Order the vertices by the "y": P0->bottom, P2: top
876  if (P2.y<P1.y) std::swap(P2,P1);
877  if (P2.y<P0.y) std::swap(P2,P0);
878  if (P1.y<P0.y) std::swap(P1,P0);
879 
880 
881  // In cell indexes:
882  P0.cx = x2idx( P0.x ); P0.cy = y2idx( P0.y );
883  P1.cx = x2idx( P1.x ); P1.cy = y2idx( P1.y );
884  P2.cx = x2idx( P2.x ); P2.cy = y2idx( P2.y );
885 
886 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
887  // The x> comparison implicitly holds if x<0
888  ASSERT_( static_cast<unsigned int>(P0.cx)<size_x && static_cast<unsigned int>(P0.cy)<size_y );
889  ASSERT_( static_cast<unsigned int>(P1.cx)<size_x && static_cast<unsigned int>(P1.cy)<size_y );
890  ASSERT_( static_cast<unsigned int>(P2.cx)<size_x && static_cast<unsigned int>(P2.cy)<size_y );
891 #endif
892 
893  struct { int frX,frY; int cx,cy; } R1,R2; // Fractional coords of the two rays:
894 
895  // Special case: one single row
896  if (P0.cy==P2.cy && P0.cy==P1.cy)
897  {
898  // Optimized case:
899  int min_cx = min3(P0.cx,P1.cx,P2.cx);
900  int max_cx = max3(P0.cx,P1.cx,P2.cx);
901 
902  for (int ccx=min_cx;ccx<=max_cx;ccx++)
903  updateCell_fast_free(ccx,P0.cy, logodd_observation_free, logodd_thres_free, theMapArray, theMapSize_x );
904  }
905  else
906  {
907  // The intersection point P1b in the segment P0-P2 at the "y" of P1:
908  P1b.y = P1.y;
909  P1b.x = P0.x + (P1.y-P0.y) * (P2.x-P0.x) / (P2.y-P0.y);
910 
911  P1b.cx= x2idx( P1b.x ); P1b.cy= y2idx( P1b.y );
912 
913 
914  // Use "fractional integers" to approximate float operations during the ray tracing:
915  // Integers store "float values * 128"
916  const int Acx01 = P1.cx - P0.cx;
917  const int Acy01 = P1.cy - P0.cy;
918  const int Acx01b = P1b.cx - P0.cx;
919  //const int Acy01b = P1b.cy - P0.cy; // = Acy01
920 
921  // Increments at each raytracing step:
922  const float inv_N_01 = 1.0f / ( max3(abs(Acx01),abs(Acy01),abs(Acx01b)) + 1 ); // Number of steps ^ -1
923  const int frAcx01 = round( (Acx01<<FRBITS) * inv_N_01 ); // Acx*128 / N
924  const int frAcy01 = round( (Acy01<<FRBITS) * inv_N_01 ); // Acy*128 / N
925  const int frAcx01b = round((Acx01b<<FRBITS)* inv_N_01 ); // Acx*128 / N
926 
927  // ------------------------------------
928  // First sub-triangle: P0-P1-P1b
929  // ------------------------------------
930  R1.cx = P0.cx;
931  R1.cy = P0.cy;
932  R1.frX = P0.cx <<FRBITS;
933  R1.frY = P0.cy <<FRBITS;
934 
935  int frAx_R1=0, frAx_R2=0; //, frAy_R2;
936  int frAy_R1 = frAcy01;
937 
938  // Start R1=R2 = P0... unlesss P0.cy == P1.cy, i.e. there is only one row:
939  if (P0.cy!=P1.cy)
940  {
941  R2 = R1;
942  // R1 & R2 follow the edges: P0->P1 & P0->P1b
943  // R1 is forced to be at the left hand:
944  if (P1.x<P1b.x)
945  {
946  // R1: P0->P1
947  frAx_R1 = frAcx01;
948  frAx_R2 = frAcx01b;
949  }
950  else
951  {
952  // R1: P0->P1b
953  frAx_R1 = frAcx01b;
954  frAx_R2 = frAcx01;
955  }
956  }
957  else
958  {
959  R2.cx = P1.cx;
960  R2.cy = P1.cy;
961  R2.frX = P1.cx <<FRBITS;
962  //R2.frY = P1.cy <<FRBITS;
963  }
964  int last_insert_cy = -1;
965  //int last_insert_cx = -1;
966  do
967  {
968  if (last_insert_cy!=R1.cy) // || last_insert_cx!=R1.cx)
969  {
970  last_insert_cy = R1.cy;
971  // last_insert_cx = R1.cx;
972 
973  for (int ccx=R1.cx;ccx<=R2.cx;ccx++)
974  updateCell_fast_free(ccx,R1.cy, logodd_observation_free, logodd_thres_free, theMapArray, theMapSize_x );
975  }
976 
977  R1.frX += frAx_R1; R1.frY += frAy_R1;
978  R2.frX += frAx_R2; // R1.frY += frAcy01;
979 
980  R1.cx = R1.frX >> FRBITS;
981  R1.cy = R1.frY >> FRBITS;
982  R2.cx = R2.frX >> FRBITS;
983  } while ( R1.cy < P1.cy );
984  // ------------------------------------
985  // Second sub-triangle: P1-P1b-P2
986  // ------------------------------------
987 
988  // Use "fractional integers" to approximate float operations during the ray tracing:
989  // Integers store "float values * 128"
990  const int Acx12 = P2.cx - P1.cx;
991  const int Acy12 = P2.cy - P1.cy;
992  const int Acx1b2 = P2.cx - P1b.cx;
993  //const int Acy1b2 = Acy12
994 
995  // Increments at each raytracing step:
996  const float inv_N_12 = 1.0f / ( max3(abs(Acx12),abs(Acy12),abs(Acx1b2)) + 1 ); // Number of steps ^ -1
997  const int frAcx12 = round( (Acx12<<FRBITS) * inv_N_12 ); // Acx*128 / N
998  const int frAcy12 = round( (Acy12<<FRBITS) * inv_N_12 ); // Acy*128 / N
999  const int frAcx1b2 = round((Acx1b2<<FRBITS)* inv_N_12 ); // Acx*128 / N
1000 
1001  //struct { int frX,frY; int cx,cy; } R1,R2; // Fractional coords of the two rays:
1002  // R1, R2 follow edges P1->P2 & P1b->P2
1003  // R1 forced to be at the left hand
1004  frAy_R1 = frAcy12;
1005  if (!frAy_R1)
1006  frAy_R1 = 2 <<FRBITS; // If Ay=0, force it to be >0 so the "do...while" loop below ends in ONE iteration.
1007 
1008  if (P1.x<P1b.x)
1009  {
1010  // R1: P1->P2, R2: P1b->P2
1011  R1.cx = P1.cx;
1012  R1.cy = P1.cy;
1013  R2.cx = P1b.cx;
1014  R2.cy = P1b.cy;
1015  frAx_R1 = frAcx12;
1016  frAx_R2 = frAcx1b2;
1017  }
1018  else
1019  {
1020  // R1: P1b->P2, R2: P1->P2
1021  R1.cx = P1b.cx;
1022  R1.cy = P1b.cy;
1023  R2.cx = P1.cx;
1024  R2.cy = P1.cy;
1025  frAx_R1 = frAcx1b2;
1026  frAx_R2 = frAcx12;
1027  }
1028 
1029  R1.frX = R1.cx <<FRBITS;
1030  R1.frY = R1.cy <<FRBITS;
1031  R2.frX = R2.cx <<FRBITS;
1032  R2.frY = R2.cy <<FRBITS;
1033 
1034  last_insert_cy=-100;
1035  //last_insert_cx=-100;
1036 
1037  do
1038  {
1039  if (last_insert_cy!=R1.cy) // || last_insert_cx!=R1.cx)
1040  {
1041  // last_insert_cx = R1.cx;
1042  last_insert_cy = R1.cy;
1043  for (int ccx=R1.cx;ccx<=R2.cx;ccx++)
1044  updateCell_fast_free(ccx,R1.cy, logodd_observation_free, logodd_thres_free, theMapArray, theMapSize_x );
1045  }
1046 
1047  R1.frX += frAx_R1; R1.frY += frAy_R1;
1048  R2.frX += frAx_R2; // R1.frY += frAcy01;
1049 
1050  R1.cx = R1.frX >> FRBITS;
1051  R1.cy = R1.frY >> FRBITS;
1052  R2.cx = R2.frX >> FRBITS;
1053  } while ( R1.cy <= P2.cy );
1054 
1055  } // end of free-area normal case (not a single row)
1056 
1057  // ----------------------------------------------------
1058  // The final occupied cells along the edge P1<->P2
1059  // Only if:
1060  // - It was a valid ray, and
1061  // - The ray was not truncated
1062  // ----------------------------------------------------
1063  if ( o->sensedData[idx].sensedDistance < maxDistanceInsertion )
1064  {
1065  theR += resolution;
1066 
1067  P1.x = px + cos(A-dA_2) * theR;
1068  P1.y = py + sin(A-dA_2) * theR;
1069 
1070  P2.x = px + cos(A+dA_2) * theR;
1071  P2.y = py + sin(A+dA_2) * theR;
1072 
1073  P1.cx = x2idx( P1.x ); P1.cy = y2idx( P1.y );
1074  P2.cx = x2idx( P2.x ); P2.cy = y2idx( P2.y );
1075 
1076  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
1077  // The x> comparison implicitly holds if x<0
1078  ASSERT_( static_cast<unsigned int>(P1.cx)<size_x && static_cast<unsigned int>(P1.cy)<size_y );
1079  ASSERT_( static_cast<unsigned int>(P2.cx)<size_x && static_cast<unsigned int>(P2.cy)<size_y );
1080  #endif
1081 
1082  // Special case: Only one cell:
1083  if (P2.cx==P1.cx && P2.cy==P1.cy)
1084  {
1085  updateCell_fast_occupied(P1.cx,P1.cy, logodd_observation_occupied, logodd_thres_occupied, theMapArray, theMapSize_x );
1086  }
1087  else
1088  {
1089  // Use "fractional integers" to approximate float operations during the ray tracing:
1090  // Integers store "float values * 128"
1091  const int AcxE = P2.cx - P1.cx;
1092  const int AcyE = P2.cy - P1.cy;
1093 
1094  // Increments at each raytracing step:
1095  const int nSteps = ( max(abs(AcxE),abs(AcyE)) + 1 );
1096  const float inv_N_12 = 1.0f / nSteps; // Number of steps ^ -1
1097  const int frAcxE = round( (AcxE<<FRBITS) * inv_N_12 ); // Acx*128 / N
1098  const int frAcyE = round( (AcyE<<FRBITS) * inv_N_12 ); // Acy*128 / N
1099 
1100  R1.cx = P1.cx;
1101  R1.cy = P1.cy;
1102  R1.frX = R1.cx <<FRBITS;
1103  R1.frY = R1.cy <<FRBITS;
1104 
1105  for (int nStep=0;nStep<=nSteps;nStep++)
1106  {
1107  updateCell_fast_occupied(R1.cx,R1.cy, logodd_observation_occupied, logodd_thres_occupied, theMapArray, theMapSize_x );
1108 
1109  R1.frX += frAcxE;
1110  R1.frY += frAcyE;
1111  R1.cx = R1.frX >> FRBITS;
1112  R1.cy = R1.frY >> FRBITS;
1113  }
1114 
1115  } // end do a line
1116 
1117  } // end if we must set occupied cells
1118 
1119  } // End of each range
1120 
1121  return true;
1122  } // end reallyInsert
1123  else
1124  return false;
1125  }
1126  else
1127  {
1128  /********************************************************************
1129  OBSERVATION TYPE: Unknown
1130  ********************************************************************/
1131  return false;
1132  }
1133 
1134 // MRPT_END
1135 }
1136 
1137 
1138 /*---------------------------------------------------------------
1139  Initilization of values, don't needed to be called directly.
1140  ---------------------------------------------------------------*/
1141 COccupancyGridMap2D::TInsertionOptions::TInsertionOptions() :
1142  mapAltitude ( 0 ),
1143  useMapAltitude ( false ),
1144  maxDistanceInsertion ( 15.0f ),
1145  maxOccupancyUpdateCertainty ( 0.65f ),
1146  maxFreenessUpdateCertainty ( .0f ),
1147  maxFreenessInvalidRanges ( .0f ),
1148  considerInvalidRangesAsFreeSpace ( true ),
1149  decimation ( 1 ),
1150  horizontalTolerance ( DEG2RAD(0.05) ),
1151 
1152  CFD_features_gaussian_size ( 1 ),
1153  CFD_features_median_size ( 3 ),
1154 
1155  wideningBeamsWithDistance ( false )
1156 {
1157 }
1158 
1159 /*---------------------------------------------------------------
1160  loadFromConfigFile
1161  ---------------------------------------------------------------*/
1162 void COccupancyGridMap2D::TInsertionOptions::loadFromConfigFile(
1163  const mrpt::utils::CConfigFileBase &iniFile,
1164  const std::string &section)
1165 {
1166  MRPT_LOAD_CONFIG_VAR(mapAltitude,float, iniFile, section );
1167  MRPT_LOAD_CONFIG_VAR(maxDistanceInsertion,float, iniFile, section );
1168  MRPT_LOAD_CONFIG_VAR(maxOccupancyUpdateCertainty,float, iniFile, section );
1169  MRPT_LOAD_CONFIG_VAR(maxFreenessUpdateCertainty, float, iniFile, section );
1170  MRPT_LOAD_CONFIG_VAR(maxFreenessInvalidRanges, float, iniFile, section );
1171  MRPT_LOAD_CONFIG_VAR(useMapAltitude,bool, iniFile, section );
1172  MRPT_LOAD_CONFIG_VAR(considerInvalidRangesAsFreeSpace,bool, iniFile, section );
1173  MRPT_LOAD_CONFIG_VAR(decimation,int, iniFile, section );
1174  MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section );
1175 
1176  MRPT_LOAD_CONFIG_VAR(CFD_features_gaussian_size,float, iniFile, section );
1177  MRPT_LOAD_CONFIG_VAR(CFD_features_median_size,float, iniFile, section );
1178  MRPT_LOAD_CONFIG_VAR(wideningBeamsWithDistance,bool, iniFile, section );
1179 }
1180 
1181 /*---------------------------------------------------------------
1182  dumpToTextStream
1183  ---------------------------------------------------------------*/
1184 void COccupancyGridMap2D::TInsertionOptions::dumpToTextStream(mrpt::utils::CStream &out) const
1185 {
1186  out.printf("\n----------- [COccupancyGridMap2D::TInsertionOptions] ------------ \n\n");
1187 
1188  LOADABLEOPTS_DUMP_VAR(mapAltitude, float)
1189  LOADABLEOPTS_DUMP_VAR(maxDistanceInsertion, float)
1190  LOADABLEOPTS_DUMP_VAR(maxOccupancyUpdateCertainty, float)
1191  LOADABLEOPTS_DUMP_VAR(maxFreenessUpdateCertainty, float)
1192  LOADABLEOPTS_DUMP_VAR(maxFreenessInvalidRanges, float)
1193  LOADABLEOPTS_DUMP_VAR(useMapAltitude, bool)
1194  LOADABLEOPTS_DUMP_VAR(considerInvalidRangesAsFreeSpace, bool)
1195  LOADABLEOPTS_DUMP_VAR(decimation, int)
1196  LOADABLEOPTS_DUMP_VAR(horizontalTolerance, float)
1197  LOADABLEOPTS_DUMP_VAR(CFD_features_gaussian_size, float)
1198  LOADABLEOPTS_DUMP_VAR(CFD_features_median_size, float)
1199  LOADABLEOPTS_DUMP_VAR(wideningBeamsWithDistance, bool)
1200 
1201  out.printf("\n");
1202 }
1203 
1204 
1205 void COccupancyGridMap2D::OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *)
1206 {
1207  m_is_empty = false;
1208 }
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::obs::CObservationRange
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
Definition: obs/CObservationRange.h:30
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
min
#define min(a, b)
Definition: rplidar_driver.cpp:43
mrpt::utils::min3
const T min3(const T &A, const T &B, const T &C)
Definition: bits.h:128
TLocalPoint
Local stucture used in the next method (must be here for usage within STL stuff)
Definition: COccupancyGridMap2D_insert.cpp:30
mrpt::obs::CObservationRange::sensorConeApperture
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
Definition: obs/CObservationRange.h:44
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: maps/CColouredPointsMap.h:279
FRBITS
#define FRBITS
LOADABLEOPTS_DUMP_VAR
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
Definition: CLoadableOptions.h:93
std
STL namespace.
mrpt::maps::COccupancyGridMap2D::internal_insertObservation
virtual bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Insert the observation information into this map.
Definition: COccupancyGridMap2D_insert.cpp:40
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:33
CStream.h
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::obs::CObservation::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
mrpt::poses::CPose3D::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
mrpt::obs::CObservation2DRangeScan::isPlanarScan
bool isPlanarScan(const double tolerance=0) const
Return true if the laser scanner is "horizontal", so it has an absolute value of "pitch" and "roll" l...
Definition: CObservation2DRangeScan.cpp:282
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
mrpt::obs::CObservation2DRangeScan::validRange
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
Definition: obs/CObservation2DRangeScan.h:77
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
string
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt_alloca
#define mrpt_alloca(nBytes)
In platforms and compilers with support to "alloca", allocate a memory block on the stack; if alloca ...
Definition: memory.h:38
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
CLASS_ID
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
MRPT_LOAD_CONFIG_VAR
#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...
Definition: CConfigFileBase.h:190
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: obs/CObservation2DRangeScan.h:43
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
R
const float R
Definition: CKinematicChain.cpp:141
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::obs::CObservation
Declares a class that represents any robot&#39;s observation.
Definition: obs/CObservation.h:40
MRPT_LOAD_CONFIG_VAR_DEGREES
#define MRPT_LOAD_CONFIG_VAR_DEGREES(variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
Definition: CConfigFileBase.h:197
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
mrpt::maps::COccupancyGridMap2D::m_is_empty
bool m_is_empty
True upon construction; used by isEmpty()
Definition: maps/COccupancyGridMap2D.h:114
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
Definition: obs/CObservation2DRangeScan.h:81
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
mrpt::utils::max3
const T max3(const T &A, const T &B, const T &C)
Definition: bits.h:129
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
mrpt::maps::COccupancyGridMap2D::TInsertionOptions::dumpToTextStream
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.
Definition: COccupancyGridMap2D_insert.cpp:1184
mrpt::obs::CObservationRange::getSensorPose
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
Definition: CObservationRange.cpp:106
mrpt::maps::COccupancyGridMap2D::TInsertionOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini" file.
Definition: COccupancyGridMap2D_insert.cpp:1162
mrpt_alloca_free
#define mrpt_alloca_free(mem_block)
This method must be called to "free" each memory block allocated with "system::alloca": If the block ...
Definition: memory.h:53
x
GLenum GLint x
Definition: glext.h:3516
mrpt::obs::CObservation2DRangeScan::scan
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
Definition: obs/CObservation2DRangeScan.h:69
mrpt::maps::COccupancyGridMap2D::cellType
int16_t cellType
The type of the map cells:
Definition: maps/COccupancyGridMap2D.h:79
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: obs/CObservation2DRangeScan.h:84
mrpt::obs::CObservationRange::sensedData
TMeasurementList sensedData
All the measurements.
Definition: obs/CObservationRange.h:68
mrpt::utils::CStream::printf
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
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: obs/CObservation2DRangeScan.h:82
mrpt::maps::COccupancyGridMap2D::OnPostSuccesfulInsertObs
virtual void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *) MRPT_OVERRIDE
See base class.
Definition: COccupancyGridMap2D_insert.cpp:1205


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019