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