Main MRPT website > C++ reference for MRPT 1.5.6
COccupancyGridMap2D_voronoi.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/utils/round.h> // round()
14 
15 using namespace mrpt;
16 using namespace mrpt::maps;
17 using namespace mrpt::obs;
18 using namespace mrpt::utils;
19 using namespace mrpt::poses;
20 using namespace std;
21 
22 /*---------------------------------------------------------------
23  Build_VoronoiDiagram
24  ---------------------------------------------------------------*/
25 void COccupancyGridMap2D::buildVoronoiDiagram(
26  float threshold,
27  float robot_size,
28  int x1,
29  int x2,
30  int y1,
31  int y2)
32 {
33  // The whole map?
34  if (!x1 && !x2 && !y1 && !y2) {
35  x1=y1=0;
36  x2=size_x-1;
37  y2=size_y-1;
38  }
39  else {
40  x1=max(0,x1);
41  y1=max(0,y1);
42  x2=min(x2,static_cast<int>(size_x)-1);
43  y2=min(y2,static_cast<int>(size_y)-1);
44  }
45 
46  int robot_size_units= round(100*robot_size / resolution);
47 
48  /* We store 0 in cells NOT belonging to Voronoi, or the closest distance
49  * to obstacle otherwise, the "clearance" in "int" distance units.
50  */
51  m_voronoi_diagram.setSize(x_min,x_max, y_min,y_max, resolution); // assign(size_x*size_y,0);
52  ASSERT_EQUAL_(m_voronoi_diagram.getSizeX(), size_x);
53  ASSERT_EQUAL_(m_voronoi_diagram.getSizeY(), size_y);
54  m_voronoi_diagram.fill(0);
55 
56  // freeness threshold
57  voroni_free_threshold= 1.0f - threshold;
58 
59  int basis_x[2],basis_y[2];
60  int nBasis;
61 
62  // Build Voronoi:
63  for (int x=x1;x<=x2;x++) {
64  for (int y=y1;y<=y2;y++)
65  {
66  const int Clearance = computeClearance(x,y,basis_x,basis_y,&nBasis);
67 
68  if (Clearance > robot_size_units )
69  setVoroniClearance(x,y,Clearance );
70  }
71  }
72 
73  // Limpiar: Hacer que los trazos sean de grosor 1:
74  // Si un punto del diagrama esta rodeada de mas de 2
75  // puntos tb del diagrama, eliminarlo:
76  int nDiag;
77  for (int x=x1;x<=x2;x++) {
78  for (int y=y1;y<=y2;y++) {
79  if ( getVoroniClearance(x,y) )
80  {
81  nDiag=0;
82  for (int xx=x-1;xx<=(x+1);xx++)
83  for (int yy=y-1;yy<=(y+1);yy++)
84  if (getVoroniClearance(xx,yy)) nDiag++;
85 
86  // Eliminar?
87  if (nDiag>3)
88  setVoroniClearance(x,y,0);
89  }
90  }
91  }
92 }
93 
94 /*---------------------------------------------------------------
95  findCriticalPoints
96  ---------------------------------------------------------------*/
97 void COccupancyGridMap2D::findCriticalPoints( float filter_distance )
98 {
99  int clear_xy, clear;
100 
101  int filter_dist = round(filter_distance / resolution);
102  int min_clear_near, max_clear_near;
103 
104 
105  // Resize basis-points map & set to zero:
106  m_basis_map.setSize(x_min,x_max, y_min,y_max, resolution); //m_basis_map.assign(size_x*size_y, 0);
107  ASSERT_EQUAL_(m_basis_map.getSizeX(), size_x);
108  ASSERT_EQUAL_(m_basis_map.getSizeY(), size_y);
109  m_basis_map.fill(0);
110 
111  // Temp list of candidate
112  std::vector<int> temp_x,temp_y, temp_clear, temp_borrar;
113 
114  // Scan for critical points
115  // ---------------------------------------------
116  for (int x=1;x<(static_cast<int>(size_x)-1);x++) {
117  for (int y=1;y<(static_cast<int>(size_y)-1);y++) {
118  if ( 0!=(clear_xy=getVoroniClearance(x,y)) )
119  {
120  // Is this a critical point?
121  int nVecinosVoroni = 0;
122  min_clear_near = max_clear_near = clear_xy;
123 
124  for (int xx=x-2;xx<=(x+2);xx++)
125  for (int yy=y-2;yy<=(y+2);yy++)
126  {
127  if ( 0!=(clear = getVoroniClearance(xx,yy)) )
128  {
129  nVecinosVoroni++;
130  min_clear_near = min( min_clear_near , clear );
131  max_clear_near = max( max_clear_near , clear );
132  }
133  }
134 
135  // At least 2 more neighbors
136  if (nVecinosVoroni>=3 && min_clear_near==clear_xy && max_clear_near!=clear_xy )
137  {
138  // Add to temp list:
139  temp_x.push_back( x );
140  temp_y.push_back( y );
141  temp_clear.push_back( clear_xy );
142  temp_borrar.push_back( 0 );
143  }
144 
145  }
146  }
147  }
148 
149  // Filter: find "basis points". If two coincide, leave the one with the shortest clearance.
150  std::vector<int> basis1_x,basis1_y, basis2_x,basis2_y;
151  for (unsigned i=0;i<temp_x.size();i++)
152  {
153  int basis_x[2];
154  int basis_y[2];
155  int nBasis;
156 
157  computeClearance(temp_x[i],temp_y[i],basis_x,basis_y,&nBasis);
158 
159  if (nBasis==2)
160  {
161  basis1_x.push_back(basis_x[0]);
162  basis1_y.push_back(basis_y[0]);
163 
164  basis2_x.push_back(basis_x[1]);
165  basis2_y.push_back(basis_y[1]);
166  }
167  }
168 
169  // Ver basis que coincidan:
170  for (unsigned i=0;i<(((temp_x.size()))-1);i++) {
171  if (!temp_borrar[i]) {
172  for (unsigned int j=i+1;j<temp_x.size();j++) {
173  if (!temp_borrar[j])
174  {
175  int ax,ay;
176 
177  // i1-j1
178  ax = basis1_x[i]-basis1_x[j];
179  ay = basis1_y[i]-basis1_y[j];
180  bool i1j1= (sqrt(1.0f*ax*ax+ay*ay)<filter_dist);
181 
182  // i1-j2
183  ax = basis1_x[i]-basis2_x[j];
184  ay = basis1_y[i]-basis2_y[j];
185  bool i1j2= (sqrt(1.0f*ax*ax+ay*ay)<filter_dist);
186 
187  // i2-j1
188  ax = basis2_x[i]-basis1_x[j];
189  ay = basis2_y[i]-basis1_y[j];
190  bool i2j1= (sqrt(1.0f*ax*ax+ay*ay)<filter_dist);
191 
192  // i2-j2
193  ax = basis2_x[i]-basis2_x[j];
194  ay = basis2_y[i]-basis2_y[j];
195  bool i2j2= (sqrt(1.0f*ax*ax+ay*ay)<filter_dist);
196 
197 
198  // Si coincide, eliminar el de mas "dist."
199  if ( (i1j1 && i2j2) || (i1j2 && i2j1) )
200  {
201  if ( temp_clear[i]<temp_clear[j] )
202  temp_borrar[j]=1;
203  else temp_borrar[i]=1;
204  }
205 
206  }
207  }
208  }
209  }
210 
211  // Copy to permanent list:
212  // ----------------------------------------------------------
213  CriticalPointsList.clearance.clear();
214  CriticalPointsList.x.clear();
215  CriticalPointsList.y.clear();
216  CriticalPointsList.x_basis1.clear();
217  CriticalPointsList.y_basis1.clear();
218  CriticalPointsList.x_basis2.clear();
219  CriticalPointsList.y_basis2.clear();
220 
221  for (unsigned i=0;i<temp_x.size();i++)
222  {
223  if (!temp_borrar[i])
224  {
225  CriticalPointsList.x.push_back( temp_x[i] );
226  CriticalPointsList.y.push_back( temp_y[i] );
227  CriticalPointsList.clearance.push_back( temp_clear[i] );
228 
229  // Add to the basis points as well:
230  setBasisCell( temp_x[i],temp_y[i] ,1);
231  }
232  }
233 }
234 
235 /*---------------------------------------------------------------
236  Calcula la "clearance" de una celda, y devuelve sus
237  dos (primeros) "basis"
238  -Devuelve la "clearance" en unidades de centesimas de "celdas"
239  -basis_x/y deben dar sitio para 2 int's
240 
241  - Devuelve no cero solo si la celda pertenece a Voroni
242 
243  Si se pone "GetContourPoint"=true, no se devuelven los puntos
244  ocupados como basis, sino los libres mas cercanos (Esto
245  se usa para el calculo de regiones)
246 
247  Sirve para calcular diagramas de Voronoi y crit. points,etc...
248  ---------------------------------------------------------------*/
249 int COccupancyGridMap2D::computeClearance( int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint ) const
250 {
251  static const cellType thresholdCellValue = p2l(0.5f);
252 
253  // Si la celda esta ocupada, clearance de cero!
254  if ( static_cast<unsigned>(cx)>=size_x || static_cast<unsigned>(cy)>=size_y )
255  return 0;
256 
257  if ( map[cx+cy*size_y]<thresholdCellValue )
258  return 0;
259 
260  // Truco para acelerar MUCHO:
261  // Si miramos un punto junto al mirado antes,
262  // usar sus resultados, xk SEGURO que no hay obstaculos
263  // mucho antes:
264  static int ultimo_cx = -10, ultimo_cy = -10;
265  int estimated_min_free_circle;
266  static int ultimo_free_circle;
267 
268  if ( abs(ultimo_cx-cx)<=1 && abs(ultimo_cy-cy)<=1)
269  estimated_min_free_circle = max(1,ultimo_free_circle - 3);
270  else
271  estimated_min_free_circle = 1;
272 
273  ultimo_cx = cx;
274  ultimo_cy = cy;
275 
276  // Tabla de circulos:
277  #define N_CIRCULOS 100
278  static bool tabla_construida = false;
279  static int nEntradasCirculo[N_CIRCULOS];
280  static int circ_PrimeraEntrada[N_CIRCULOS];
281  static int circs_x[32000],circs_y[32000];
282 
283  if (!tabla_construida)
284  {
285  tabla_construida=true;
286  int indice_absoluto = 0;
287  for (int i=0;i<N_CIRCULOS;i++)
288  {
289  int nPasos = round(1+(M_2PI*i)); // Estimacion de # de entradas (luego seran menos)
290  float A = 0;
291  float AA = (2.0f*M_PIf / nPasos);
292  int ult_x=0,x,ult_y=0,y;
293  int nEntradas = 0;
294 
295  circ_PrimeraEntrada[i] = indice_absoluto;
296 
297  while (A<2*M_PI)
298  {
299  x = round( i*cos( A ) );
300  y = round( i*sin( A ) );
301 
302  if ((x!=ult_x || y!=ult_y) && !(x==i && y==0) )
303  {
304  circs_x[indice_absoluto]=x;
305  circs_y[indice_absoluto++]=y;
306 
307  nEntradas++;
308  ult_x=x;
309  ult_y=y;
310  }
311 
312  A+=AA;
313  }
314 
315  nEntradasCirculo[i]=nEntradas;
316 
317  }
318 
319  }
320 
321 
322  // La celda esta libre. Buscar en un circulo creciente hasta dar
323  // dar con el obstaculo mas cercano:
324  *nBasis=0;
325  int tam_circ;
326 
327  int vueltas_extra = 2;
328 
329  for (tam_circ=estimated_min_free_circle;tam_circ<N_CIRCULOS && (!(*nBasis) || vueltas_extra );tam_circ++)
330  {
331  int nEnts = nEntradasCirculo[tam_circ];
332  bool dentro_obs = false;
333  int idx = circ_PrimeraEntrada[tam_circ];
334 
335  for (int j=0;j<nEnts && (*nBasis)<2;j++,idx++)
336  {
337  int xx = cx+circs_x[idx];
338  int yy = cy+circs_y[idx];
339 
340  if (xx>=0 && xx<static_cast<int>(size_x) && yy>=0 && yy<static_cast<int>(size_y))
341  {
342  //if ( getCell(xx,yy)<=voroni_free_threshold )
343  if ( map[xx+yy*size_y]<thresholdCellValue )
344  {
345  if (!dentro_obs)
346  {
347  dentro_obs = true;
348 
349  // Esta el 2o punto separado del 1o??
350  bool pasa;
351 
352  if (!(*nBasis))
353  pasa = true;
354  else
355  {
356  int ax = basis_x[0] - xx;
357  int ay = basis_y[0] - yy;
358  pasa = sqrt(1.0f*ax*ax+ay*ay)>(1.75f*tam_circ);
359  }
360 
361  if (pasa)
362  {
363  basis_x[*nBasis] = cx+circs_x[idx];
364  basis_y[*nBasis] = cy+circs_y[idx];
365  (*nBasis)++;
366  }
367  }
368  }
369  else
370  dentro_obs = false;
371  }
372  }
373 
374  // Si solo encontramos 1 obstaculo, 1 sola vuelta extra mas:
375  if (*nBasis)
376  {
377  if (*nBasis==1) vueltas_extra--;
378  else vueltas_extra=0;
379  }
380  }
381 
382  // Estimacion para siguiente punto:
383  ultimo_free_circle = tam_circ;
384 
385  if (*nBasis>=2)
386  {
387  if (GetContourPoint)
388  {
389  unsigned char vec;
390  int dx, dy, dir_predilecta,dir;
391 
392  // Hayar punto libre mas cercano al basis 0:
393  dx = cx - basis_x[0];
394  dy = cy - basis_y[0];
395  if (abs(dx)>abs(dy))
396  if (dx>0) dir_predilecta=4;
397  else dir_predilecta=3;
398  else
399  if (dy>0) dir_predilecta=1;
400  else dir_predilecta=6;
401 
402  vec = GetNeighborhood( basis_x[0], basis_y[0] );
403  dir = -1;
404  if (!(vec & (1<<dir_predilecta))) dir = dir_predilecta;
405  else if (!(vec & (1<<1))) dir = 1;
406  else if (!(vec & (1<<3))) dir = 3;
407  else if (!(vec & (1<<4))) dir = 4;
408  else if (!(vec & (1<<6))) dir = 6;
409  if (dir!=-1)
410  {
411  vec = GetNeighborhood(basis_x[0]+direccion_vecino_x[dir],basis_y[0]+direccion_vecino_y[dir]);
412  if (vec!=0x00 && vec!=0xFF)
413  {
414  basis_x[0]+=direccion_vecino_x[dir];
415  basis_y[0]+=direccion_vecino_y[dir];
416  }
417  }
418 
419  // Hayar punto libre mas cercano al basis 1:
420  dx = cx - basis_x[1];
421  dy = cy - basis_y[1];
422  if (abs(dx)>abs(dy))
423  if (dx>0) dir_predilecta=4;
424  else dir_predilecta=3;
425  else
426  if (dy>0) dir_predilecta=1;
427  else dir_predilecta=6;
428 
429  vec = GetNeighborhood( basis_x[1], basis_y[1] );
430  dir = -1;
431  if (!(vec & (1<<dir_predilecta))) dir = dir_predilecta;
432  else if (!(vec & (1<<1))) dir = 1;
433  else if (!(vec & (1<<3))) dir = 3;
434  else if (!(vec & (1<<4))) dir = 4;
435  else if (!(vec & (1<<6))) dir = 6;
436  if (dir!=-1)
437  {
438  vec = GetNeighborhood(basis_x[1]+direccion_vecino_x[dir],basis_y[1]+direccion_vecino_y[dir]);
439  if (vec!=0x00 && vec!=0xFF)
440  {
441  basis_x[1]+=direccion_vecino_x[dir];
442  basis_y[1]+=direccion_vecino_y[dir];
443  }
444  }
445  }
446 
447  return tam_circ*100;
448  }
449  else return 0;
450 }
451 
452 
453 /*---------------------------------------------------------------
454  Devuelve un BYTE, con bits=1 si el vecino esta ocupado:
455  Asociacion de numero de bit a vecinos:
456 
457  0 1 2
458  3 X 4
459  5 6 7
460  ---------------------------------------------------------------*/
461 inline unsigned char COccupancyGridMap2D::GetNeighborhood( int cx, int cy ) const
462 {
463  unsigned char res=0;
464 
465  if (getCell(cx-1,cy-1)<=voroni_free_threshold) res |= (1<<0);
466  if (getCell( cx ,cy-1)<=voroni_free_threshold) res |= (1<<1);
467  if (getCell(cx+1,cy-1)<=voroni_free_threshold) res |= (1<<2);
468  if (getCell(cx-1, cy )<=voroni_free_threshold) res |= (1<<3);
469  if (getCell(cx+1, cy )<=voroni_free_threshold) res |= (1<<4);
470  if (getCell(cx-1,cy+1)<=voroni_free_threshold) res |= (1<<5);
471  if (getCell( cx ,cy+1)<=voroni_free_threshold) res |= (1<<6);
472  if (getCell(cx+1,cy+1)<=voroni_free_threshold) res |= (1<<7);
473 
474  return res;
475 }
476 
477 /*---------------------------------------------------------------
478 Devuelve el indice 0..7 de la direccion, o -1 si no es valida:
479  0 1 2
480  3 X 4
481  5 6 7
482  ---------------------------------------------------------------*/
483 int COccupancyGridMap2D::direction2idx(int dx, int dy)
484 {
485  switch (dx)
486  {
487  case -1:
488  switch(dy)
489  {
490  case -1: return 0;
491  case 0: return 3;
492  case 1: return 5;
493  default: return -1;
494  };
495  case 0:
496  switch(dy)
497  {
498  case -1: return 1;
499  case 1: return 6;
500  default: return -1;
501  };
502  case 1:
503  switch(dy)
504  {
505  case -1: return 2;
506  case 0: return 4;
507  case 1: return 7;
508  default: return -1;
509  };
510  default: return -1;
511  };
512 
513 }
514 
515 /*---------------------------------------------------------------
516  computeClearance
517  ---------------------------------------------------------------*/
518 float COccupancyGridMap2D::computeClearance( float x, float y, float maxSearchDistance ) const
519 {
520  int xx1 = max(0,x2idx(x-maxSearchDistance));
521  int xx2 = min(static_cast<unsigned>( size_x-1),static_cast<unsigned>( x2idx(x+maxSearchDistance)) );
522  int yy1 = max(0,y2idx(y-maxSearchDistance));
523  int yy2 = min(static_cast<unsigned>( size_y-1),static_cast<unsigned>( y2idx(y+maxSearchDistance)));
524 
525  int cx = x2idx(x);
526  int cy = y2idx(y);
527 
528  int xx,yy;
529  float clearance_sq = square(maxSearchDistance);
530  cellType thresholdCellValue = p2l(0.5f);
531 
532  // At least 1 free cell nearby!
533  bool atLeastOneFree = false;
534  for (xx=cx-1;!atLeastOneFree && xx<=cx+1;xx++)
535  for (yy=cy-1;!atLeastOneFree && yy<=cy+1;yy++)
536  if (getCell(xx,yy)>0.505f)
537  atLeastOneFree = true;
538 
539 
540  if (!atLeastOneFree)
541  return 0;
542 
543  for (xx=xx1;xx<=xx2;xx++)
544  for (yy=yy1;yy<=yy2;yy++)
545  if (map[xx+yy*size_x]<thresholdCellValue)
546  clearance_sq = min( clearance_sq, square(resolution)*(square(xx-cx)+square(yy-cy)) );
547 
548  return sqrt(clearance_sq);
549 }
550 
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Definition: maps/CColouredPointsMap.h:281
mrpt::maps::COccupancyGridMap2D::findCriticalPoints
void findCriticalPoints(float filter_distance)
Builds a list with the critical points from Voronoi diagram, which must must be built before calling ...
Definition: COccupancyGridMap2D_voronoi.cpp:97
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
std
STL namespace.
M_PI
#define M_PI
Definition: bits.h:78
mrpt::maps::COccupancyGridMap2D::buildVoronoiDiagram
void buildVoronoiDiagram(float threshold, float robot_size, int x1=0, int x2=0, int y1=0, int y2=0)
Build the Voronoi diagram of the grid map.
Definition: COccupancyGridMap2D_voronoi.cpp:25
mrpt::utils::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
M_2PI
#define M_2PI
Definition: mrpt_macros.h:380
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
M_PIf
#define M_PIf
Definition: maps/CColouredPointsMap.h:383
N_CIRCULOS
#define N_CIRCULOS
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::maps::COccupancyGridMap2D::direction2idx
int direction2idx(int dx, int dy)
Returns the index [0,7] of the given movement, or -1 if invalid one.
Definition: COccupancyGridMap2D_voronoi.cpp:483
mrpt::maps::COccupancyGridMap2D::GetNeighborhood
unsigned char GetNeighborhood(int cx, int cy) const
Returns a byte with the occupancy of the 8 sorrounding cells.
Definition: COccupancyGridMap2D_voronoi.cpp:461
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
y
GLenum GLint GLint y
Definition: glext.h:3516
res
GLuint res
Definition: glext.h:6298
x
GLenum GLint x
Definition: glext.h:3516
mrpt::maps::COccupancyGridMap2D::computeClearance
int computeClearance(int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint=false) const
Compute the clearance of a given cell, and returns its two first basis (closest obstacle) points...
Definition: COccupancyGridMap2D_voronoi.cpp:249
mrpt::maps::COccupancyGridMap2D::cellType
int16_t cellType
The type of the map cells:
Definition: maps/COccupancyGridMap2D.h:79


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019