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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST