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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018