Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/maps/COccupancyGridMap2D.h>
13 #include <mrpt/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  ---------------------------------------------------------------*/
24 void COccupancyGridMap2D::buildVoronoiDiagram(
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  ---------------------------------------------------------------*/
255 int COccupancyGridMap2D::computeClearance(
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  ---------------------------------------------------------------*/
515 int COccupancyGridMap2D::direction2idx(int dx, int dy)
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  ---------------------------------------------------------------*/
561 float COccupancyGridMap2D::computeClearance(
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 }
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:515
COccupancyGridMap2D.h
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
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:493
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
N_CIRCULOS
#define N_CIRCULOS
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:38
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:255
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23
mrpt::maps::COccupancyGridMap2D::cellType
int16_t cellType
The type of the map cells:
Definition: COccupancyGridMap2D.h:77
res
GLuint res
Definition: glext.h:7268
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
round.h
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
maps-precomp.h
M_2PI
#define M_2PI
Definition: common.h:58
M_PIf
#define M_PIf
Definition: common.h:61
mrpt::maps
Definition: CBeacon.h:24
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:24
y
GLenum GLint GLint y
Definition: glext.h:3538
x
GLenum GLint x
Definition: glext.h:3538


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