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