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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019