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


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST