MRPT  1.9.9
COccupancyGridMap2D_insert.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
12 #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{0}, y{0};
35  int cx{0}, cy{0};
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  m_likelihoodCacheOutDated = 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 auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
97  CPose3D sensorPose3D = robotPose3D + o.sensorPose;
98  CPose2D laserPose(sensorPose3D);
99 
100  // Insert only HORIZONTAL scans, since the grid is supposed to
101  // be a horizontal representation of space.
102  bool reallyInsert =
103  o.isPlanarScan(insertionOptions.horizontalTolerance);
104  unsigned int decimation = insertionOptions.decimation;
105 
106  // Check the altitude of the map (if feature enabled!)
107  if (insertionOptions.useMapAltitude &&
108  fabs(insertionOptions.mapAltitude - sensorPose3D.z()) > 0.001)
109  {
110  reallyInsert = false;
111  }
112 
113  // Manage horizontal scans, but with the sensor bottom-up:
114  // Use the z-axis direction of the transformed Z axis of the sensor
115  // coordinates:
116  bool sensorIsBottomwards =
117  sensorPose3D.getHomogeneousMatrixVal<CMatrixDouble44>()(2, 2) < 0;
118 
119  if (reallyInsert)
120  {
121  // ---------------------------------------------
122  // Insert the scan as simple rays:
123  // ---------------------------------------------
124  int cx, cy, N = o.getScanSize();
125  float px, py;
126  double A, dAK;
127 
128  // Parameters values:
129  const float maxDistanceInsertion =
130  insertionOptions.maxDistanceInsertion;
131  const bool invalidAsFree =
132  insertionOptions.considerInvalidRangesAsFreeSpace;
133  float new_x_max, new_x_min;
134  float new_y_max, new_y_min;
135  float last_valid_range = maxDistanceInsertion;
136 
137  int K = updateInfoChangeOnly.enabled
138  ? updateInfoChangeOnly.laserRaysSkip
139  : decimation;
140  size_t idx, nRanges = o.getScanSize();
141  float curRange = 0;
142 
143  // Start position:
144  px = laserPose.x();
145  py = laserPose.y();
146 
147 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
150 #endif
151 
152  // Here we go! Now really insert changes in the grid:
153  if (!insertionOptions.wideningBeamsWithDistance)
154  {
155  // Method: Simple rays:
156  // -------------------------------------
157 
158  // Reserve a temporary block of memory on the stack with
159  // "alloca": this memory has NOT to be deallocated,
160  // so it's ideal for an efficient, small buffer:
161  auto* scanPoints_x =
162  (float*)mrpt_alloca(sizeof(float) * nRanges);
163  auto* scanPoints_y =
164  (float*)mrpt_alloca(sizeof(float) * nRanges);
165 
166  float *scanPoint_x, *scanPoint_y;
167 
168  if (o.rightToLeft ^ sensorIsBottomwards)
169  {
170  A = laserPose.phi() - 0.5 * o.aperture;
171  dAK = K * o.aperture / N;
172  }
173  else
174  {
175  A = laserPose.phi() + 0.5 * o.aperture;
176  dAK = -K * o.aperture / N;
177  }
178 
179  new_x_max = -(numeric_limits<float>::max)();
180  new_x_min = (numeric_limits<float>::max)();
181  new_y_max = -(numeric_limits<float>::max)();
182  new_y_min = (numeric_limits<float>::max)();
183 
184  for (idx = 0, scanPoint_x = scanPoints_x,
185  scanPoint_y = scanPoints_y;
186  idx < nRanges;
187  idx += K, scanPoint_x += K, scanPoint_y += K)
188  {
189  if (o.getScanRangeValidity(idx))
190  {
191  curRange = o.getScanRange(idx);
192  float R = min(maxDistanceInsertion, curRange);
193 
194  *scanPoint_x = px + cos(A) * R;
195  *scanPoint_y = py + sin(A) * R;
196  last_valid_range = curRange;
197  }
198  else
199  {
200  if (invalidAsFree)
201  {
202  // Invalid range:
203  float R = min(
204  maxDistanceInsertion, 0.5f * last_valid_range);
205  *scanPoint_x = px + cos(A) * R;
206  *scanPoint_y = py + sin(A) * R;
207  }
208  else
209  {
210  *scanPoint_x = px;
211  *scanPoint_y = py;
212  }
213  }
214  A += dAK;
215 
216  // Asjust size (will not change if not required):
217  new_x_max = max(new_x_max, *scanPoint_x);
218  new_x_min = min(new_x_min, *scanPoint_x);
219  new_y_max = max(new_y_max, *scanPoint_y);
220  new_y_min = min(new_y_min, *scanPoint_y);
221  }
222 
223  // Add an extra margin:
224  float securMargen = 15 * resolution;
225 
226  if (new_x_max > x_max - securMargen)
227  new_x_max += 2 * securMargen;
228  else
229  new_x_max = x_max;
230  if (new_x_min < x_min + securMargen)
231  new_x_min -= 2;
232  else
233  new_x_min = x_min;
234 
235  if (new_y_max > y_max - securMargen)
236  new_y_max += 2 * securMargen;
237  else
238  new_y_max = y_max;
239  if (new_y_min < y_min + securMargen)
240  new_y_min -= 2;
241  else
242  new_y_min = y_min;
243 
244  // -----------------------
245  // Resize to make room:
246  // -----------------------
247  resizeGrid(new_x_min, new_x_max, new_y_min, new_y_max, 0.5);
248 
249  // For updateCell_fast methods:
250  cellType* theMapArray = &map[0];
251  unsigned theMapSize_x = size_x;
252 
253  int cx0 =
254  x2idx(px); // Remember: This must be after the resizeGrid!!
255  int cy0 = y2idx(py);
256 
257  // Insert rays:
258  for (idx = 0; idx < nRanges; idx += K)
259  {
260  if (!o.getScanRangeValidity(idx) && !invalidAsFree)
261  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.getScanRangeValidity(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.getScanRangeValidity(idx) &&
320  o.getScanRange(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: https://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.getScanRangeValidity(idx))
358  {
359  curRange = o.getScanRange(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.getScanRangeValidity(idx))
451  {
452  curRange = o.getScanRange(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.getScanRangeValidity(idx) &&
720  o.getScanRange(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 (IS_CLASS(obs, CObservationRange))
806  {
807  const auto& o = dynamic_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: https://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  loadFromConfigFile
1315  ---------------------------------------------------------------*/
1317  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
1318 {
1319  MRPT_LOAD_CONFIG_VAR(mapAltitude, float, iniFile, section);
1320  MRPT_LOAD_CONFIG_VAR(maxDistanceInsertion, float, iniFile, section);
1321  MRPT_LOAD_CONFIG_VAR(maxOccupancyUpdateCertainty, float, iniFile, section);
1322  MRPT_LOAD_CONFIG_VAR(maxFreenessUpdateCertainty, float, iniFile, section);
1323  MRPT_LOAD_CONFIG_VAR(maxFreenessInvalidRanges, float, iniFile, section);
1324  MRPT_LOAD_CONFIG_VAR(useMapAltitude, bool, iniFile, section);
1326  considerInvalidRangesAsFreeSpace, bool, iniFile, section);
1327  MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
1328  MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section);
1329 
1330  MRPT_LOAD_CONFIG_VAR(CFD_features_gaussian_size, float, iniFile, section);
1331  MRPT_LOAD_CONFIG_VAR(CFD_features_median_size, float, iniFile, section);
1332  MRPT_LOAD_CONFIG_VAR(wideningBeamsWithDistance, bool, iniFile, section);
1333 }
1334 
1335 /*---------------------------------------------------------------
1336  dumpToTextStream
1337  ---------------------------------------------------------------*/
1339  std::ostream& out) const
1340 {
1341  out << "\n----------- [COccupancyGridMap2D::TInsertionOptions] "
1342  "------------ "
1343  "\n\n";
1344 
1345  LOADABLEOPTS_DUMP_VAR(mapAltitude, float)
1346  LOADABLEOPTS_DUMP_VAR(maxDistanceInsertion, float)
1347  LOADABLEOPTS_DUMP_VAR(maxOccupancyUpdateCertainty, float)
1348  LOADABLEOPTS_DUMP_VAR(maxFreenessUpdateCertainty, float)
1349  LOADABLEOPTS_DUMP_VAR(maxFreenessInvalidRanges, float)
1350  LOADABLEOPTS_DUMP_VAR(useMapAltitude, bool)
1351  LOADABLEOPTS_DUMP_VAR(considerInvalidRangesAsFreeSpace, bool)
1352  LOADABLEOPTS_DUMP_VAR(decimation, int)
1353  LOADABLEOPTS_DUMP_VAR(horizontalTolerance, float)
1354  LOADABLEOPTS_DUMP_VAR(CFD_features_gaussian_size, float)
1355  LOADABLEOPTS_DUMP_VAR(CFD_features_median_size, float)
1356  LOADABLEOPTS_DUMP_VAR(wideningBeamsWithDistance, bool)
1357 
1358  out << "\n";
1359 }
1360 
1362  const mrpt::obs::CObservation&)
1363 {
1364  m_is_empty = false;
1365 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Insert the observation information into this map.
Local stucture used in the next method (must be here for usage within STL stuff)
#define FRBITS
STL namespace.
const T min3(const T &A, const T &B, const T &C)
void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation &) override
See base class.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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.
#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:125
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
GLsizei const GLchar ** string
Definition: glext.h:4116
#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...
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...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
OccGridCellTraits::cellType cellType
The type of the map cells:
GLenum GLint GLint y
Definition: glext.h:3542
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
#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:3542
const T max3(const T &A, const T &B, const T &C)
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019