26 float threshold,
float robot_size,
int x1,
int x2,
int y1,
int y2)
29 if (!x1 && !x2 && !y1 && !y2)
39 x2 =
min(x2, static_cast<int>(size_x) - 1);
40 y2 =
min(y2, static_cast<int>(size_y) - 1);
43 int robot_size_units =
round(100 * robot_size / resolution);
48 m_voronoi_diagram.setSize(
49 x_min, x_max, y_min, y_max, resolution);
52 m_voronoi_diagram.fill(0);
55 voroni_free_threshold = 1.0f - threshold;
57 int basis_x[2], basis_y[2];
61 for (
int x = x1;
x <= x2;
x++)
63 for (
int y = y1;
y <= y2;
y++)
66 computeClearance(
x,
y, basis_x, basis_y, &nBasis);
68 if (Clearance > robot_size_units)
69 setVoroniClearance(
x,
y, Clearance);
77 for (
int x = x1;
x <= x2;
x++)
79 for (
int y = y1;
y <= y2;
y++)
81 if (getVoroniClearance(
x,
y))
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++;
89 if (nDiag > 3) setVoroniClearance(
x,
y, 0);
102 int filter_dist =
round(filter_distance / resolution);
103 int min_clear_near, max_clear_near;
107 x_min, x_max, y_min, y_max,
114 std::vector<int> temp_x, temp_y, temp_clear, temp_borrar;
122 if (0 != (clear_xy = getVoroniClearance(
x,
y)))
125 int nVecinosVoroni = 0;
126 min_clear_near = max_clear_near = clear_xy;
128 for (
int xx =
x - 2; xx <= (
x + 2); xx++)
129 for (
int yy =
y - 2; yy <= (
y + 2); yy++)
131 if (0 != (
clear = getVoroniClearance(xx, yy)))
134 min_clear_near =
min(min_clear_near,
clear);
135 max_clear_near = max(max_clear_near,
clear);
140 if (nVecinosVoroni >= 3 && min_clear_near == clear_xy &&
141 max_clear_near != clear_xy)
146 temp_clear.push_back(clear_xy);
147 temp_borrar.push_back(0);
155 std::vector<int> basis1_x, basis1_y, basis2_x, basis2_y;
156 for (
unsigned i = 0; i < temp_x.size(); i++)
162 computeClearance(temp_x[i], temp_y[i], basis_x, basis_y, &nBasis);
166 basis1_x.push_back(basis_x[0]);
167 basis1_y.push_back(basis_y[0]);
169 basis2_x.push_back(basis_x[1]);
170 basis2_y.push_back(basis_y[1]);
175 for (
unsigned i = 0; i < (((temp_x.size())) - 1); i++)
179 for (
unsigned int j = i + 1; j < temp_x.size(); j++)
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);
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);
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);
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);
206 if ((i1j1 && i2j2) || (i1j2 && i2j1))
208 if (temp_clear[i] < temp_clear[j])
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();
228 for (
unsigned i = 0; i < temp_x.size(); i++)
232 CriticalPointsList.x.push_back(temp_x[i]);
233 CriticalPointsList.y.push_back(temp_y[i]);
234 CriticalPointsList.clearance.push_back(temp_clear[i]);
237 setBasisCell(temp_x[i], temp_y[i], 1);
257 int cx,
int cy,
int* basis_x,
int* basis_y,
int* nBasis,
258 bool GetContourPoint)
const 260 static const cellType thresholdCellValue = p2l(0.5f);
263 if (static_cast<unsigned>(cx) >= size_x ||
264 static_cast<unsigned>(cy) >= size_y)
267 if (map[cx + cy * size_y] < thresholdCellValue)
return 0;
273 static int ultimo_cx = -10, ultimo_cy = -10;
274 int estimated_min_free_circle;
275 static int ultimo_free_circle;
277 if (abs(ultimo_cx - cx) <= 1 && abs(ultimo_cy - cy) <= 1)
278 estimated_min_free_circle = max(1, ultimo_free_circle - 3);
280 estimated_min_free_circle = 1;
286 #define N_CIRCULOS 100 287 static bool tabla_construida =
false;
290 static int circs_x[32000], circs_y[32000];
292 if (!tabla_construida)
294 tabla_construida =
true;
295 int indice_absoluto = 0;
302 float AA = (2.0f *
M_PIf / nPasos);
303 int ult_x = 0,
x, ult_y = 0,
y;
306 circ_PrimeraEntrada[i] = indice_absoluto;
313 if ((
x != ult_x ||
y != ult_y) && !(
x == i &&
y == 0))
315 circs_x[indice_absoluto] =
x;
316 circs_y[indice_absoluto++] =
y;
326 nEntradasCirculo[i] = nEntradas;
335 int vueltas_extra = 2;
337 for (tam_circ = estimated_min_free_circle;
338 tam_circ <
N_CIRCULOS && (!(*nBasis) || vueltas_extra); tam_circ++)
340 int nEnts = nEntradasCirculo[tam_circ];
341 bool dentro_obs =
false;
342 int idx = circ_PrimeraEntrada[tam_circ];
344 for (
int j = 0; j < nEnts && (*nBasis) < 2; j++, idx++)
346 int xx = cx + circs_x[idx];
347 int yy = cy + circs_y[idx];
349 if (xx >= 0 && xx < static_cast<int>(size_x) && yy >= 0 &&
350 yy < static_cast<int>(size_y))
353 if (map[xx + yy * size_y] < thresholdCellValue)
366 int ax = basis_x[0] - xx;
367 int ay = basis_y[0] - yy;
368 pasa = sqrt(1.0f * ax * ax + ay * ay) >
374 basis_x[*nBasis] = cx + circs_x[idx];
375 basis_y[*nBasis] = cy + circs_y[idx];
396 ultimo_free_circle = tam_circ;
403 int dx, dy, dir_predilecta, dir;
406 dx = cx - basis_x[0];
407 dy = cy - basis_y[0];
408 if (abs(dx) > abs(dy))
418 vec = GetNeighborhood(basis_x[0], basis_y[0]);
420 if (!(vec & (1 << dir_predilecta)))
421 dir = dir_predilecta;
422 else if (!(vec & (1 << 1)))
424 else if (!(vec & (1 << 3)))
426 else if (!(vec & (1 << 4)))
428 else if (!(vec & (1 << 6)))
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)
437 basis_x[0] += direccion_vecino_x[dir];
438 basis_y[0] += direccion_vecino_y[dir];
443 dx = cx - basis_x[1];
444 dy = cy - basis_y[1];
445 if (abs(dx) > abs(dy))
455 vec = GetNeighborhood(basis_x[1], basis_y[1]);
457 if (!(vec & (1 << dir_predilecta)))
458 dir = dir_predilecta;
459 else if (!(vec & (1 << 1)))
461 else if (!(vec & (1 << 3)))
463 else if (!(vec & (1 << 4)))
465 else if (!(vec & (1 << 6)))
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)
474 basis_x[1] += direccion_vecino_x[dir];
475 basis_y[1] += direccion_vecino_y[dir];
480 return tam_circ * 100;
496 unsigned char res = 0;
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);
563 float x,
float y,
float maxSearchDistance)
const 565 int xx1 = max(0, x2idx(
x - maxSearchDistance));
567 min(static_cast<unsigned>(size_x - 1),
568 static_cast<unsigned>(x2idx(
x + maxSearchDistance)));
569 int yy1 = max(0, y2idx(
y - maxSearchDistance));
571 min(static_cast<unsigned>(size_y - 1),
572 static_cast<unsigned>(y2idx(
y + maxSearchDistance)));
578 float clearance_sq =
square(maxSearchDistance);
579 cellType thresholdCellValue = p2l(0.5f);
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;
587 if (!atLeastOneFree)
return 0;
589 for (xx = xx1; xx <= xx2; xx++)
590 for (yy = yy1; yy <= yy2; yy++)
591 if (map[xx + yy * size_x] < thresholdCellValue)
596 return sqrt(clearance_sq);
#define ASSERT_EQUAL_(__A, __B)
void findCriticalPoints(float filter_distance)
Builds a list with the critical points from Voronoi diagram, which must must be built before calling ...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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.
void clear()
Clear the contents of this container.
T square(const T x)
Inline function for the square of a number.
This namespace contains representation of robot actions and observations.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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.
int round(const T value)
Returns the closer integer (int) to x.
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...
int16_t cellType
The type of the map cells: