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


Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019