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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019