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.scan.size();
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.scan.size();
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; idx += K, scanPoint_x++, scanPoint_y++)
187  {
188  if (o.validRange[idx])
189  {
190  curRange = o.scan[idx];
191  float R = min(maxDistanceInsertion, curRange);
192 
193  *scanPoint_x = px + cos(A) * R;
194  *scanPoint_y = py + sin(A) * R;
195  last_valid_range = curRange;
196  }
197  else
198  {
199  if (invalidAsFree)
200  {
201  // Invalid range:
202  float R = min(
203  maxDistanceInsertion, 0.5f * last_valid_range);
204  *scanPoint_x = px + cos(A) * R;
205  *scanPoint_y = py + sin(A) * R;
206  }
207  else
208  {
209  *scanPoint_x = px;
210  *scanPoint_y = py;
211  }
212  }
213  A += dAK;
214 
215  // Asjust size (will not change if not required):
216  new_x_max = max(new_x_max, *scanPoint_x);
217  new_x_min = min(new_x_min, *scanPoint_x);
218  new_y_max = max(new_y_max, *scanPoint_y);
219  new_y_min = min(new_y_min, *scanPoint_y);
220  }
221 
222  // Add an extra margin:
223  float securMargen = 15 * resolution;
224 
225  if (new_x_max > x_max - securMargen)
226  new_x_max += 2 * securMargen;
227  else
228  new_x_max = x_max;
229  if (new_x_min < x_min + securMargen)
230  new_x_min -= 2;
231  else
232  new_x_min = x_min;
233 
234  if (new_y_max > y_max - securMargen)
235  new_y_max += 2 * securMargen;
236  else
237  new_y_max = y_max;
238  if (new_y_min < y_min + securMargen)
239  new_y_min -= 2;
240  else
241  new_y_min = y_min;
242 
243  // -----------------------
244  // Resize to make room:
245  // -----------------------
246  resizeGrid(new_x_min, new_x_max, new_y_min, new_y_max, 0.5);
247 
248  // For updateCell_fast methods:
249  cellType* theMapArray = &map[0];
250  unsigned theMapSize_x = size_x;
251 
252  int cx0 =
253  x2idx(px); // Remember: This must be after the resizeGrid!!
254  int cy0 = y2idx(py);
255 
256  // Insert rays:
257  for (idx = 0; idx < nRanges; idx += K)
258  {
259  if (!o.validRange[idx] && !invalidAsFree) continue;
260 
261  // Starting position: Laser position
262  cx = cx0;
263  cy = cy0;
264 
265  // Target, in cell indexes:
266  int trg_cx = x2idx(scanPoints_x[idx]);
267  int trg_cy = y2idx(scanPoints_y[idx]);
268 
269  // The x> comparison implicitly holds if x<0
270  ASSERT_(
271  static_cast<unsigned int>(trg_cx) < size_x &&
272  static_cast<unsigned int>(trg_cy) < size_y);
273 
274  // Use "fractional integers" to approximate float operations
275  // during the ray tracing:
276  int Acx = trg_cx - cx;
277  int Acy = trg_cy - cy;
278 
279  int Acx_ = abs(Acx);
280  int Acy_ = abs(Acy);
281 
282  int nStepsRay = max(Acx_, Acy_);
283  if (!nStepsRay) continue; // May be...
284 
285  // Integers store "float values * 128"
286  float N_1 = 1.0f / nStepsRay; // Avoid division twice.
287 
288  // Increments at each raytracing step:
289  int frAcx =
290  (Acx < 0 ? -1 : +1) * round((Acx_ << FRBITS) * N_1);
291  int frAcy =
292  (Acy < 0 ? -1 : +1) * round((Acy_ << FRBITS) * N_1);
293 
294  int frCX = cx << FRBITS;
295  int frCY = cy << FRBITS;
296  const auto logodd_free = o.validRange[idx]
297  ? logodd_observation_free
298  : logodd_noecho_free;
299 
300  for (int nStep = 0; nStep < nStepsRay; nStep++)
301  {
302  updateCell_fast_free(
303  cx, cy, logodd_free, logodd_thres_free, theMapArray,
304  theMapSize_x);
305 
306  frCX += frAcx;
307  frCY += frAcy;
308 
309  cx = frCX >> FRBITS;
310  cy = frCY >> FRBITS;
311  }
312 
313  // And finally, the occupied cell at the end:
314  // Only if:
315  // - It was a valid ray, and
316  // - The ray was not truncated
317  if (o.validRange[idx] && o.scan[idx] < maxDistanceInsertion)
318  updateCell_fast_occupied(
319  trg_cx, trg_cy, logodd_observation_occupied,
320  logodd_thres_occupied, theMapArray, theMapSize_x);
321 
322  } // End of each range
323 
324  mrpt_alloca_free(scanPoints_x);
325  mrpt_alloca_free(scanPoints_y);
326 
327  } // end insert with simple rays
328  else
329  {
330  // ---------------------------------
331  // Widen rays
332  // Algorithm in: https://www.mrpt.org/Occupancy_Grids
333  // ---------------------------------
334  if (o.rightToLeft ^ sensorIsBottomwards)
335  {
336  A = laserPose.phi() - 0.5 * o.aperture;
337  dAK = K * o.aperture / N;
338  }
339  else
340  {
341  A = laserPose.phi() + 0.5 * o.aperture;
342  dAK = -K * o.aperture / N;
343  }
344 
345  new_x_max = -(numeric_limits<float>::max)();
346  new_x_min = (numeric_limits<float>::max)();
347  new_y_max = -(numeric_limits<float>::max)();
348  new_y_min = (numeric_limits<float>::max)();
349 
350  last_valid_range = maxDistanceInsertion;
351  for (idx = 0; idx < nRanges; idx += K)
352  {
353  float scanPoint_x, scanPoint_y;
354  if (o.validRange[idx])
355  {
356  curRange = o.scan[idx];
357  float R = min(maxDistanceInsertion, curRange);
358 
359  scanPoint_x = px + cos(A) * R;
360  scanPoint_y = py + sin(A) * R;
361  last_valid_range = curRange;
362  }
363  else
364  {
365  if (invalidAsFree)
366  {
367  // Invalid range:
368  float R = min(
369  maxDistanceInsertion, 0.5f * last_valid_range);
370  scanPoint_x = px + cos(A) * R;
371  scanPoint_y = py + sin(A) * R;
372  }
373  else
374  {
375  scanPoint_x = px;
376  scanPoint_y = py;
377  }
378  }
379  A += dAK;
380 
381  // Asjust size (will not change if not required):
382  new_x_max = max(new_x_max, scanPoint_x);
383  new_x_min = min(new_x_min, scanPoint_x);
384  new_y_max = max(new_y_max, scanPoint_y);
385  new_y_min = min(new_y_min, scanPoint_y);
386  }
387 
388  // Add an extra margin:
389  float securMargen = 15 * resolution;
390 
391  if (new_x_max > x_max - securMargen)
392  new_x_max += 2 * securMargen;
393  else
394  new_x_max = x_max;
395  if (new_x_min < x_min + securMargen)
396  new_x_min -= 2;
397  else
398  new_x_min = x_min;
399 
400  if (new_y_max > y_max - securMargen)
401  new_y_max += 2 * securMargen;
402  else
403  new_y_max = y_max;
404  if (new_y_min < y_min + securMargen)
405  new_y_min -= 2;
406  else
407  new_y_min = y_min;
408 
409  // -----------------------
410  // Resize to make room:
411  // -----------------------
412  resizeGrid(new_x_min, new_x_max, new_y_min, new_y_max, 0.5);
413 
414  // For updateCell_fast methods:
415  cellType* theMapArray = &map[0];
416  unsigned theMapSize_x = size_x;
417 
418  // int cx0 = x2idx(px); // Remember: This must be after
419  // the
420  // resizeGrid!!
421  // int cy0 = y2idx(py);
422 
423  // Now go and insert the triangles of each beam:
424  // -----------------------------------------------
425  if (o.rightToLeft ^ sensorIsBottomwards)
426  {
427  A = laserPose.phi() - 0.5 * o.aperture;
428  dAK = K * o.aperture / N;
429  }
430  else
431  {
432  A = laserPose.phi() + 0.5 * o.aperture;
433  dAK = -K * o.aperture / N;
434  }
435 
436  // Insert the rays:
437  // ------------------------------------------
438  // Vertices of the triangle: In meters
439  TLocalPoint P0, P1, P2, P1b;
440 
441  last_valid_range = maxDistanceInsertion;
442 
443  const double dA_2 = 0.5 * o.aperture / N;
444  for (idx = 0; idx < nRanges; idx += K, A += dAK)
445  {
446  float theR; // The range of this beam
447  if (o.validRange[idx])
448  {
449  curRange = o.scan[idx];
450  last_valid_range = curRange;
451  theR = min(maxDistanceInsertion, curRange);
452  }
453  else
454  {
455  // Invalid range:
456  if (invalidAsFree)
457  {
458  theR = min(
459  maxDistanceInsertion, 0.5f * last_valid_range);
460  }
461  else
462  continue; // Nothing to do
463  }
464  if (theR < resolution)
465  continue; // Range must be larger than a cell...
466  theR -= resolution; // Remove one cell of length, which
467  // will be filled with "occupied"
468  // later.
469 
470  /* ---------------------------------------------------------
471  Fill one triangle with vertices: P0,P1,P2
472  ---------------------------------------------------------
473  */
474  P0.x = px;
475  P0.y = py;
476 
477  P1.x = px + cos(A - dA_2) * theR;
478  P1.y = py + sin(A - dA_2) * theR;
479 
480  P2.x = px + cos(A + dA_2) * theR;
481  P2.y = py + sin(A + dA_2) * theR;
482 
483  // Order the vertices by the "y": P0->bottom, P2: top
484  if (P2.y < P1.y) std::swap(P2, P1);
485  if (P2.y < P0.y) std::swap(P2, P0);
486  if (P1.y < P0.y) std::swap(P1, P0);
487 
488  // In cell indexes:
489  P0.cx = x2idx(P0.x);
490  P0.cy = y2idx(P0.y);
491  P1.cx = x2idx(P1.x);
492  P1.cy = y2idx(P1.y);
493  P2.cx = x2idx(P2.x);
494  P2.cy = y2idx(P2.y);
495 
496 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
497  // The x> comparison implicitly holds if x<0
498  ASSERT_(
499  static_cast<unsigned int>(P0.cx) < size_x &&
500  static_cast<unsigned int>(P0.cy) < size_y);
501  ASSERT_(
502  static_cast<unsigned int>(P1.cx) < size_x &&
503  static_cast<unsigned int>(P1.cy) < size_y);
504  ASSERT_(
505  static_cast<unsigned int>(P2.cx) < size_x &&
506  static_cast<unsigned int>(P2.cy) < size_y);
507 #endif
508 
509  struct
510  {
511  int frX, frY;
512  int cx, cy;
513  } R1, R2; // Fractional coords of the two rays:
514 
515  // Special case: one single row
516  if (P0.cy == P2.cy && P0.cy == P1.cy)
517  {
518  // Optimized case:
519  int min_cx = min3(P0.cx, P1.cx, P2.cx);
520  int max_cx = max3(P0.cx, P1.cx, P2.cx);
521 
522  for (int ccx = min_cx; ccx <= max_cx; ccx++)
523  updateCell_fast_free(
524  ccx, P0.cy, logodd_observation_free,
525  logodd_thres_free, theMapArray, theMapSize_x);
526  }
527  else
528  {
529  // The intersection point P1b in the segment P0-P2 at
530  // the "y" of P1:
531  P1b.y = P1.y;
532  P1b.x = P0.x +
533  (P1.y - P0.y) * (P2.x - P0.x) / (P2.y - P0.y);
534 
535  P1b.cx = x2idx(P1b.x);
536  P1b.cy = y2idx(P1b.y);
537 
538  // Use "fractional integers" to approximate float
539  // operations during the ray tracing:
540  // Integers store "float values * 128"
541  const int Acx01 = P1.cx - P0.cx;
542  const int Acy01 = P1.cy - P0.cy;
543  const int Acx01b = P1b.cx - P0.cx;
544  // const int Acy01b = P1b.cy - P0.cy; // = Acy01
545 
546  // Increments at each raytracing step:
547  const float inv_N_01 =
548  1.0f / (max3(abs(Acx01), abs(Acy01), abs(Acx01b)) +
549  1); // Number of steps ^ -1
550  const int frAcx01 = round(
551  (Acx01 << FRBITS) * inv_N_01); // Acx*128 / N
552  const int frAcy01 = round(
553  (Acy01 << FRBITS) * inv_N_01); // Acy*128 / N
554  const int frAcx01b = round(
555  (Acx01b << FRBITS) * inv_N_01); // Acx*128 / N
556 
557  // ------------------------------------
558  // First sub-triangle: P0-P1-P1b
559  // ------------------------------------
560  R1.cx = P0.cx;
561  R1.cy = P0.cy;
562  R1.frX = P0.cx << FRBITS;
563  R1.frY = P0.cy << FRBITS;
564 
565  int frAx_R1 = 0, frAx_R2 = 0; //, frAy_R2;
566  int frAy_R1 = frAcy01;
567 
568  // Start R1=R2 = P0... unlesss P0.cy == P1.cy, i.e.
569  // there is only one row:
570  if (P0.cy != P1.cy)
571  {
572  R2 = R1;
573  // R1 & R2 follow the edges: P0->P1 & P0->P1b
574  // R1 is forced to be at the left hand:
575  if (P1.x < P1b.x)
576  {
577  // R1: P0->P1
578  frAx_R1 = frAcx01;
579  frAx_R2 = frAcx01b;
580  }
581  else
582  {
583  // R1: P0->P1b
584  frAx_R1 = frAcx01b;
585  frAx_R2 = frAcx01;
586  }
587  }
588  else
589  {
590  R2.cx = P1.cx;
591  R2.cy = P1.cy;
592  R2.frX = P1.cx << FRBITS;
593  // R2.frY = P1.cy << FRBITS;
594  }
595 
596  int last_insert_cy = -1;
597  // int last_insert_cx = -1;
598  do
599  {
600  if (last_insert_cy !=
601  R1.cy) // || last_insert_cx!=R1.cx)
602  {
603  last_insert_cy = R1.cy;
604  // last_insert_cx = R1.cx;
605 
606  for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
607  updateCell_fast_free(
608  ccx, R1.cy, logodd_observation_free,
609  logodd_thres_free, theMapArray,
610  theMapSize_x);
611  }
612 
613  R1.frX += frAx_R1;
614  R1.frY += frAy_R1;
615  R2.frX += frAx_R2; // R1.frY += frAcy01;
616 
617  R1.cx = R1.frX >> FRBITS;
618  R1.cy = R1.frY >> FRBITS;
619  R2.cx = R2.frX >> FRBITS;
620  } while (R1.cy < P1.cy);
621 
622  // ------------------------------------
623  // Second sub-triangle: P1-P1b-P2
624  // ------------------------------------
625 
626  // Use "fractional integers" to approximate float
627  // operations during the ray tracing:
628  // Integers store "float values * 128"
629  const int Acx12 = P2.cx - P1.cx;
630  const int Acy12 = P2.cy - P1.cy;
631  const int Acx1b2 = P2.cx - P1b.cx;
632  // const int Acy1b2 = Acy12
633 
634  // Increments at each raytracing step:
635  const float inv_N_12 =
636  1.0f / (max3(abs(Acx12), abs(Acy12), abs(Acx1b2)) +
637  1); // Number of steps ^ -1
638  const int frAcx12 = round(
639  (Acx12 << FRBITS) * inv_N_12); // Acx*128 / N
640  const int frAcy12 = round(
641  (Acy12 << FRBITS) * inv_N_12); // Acy*128 / N
642  const int frAcx1b2 = round(
643  (Acx1b2 << FRBITS) * inv_N_12); // Acx*128 / N
644 
645  // struct { int frX,frY; int cx,cy; } R1,R2; //
646  // Fractional coords of the two rays:
647  // R1, R2 follow edges P1->P2 & P1b->P2
648  // R1 forced to be at the left hand
649  frAy_R1 = frAcy12;
650  if (!frAy_R1)
651  frAy_R1 = 2 << FRBITS; // If Ay=0, force it to be
652  // >0 so the "do...while"
653  // loop below ends in ONE
654  // iteration.
655 
656  if (P1.x < P1b.x)
657  {
658  // R1: P1->P2, R2: P1b->P2
659  R1.cx = P1.cx;
660  R1.cy = P1.cy;
661  R2.cx = P1b.cx;
662  R2.cy = P1b.cy;
663  frAx_R1 = frAcx12;
664  frAx_R2 = frAcx1b2;
665  }
666  else
667  {
668  // R1: P1b->P2, R2: P1->P2
669  R1.cx = P1b.cx;
670  R1.cy = P1b.cy;
671  R2.cx = P1.cx;
672  R2.cy = P1.cy;
673  frAx_R1 = frAcx1b2;
674  frAx_R2 = frAcx12;
675  }
676 
677  R1.frX = R1.cx << FRBITS;
678  R1.frY = R1.cy << FRBITS;
679  R2.frX = R2.cx << FRBITS;
680  R2.frY = R2.cy << FRBITS;
681 
682  last_insert_cy = -100;
683  // last_insert_cx=-100;
684 
685  do
686  {
687  if (last_insert_cy !=
688  R1.cy) // || last_insert_cx!=R1.cx)
689  {
690  // last_insert_cx = R1.cx;
691  last_insert_cy = R1.cy;
692  for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
693  updateCell_fast_free(
694  ccx, R1.cy, logodd_observation_free,
695  logodd_thres_free, theMapArray,
696  theMapSize_x);
697  }
698 
699  R1.frX += frAx_R1;
700  R1.frY += frAy_R1;
701  R2.frX += frAx_R2; // R1.frY += frAcy01;
702 
703  R1.cx = R1.frX >> FRBITS;
704  R1.cy = R1.frY >> FRBITS;
705  R2.cx = R2.frX >> FRBITS;
706  } while (R1.cy <= P2.cy);
707 
708  } // end of free-area normal case (not a single row)
709 
710  // ----------------------------------------------------
711  // The final occupied cells along the edge P1<->P2
712  // Only if:
713  // - It was a valid ray, and
714  // - The ray was not truncated
715  // ----------------------------------------------------
716  if (o.validRange[idx] && o.scan[idx] < maxDistanceInsertion)
717  {
718  theR += resolution;
719 
720  P1.x = px + cos(A - dA_2) * theR;
721  P1.y = py + sin(A - dA_2) * theR;
722 
723  P2.x = px + cos(A + dA_2) * theR;
724  P2.y = py + sin(A + dA_2) * theR;
725 
726  P1.cx = x2idx(P1.x);
727  P1.cy = y2idx(P1.y);
728  P2.cx = x2idx(P2.x);
729  P2.cy = y2idx(P2.y);
730 
731 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
732  // The x> comparison implicitly holds if x<0
733  ASSERT_(
734  static_cast<unsigned int>(P1.cx) < size_x &&
735  static_cast<unsigned int>(P1.cy) < size_y);
736  ASSERT_(
737  static_cast<unsigned int>(P2.cx) < size_x &&
738  static_cast<unsigned int>(P2.cy) < size_y);
739 #endif
740 
741  // Special case: Only one cell:
742  if (P2.cx == P1.cx && P2.cy == P1.cy)
743  {
744  updateCell_fast_occupied(
745  P1.cx, P1.cy, logodd_observation_occupied,
746  logodd_thres_occupied, theMapArray,
747  theMapSize_x);
748  }
749  else
750  {
751  // Use "fractional integers" to approximate float
752  // operations during the ray tracing:
753  // Integers store "float values * 128"
754  const int AcxE = P2.cx - P1.cx;
755  const int AcyE = P2.cy - P1.cy;
756 
757  // Increments at each raytracing step:
758  const int nSteps = (max(abs(AcxE), abs(AcyE)) + 1);
759  const float inv_N_12 =
760  1.0f / nSteps; // Number of steps ^ -1
761  const int frAcxE = round(
762  (AcxE << FRBITS) * inv_N_12); // Acx*128 / N
763  const int frAcyE = round(
764  (AcyE << FRBITS) * inv_N_12); // Acy*128 / N
765 
766  R1.cx = P1.cx;
767  R1.cy = P1.cy;
768  R1.frX = R1.cx << FRBITS;
769  R1.frY = R1.cy << FRBITS;
770 
771  for (int nStep = 0; nStep <= nSteps; nStep++)
772  {
773  updateCell_fast_occupied(
774  R1.cx, R1.cy, logodd_observation_occupied,
775  logodd_thres_occupied, theMapArray,
776  theMapSize_x);
777 
778  R1.frX += frAcxE;
779  R1.frY += frAcyE;
780  R1.cx = R1.frX >> FRBITS;
781  R1.cy = R1.frY >> FRBITS;
782  }
783 
784  } // end do a line
785 
786  } // end if we must set occupied cells
787 
788  } // End of each range
789 
790  } // end insert with beam widening
791 
792  // Finished:
793  return true;
794  }
795  else
796  {
797  // A non-horizontal scan:
798  return false;
799  }
800  }
801  else if (IS_CLASS(obs, CObservationRange))
802  {
803  const auto& o = dynamic_cast<const CObservationRange&>(obs);
804  CPose3D spose;
805  o.getSensorPose(spose);
806  CPose3D sensorPose3D = robotPose3D + spose;
807  CPose2D laserPose(sensorPose3D);
808 
809  // Insert only HORIZONTAL scans, since the grid is supposed to
810  // be a horizontal representation of space.
811  bool reallyInsert = true;
812  unsigned int decimation = insertionOptions.decimation;
813 
814  // Check the altitude of the map (if feature enabled!)
815  if (insertionOptions.useMapAltitude &&
816  fabs(insertionOptions.mapAltitude - sensorPose3D.z()) > 0.001)
817  {
818  reallyInsert = false;
819  }
820  if (reallyInsert)
821  {
822  // ---------------------------------------------
823  // Insert the scan as simple rays:
824  // ---------------------------------------------
825 
826  // int /*cx,cy,*/ N = o.sensedData.size();
827  float px, py;
828  double A, dAK;
829 
830  // Parameters values:
831  const float maxDistanceInsertion =
832  insertionOptions.maxDistanceInsertion;
833  const bool invalidAsFree =
834  insertionOptions.considerInvalidRangesAsFreeSpace;
835  float new_x_max, new_x_min;
836  float new_y_max, new_y_min;
837  float last_valid_range = maxDistanceInsertion;
838 
839  int K = updateInfoChangeOnly.enabled
840  ? updateInfoChangeOnly.laserRaysSkip
841  : decimation;
842  size_t idx, nRanges = o.sensedData.size();
843  float curRange = 0;
844 
845  // Start position:
846  px = laserPose.x();
847  py = laserPose.y();
848 
849 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
852 #endif
853  // ---------------------------------
854  // Widen rays
855  // Algorithm in: https://www.mrpt.org/Occupancy_Grids
856  // FIXME: doesn't support many different poses in one measurement
857  // ---------------------------------
858  A = laserPose.phi();
859  dAK = 0;
860 
861  new_x_max = -(numeric_limits<float>::max)();
862  new_x_min = (numeric_limits<float>::max)();
863  new_y_max = -(numeric_limits<float>::max)();
864  new_y_min = (numeric_limits<float>::max)();
865 
866  last_valid_range = maxDistanceInsertion;
867 
868  for (idx = 0; idx < nRanges; idx += K)
869  {
870  float scanPoint_x, scanPoint_y;
871  if (o.sensedData[idx].sensedDistance < maxDistanceInsertion)
872  {
873  curRange = o.sensedData[idx].sensedDistance;
874  float R = min(maxDistanceInsertion, curRange);
875 
876  scanPoint_x = px + cos(A) * R;
877  scanPoint_y = py + sin(A) * R;
878  last_valid_range = curRange;
879  }
880  else
881  {
882  if (invalidAsFree)
883  {
884  // Invalid range:
885  float R =
886  min(maxDistanceInsertion, 0.5f * last_valid_range);
887  scanPoint_x = px + cos(A) * R;
888  scanPoint_y = py + sin(A) * R;
889  }
890  else
891  {
892  scanPoint_x = px;
893  scanPoint_y = py;
894  }
895  }
896  A += dAK;
897 
898  // Asjust size (will not change if not required):
899  new_x_max = max(new_x_max, scanPoint_x);
900  new_x_min = min(new_x_min, scanPoint_x);
901  new_y_max = max(new_y_max, scanPoint_y);
902  new_y_min = min(new_y_min, scanPoint_y);
903  }
904 
905  // Add an extra margin:
906  float securMargen = 15 * resolution;
907 
908  if (new_x_max > x_max - securMargen)
909  new_x_max += 2 * securMargen;
910  else
911  new_x_max = x_max;
912  if (new_x_min < x_min + securMargen)
913  new_x_min -= 2;
914  else
915  new_x_min = x_min;
916 
917  if (new_y_max > y_max - securMargen)
918  new_y_max += 2 * securMargen;
919  else
920  new_y_max = y_max;
921  if (new_y_min < y_min + securMargen)
922  new_y_min -= 2;
923  else
924  new_y_min = y_min;
925 
926  // -----------------------
927  // Resize to make room:
928  // -----------------------
929  resizeGrid(new_x_min, new_x_max, new_y_min, new_y_max, 0.5);
930 
931  // For updateCell_fast methods:
932  cellType* theMapArray = &map[0];
933  unsigned theMapSize_x = size_x;
934 
935  // int cx0 = x2idx(px); // Remember: This must be after the
936  // resizeGrid!!
937  // int cy0 = y2idx(py);
938 
939  // Now go and insert the triangles of each beam:
940  // -----------------------------------------------
941  A = laserPose.phi() - 0.5 * o.sensorConeApperture;
942  dAK = 0;
943 
944  // Insert the rays:
945  // ------------------------------------------
946  // Vertices of the triangle: In meters
947  TLocalPoint P0, P1, P2, P1b;
948 
949  last_valid_range = maxDistanceInsertion;
950 
951  const double dA_2 = 0.5 * o.sensorConeApperture;
952  for (idx = 0; idx < nRanges; idx += K, A += dAK)
953  {
954  float theR; // The range of this beam
955  if (o.sensedData[idx].sensedDistance < maxDistanceInsertion)
956  {
957  curRange = o.sensedData[idx].sensedDistance;
958  last_valid_range = curRange;
959  theR = min(maxDistanceInsertion, curRange);
960  }
961  else
962  {
963  // Invalid range:
964  if (invalidAsFree)
965  {
966  theR =
967  min(maxDistanceInsertion, 0.5f * last_valid_range);
968  }
969  else
970  continue; // Nothing to do
971  }
972  if (theR < resolution)
973  continue; // Range must be larger than a cell...
974  theR -= resolution; // Remove one cell of length, which will be
975  // filled with "occupied" later.
976 
977  /* ---------------------------------------------------------
978  Fill one triangle with vertices: P0,P1,P2
979  --------------------------------------------------------- */
980  P0.x = px;
981  P0.y = py;
982 
983  P1.x = px + cos(A - dA_2) * theR;
984  P1.y = py + sin(A - dA_2) * theR;
985 
986  P2.x = px + cos(A + dA_2) * theR;
987  P2.y = py + sin(A + dA_2) * theR;
988 
989  // Order the vertices by the "y": P0->bottom, P2: top
990  if (P2.y < P1.y) std::swap(P2, P1);
991  if (P2.y < P0.y) std::swap(P2, P0);
992  if (P1.y < P0.y) std::swap(P1, P0);
993 
994  // In cell indexes:
995  P0.cx = x2idx(P0.x);
996  P0.cy = y2idx(P0.y);
997  P1.cx = x2idx(P1.x);
998  P1.cy = y2idx(P1.y);
999  P2.cx = x2idx(P2.x);
1000  P2.cy = y2idx(P2.y);
1001 
1002 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
1003  // The x> comparison implicitly holds if x<0
1004  ASSERT_(
1005  static_cast<unsigned int>(P0.cx) < size_x &&
1006  static_cast<unsigned int>(P0.cy) < size_y);
1007  ASSERT_(
1008  static_cast<unsigned int>(P1.cx) < size_x &&
1009  static_cast<unsigned int>(P1.cy) < size_y);
1010  ASSERT_(
1011  static_cast<unsigned int>(P2.cx) < size_x &&
1012  static_cast<unsigned int>(P2.cy) < size_y);
1013 #endif
1014 
1015  struct
1016  {
1017  int frX, frY;
1018  int cx, cy;
1019  } R1, R2; // Fractional coords of the two rays:
1020 
1021  // Special case: one single row
1022  if (P0.cy == P2.cy && P0.cy == P1.cy)
1023  {
1024  // Optimized case:
1025  int min_cx = min3(P0.cx, P1.cx, P2.cx);
1026  int max_cx = max3(P0.cx, P1.cx, P2.cx);
1027 
1028  for (int ccx = min_cx; ccx <= max_cx; ccx++)
1029  updateCell_fast_free(
1030  ccx, P0.cy, logodd_observation_free,
1031  logodd_thres_free, theMapArray, theMapSize_x);
1032  }
1033  else
1034  {
1035  // The intersection point P1b in the segment P0-P2 at the
1036  // "y" of P1:
1037  P1b.y = P1.y;
1038  P1b.x =
1039  P0.x + (P1.y - P0.y) * (P2.x - P0.x) / (P2.y - P0.y);
1040 
1041  P1b.cx = x2idx(P1b.x);
1042  P1b.cy = y2idx(P1b.y);
1043 
1044  // Use "fractional integers" to approximate float operations
1045  // during the ray tracing:
1046  // Integers store "float values * 128"
1047  const int Acx01 = P1.cx - P0.cx;
1048  const int Acy01 = P1.cy - P0.cy;
1049  const int Acx01b = P1b.cx - P0.cx;
1050  // const int Acy01b = P1b.cy - P0.cy; // = Acy01
1051 
1052  // Increments at each raytracing step:
1053  const float inv_N_01 =
1054  1.0f / (max3(abs(Acx01), abs(Acy01), abs(Acx01b)) +
1055  1); // Number of steps ^ -1
1056  const int frAcx01 =
1057  round((Acx01 << FRBITS) * inv_N_01); // Acx*128 / N
1058  const int frAcy01 =
1059  round((Acy01 << FRBITS) * inv_N_01); // Acy*128 / N
1060  const int frAcx01b =
1061  round((Acx01b << FRBITS) * inv_N_01); // Acx*128 / N
1062 
1063  // ------------------------------------
1064  // First sub-triangle: P0-P1-P1b
1065  // ------------------------------------
1066  R1.cx = P0.cx;
1067  R1.cy = P0.cy;
1068  R1.frX = P0.cx << FRBITS;
1069  R1.frY = P0.cy << FRBITS;
1070 
1071  int frAx_R1 = 0, frAx_R2 = 0; //, frAy_R2;
1072  int frAy_R1 = frAcy01;
1073 
1074  // Start R1=R2 = P0... unlesss P0.cy == P1.cy, i.e. there is
1075  // only one row:
1076  if (P0.cy != P1.cy)
1077  {
1078  R2 = R1;
1079  // R1 & R2 follow the edges: P0->P1 & P0->P1b
1080  // R1 is forced to be at the left hand:
1081  if (P1.x < P1b.x)
1082  {
1083  // R1: P0->P1
1084  frAx_R1 = frAcx01;
1085  frAx_R2 = frAcx01b;
1086  }
1087  else
1088  {
1089  // R1: P0->P1b
1090  frAx_R1 = frAcx01b;
1091  frAx_R2 = frAcx01;
1092  }
1093  }
1094  else
1095  {
1096  R2.cx = P1.cx;
1097  R2.cy = P1.cy;
1098  R2.frX = P1.cx << FRBITS;
1099  // R2.frY = P1.cy <<FRBITS;
1100  }
1101  int last_insert_cy = -1;
1102  // int last_insert_cx = -1;
1103  do
1104  {
1105  if (last_insert_cy !=
1106  R1.cy) // || last_insert_cx!=R1.cx)
1107  {
1108  last_insert_cy = R1.cy;
1109  // last_insert_cx = R1.cx;
1110 
1111  for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
1112  updateCell_fast_free(
1113  ccx, R1.cy, logodd_observation_free,
1114  logodd_thres_free, theMapArray,
1115  theMapSize_x);
1116  }
1117 
1118  R1.frX += frAx_R1;
1119  R1.frY += frAy_R1;
1120  R2.frX += frAx_R2; // R1.frY += frAcy01;
1121 
1122  R1.cx = R1.frX >> FRBITS;
1123  R1.cy = R1.frY >> FRBITS;
1124  R2.cx = R2.frX >> FRBITS;
1125  } while (R1.cy < P1.cy);
1126  // ------------------------------------
1127  // Second sub-triangle: P1-P1b-P2
1128  // ------------------------------------
1129 
1130  // Use "fractional integers" to approximate float operations
1131  // during the ray tracing:
1132  // Integers store "float values * 128"
1133  const int Acx12 = P2.cx - P1.cx;
1134  const int Acy12 = P2.cy - P1.cy;
1135  const int Acx1b2 = P2.cx - P1b.cx;
1136  // const int Acy1b2 = Acy12
1137 
1138  // Increments at each raytracing step:
1139  const float inv_N_12 =
1140  1.0f / (max3(abs(Acx12), abs(Acy12), abs(Acx1b2)) +
1141  1); // Number of steps ^ -1
1142  const int frAcx12 =
1143  round((Acx12 << FRBITS) * inv_N_12); // Acx*128 / N
1144  const int frAcy12 =
1145  round((Acy12 << FRBITS) * inv_N_12); // Acy*128 / N
1146  const int frAcx1b2 =
1147  round((Acx1b2 << FRBITS) * inv_N_12); // Acx*128 / N
1148 
1149  // struct { int frX,frY; int cx,cy; } R1,R2; //
1150  // Fractional
1151  // coords of the two rays:
1152  // R1, R2 follow edges P1->P2 & P1b->P2
1153  // R1 forced to be at the left hand
1154  frAy_R1 = frAcy12;
1155  if (!frAy_R1)
1156  frAy_R1 = 2 << FRBITS; // If Ay=0, force it to be >0 so
1157  // the "do...while" loop below
1158  // ends in ONE iteration.
1159 
1160  if (P1.x < P1b.x)
1161  {
1162  // R1: P1->P2, R2: P1b->P2
1163  R1.cx = P1.cx;
1164  R1.cy = P1.cy;
1165  R2.cx = P1b.cx;
1166  R2.cy = P1b.cy;
1167  frAx_R1 = frAcx12;
1168  frAx_R2 = frAcx1b2;
1169  }
1170  else
1171  {
1172  // R1: P1b->P2, R2: P1->P2
1173  R1.cx = P1b.cx;
1174  R1.cy = P1b.cy;
1175  R2.cx = P1.cx;
1176  R2.cy = P1.cy;
1177  frAx_R1 = frAcx1b2;
1178  frAx_R2 = frAcx12;
1179  }
1180 
1181  R1.frX = R1.cx << FRBITS;
1182  R1.frY = R1.cy << FRBITS;
1183  R2.frX = R2.cx << FRBITS;
1184  R2.frY = R2.cy << FRBITS;
1185 
1186  last_insert_cy = -100;
1187  // last_insert_cx=-100;
1188 
1189  do
1190  {
1191  if (last_insert_cy !=
1192  R1.cy) // || last_insert_cx!=R1.cx)
1193  {
1194  // last_insert_cx = R1.cx;
1195  last_insert_cy = R1.cy;
1196  for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
1197  updateCell_fast_free(
1198  ccx, R1.cy, logodd_observation_free,
1199  logodd_thres_free, theMapArray,
1200  theMapSize_x);
1201  }
1202 
1203  R1.frX += frAx_R1;
1204  R1.frY += frAy_R1;
1205  R2.frX += frAx_R2; // R1.frY += frAcy01;
1206 
1207  R1.cx = R1.frX >> FRBITS;
1208  R1.cy = R1.frY >> FRBITS;
1209  R2.cx = R2.frX >> FRBITS;
1210  } while (R1.cy <= P2.cy);
1211 
1212  } // end of free-area normal case (not a single row)
1213 
1214  // ----------------------------------------------------
1215  // The final occupied cells along the edge P1<->P2
1216  // Only if:
1217  // - It was a valid ray, and
1218  // - The ray was not truncated
1219  // ----------------------------------------------------
1220  if (o.sensedData[idx].sensedDistance < maxDistanceInsertion)
1221  {
1222  theR += resolution;
1223 
1224  P1.x = px + cos(A - dA_2) * theR;
1225  P1.y = py + sin(A - dA_2) * theR;
1226 
1227  P2.x = px + cos(A + dA_2) * theR;
1228  P2.y = py + sin(A + dA_2) * theR;
1229 
1230  P1.cx = x2idx(P1.x);
1231  P1.cy = y2idx(P1.y);
1232  P2.cx = x2idx(P2.x);
1233  P2.cy = y2idx(P2.y);
1234 
1235 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
1236  // The x> comparison implicitly holds if x<0
1237  ASSERT_(
1238  static_cast<unsigned int>(P1.cx) < size_x &&
1239  static_cast<unsigned int>(P1.cy) < size_y);
1240  ASSERT_(
1241  static_cast<unsigned int>(P2.cx) < size_x &&
1242  static_cast<unsigned int>(P2.cy) < size_y);
1243 #endif
1244 
1245  // Special case: Only one cell:
1246  if (P2.cx == P1.cx && P2.cy == P1.cy)
1247  {
1248  updateCell_fast_occupied(
1249  P1.cx, P1.cy, logodd_observation_occupied,
1250  logodd_thres_occupied, theMapArray, theMapSize_x);
1251  }
1252  else
1253  {
1254  // Use "fractional integers" to approximate float
1255  // operations during the ray tracing:
1256  // Integers store "float values * 128"
1257  const int AcxE = P2.cx - P1.cx;
1258  const int AcyE = P2.cy - P1.cy;
1259 
1260  // Increments at each raytracing step:
1261  const int nSteps = (max(abs(AcxE), abs(AcyE)) + 1);
1262  const float inv_N_12 =
1263  1.0f / nSteps; // Number of steps ^ -1
1264  const int frAcxE =
1265  round((AcxE << FRBITS) * inv_N_12); // Acx*128 / N
1266  const int frAcyE =
1267  round((AcyE << FRBITS) * inv_N_12); // Acy*128 / N
1268 
1269  R1.cx = P1.cx;
1270  R1.cy = P1.cy;
1271  R1.frX = R1.cx << FRBITS;
1272  R1.frY = R1.cy << FRBITS;
1273 
1274  for (int nStep = 0; nStep <= nSteps; nStep++)
1275  {
1276  updateCell_fast_occupied(
1277  R1.cx, R1.cy, logodd_observation_occupied,
1278  logodd_thres_occupied, theMapArray,
1279  theMapSize_x);
1280 
1281  R1.frX += frAcxE;
1282  R1.frY += frAcyE;
1283  R1.cx = R1.frX >> FRBITS;
1284  R1.cy = R1.frY >> FRBITS;
1285  }
1286 
1287  } // end do a line
1288 
1289  } // end if we must set occupied cells
1290 
1291  } // End of each range
1292 
1293  return true;
1294  } // end reallyInsert
1295  else
1296  return false;
1297  }
1298  else
1299  {
1300  /********************************************************************
1301  OBSERVATION TYPE: Unknown
1302  ********************************************************************/
1303  return false;
1304  }
1305 
1306  // MRPT_END
1307 }
1308 
1309 /*---------------------------------------------------------------
1310  loadFromConfigFile
1311  ---------------------------------------------------------------*/
1313  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
1314 {
1315  MRPT_LOAD_CONFIG_VAR(mapAltitude, float, iniFile, section);
1316  MRPT_LOAD_CONFIG_VAR(maxDistanceInsertion, float, iniFile, section);
1317  MRPT_LOAD_CONFIG_VAR(maxOccupancyUpdateCertainty, float, iniFile, section);
1318  MRPT_LOAD_CONFIG_VAR(maxFreenessUpdateCertainty, float, iniFile, section);
1319  MRPT_LOAD_CONFIG_VAR(maxFreenessInvalidRanges, float, iniFile, section);
1320  MRPT_LOAD_CONFIG_VAR(useMapAltitude, bool, iniFile, section);
1322  considerInvalidRangesAsFreeSpace, bool, iniFile, section);
1323  MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
1324  MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section);
1325 
1326  MRPT_LOAD_CONFIG_VAR(CFD_features_gaussian_size, float, iniFile, section);
1327  MRPT_LOAD_CONFIG_VAR(CFD_features_median_size, float, iniFile, section);
1328  MRPT_LOAD_CONFIG_VAR(wideningBeamsWithDistance, bool, iniFile, section);
1329 }
1330 
1331 /*---------------------------------------------------------------
1332  dumpToTextStream
1333  ---------------------------------------------------------------*/
1335  std::ostream& out) const
1336 {
1337  out << mrpt::format(
1338  "\n----------- [COccupancyGridMap2D::TInsertionOptions] ------------ "
1339  "\n\n");
1340 
1341  LOADABLEOPTS_DUMP_VAR(mapAltitude, float)
1342  LOADABLEOPTS_DUMP_VAR(maxDistanceInsertion, float)
1343  LOADABLEOPTS_DUMP_VAR(maxOccupancyUpdateCertainty, float)
1344  LOADABLEOPTS_DUMP_VAR(maxFreenessUpdateCertainty, float)
1345  LOADABLEOPTS_DUMP_VAR(maxFreenessInvalidRanges, float)
1346  LOADABLEOPTS_DUMP_VAR(useMapAltitude, bool)
1347  LOADABLEOPTS_DUMP_VAR(considerInvalidRangesAsFreeSpace, bool)
1348  LOADABLEOPTS_DUMP_VAR(decimation, int)
1349  LOADABLEOPTS_DUMP_VAR(horizontalTolerance, float)
1350  LOADABLEOPTS_DUMP_VAR(CFD_features_gaussian_size, float)
1351  LOADABLEOPTS_DUMP_VAR(CFD_features_median_size, float)
1352  LOADABLEOPTS_DUMP_VAR(wideningBeamsWithDistance, bool)
1353 
1354  out << mrpt::format("\n");
1355 }
1356 
1358  const mrpt::obs::CObservation&)
1359 {
1360  m_is_empty = false;
1361 }
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.
#define min(a, b)
constexpr matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CMatrixFixed.h:233
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)
const T max3(const T &A, const T &B, const T &C)
#define FRBITS
STL namespace.
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:133
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...
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...
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:84
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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
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: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019