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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019