31 "mrpt::maps::COccupancyGridMap2D,occupancyGrid",
34 COccupancyGridMap2D::TMapDefinition::TMapDefinition() =
default;
36 void COccupancyGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(
38 const std::string& sectionNamePrefix)
41 const std::string sSectCreation =
42 sectionNamePrefix + string(
"_creationOpts");
50 insertionOpts.loadFromConfigFile(
51 source, sectionNamePrefix +
string(
"_insertOpts"));
54 likelihoodOpts.loadFromConfigFile(
55 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
58 void COccupancyGridMap2D::TMapDefinition::dumpToTextStream_map_specific(
59 std::ostream&
out)
const
67 this->insertionOpts.dumpToTextStream(
out);
68 this->likelihoodOpts.dumpToTextStream(
out);
86 std::vector<float> COccupancyGridMap2D::entropyTable;
88 static const float MAX_H = 0.69314718055994531f;
93 COccupancyGridMap2D::get_logodd_lut()
101 COccupancyGridMap2D::COccupancyGridMap2D(
102 float min_x,
float max_x,
float min_y,
float max_y,
float res)
105 precomputedLikelihood(),
110 updateInfoChangeOnly(),
117 setSize(min_x, max_x, min_y, max_y, res, 0.5f);
141 float xmin,
float xmax,
float ymin,
float ymax,
float res,
157 xmin = res *
round(xmin / res);
158 ymin = res *
round(ymin / res);
159 xmax = res *
round(xmax / res);
160 ymax = res *
round(ymax / res);
173 #ifdef ROWSIZE_MULTIPLE_16
197 float new_x_min,
float new_x_max,
float new_y_min,
float new_y_max,
198 float new_cells_default_value,
bool additionalMargin) noexcept
200 unsigned int extra_x_izq = 0, extra_y_arr = 0, new_size_x = 0,
202 std::vector<cellType> new_map;
204 if (new_x_min > new_x_max)
207 "[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: "
208 "x_min=%f x_max=%f\n",
209 new_x_min, new_x_max);
212 if (new_y_min > new_y_max)
215 "[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: "
216 "y_min=%f y_max=%f\n",
217 new_y_min, new_y_max);
222 if (new_x_min >= x_min && new_y_min >= y_min && new_x_max <= x_max &&
227 m_likelihoodCacheOutDated =
true;
230 if (additionalMargin)
232 if (new_x_min < x_min) new_x_min = floor(new_x_min - 4);
233 if (new_x_max > x_max) new_x_max = ceil(new_x_max + 4);
234 if (new_y_min < y_min) new_y_min = floor(new_y_min - 4);
235 if (new_y_max > y_max) new_y_max = ceil(new_y_max + 4);
239 new_x_min = min(new_x_min, x_min);
240 new_x_max = max(new_x_max, x_max);
241 new_y_min = min(new_y_min, y_min);
242 new_y_max = max(new_y_max, y_max);
246 if (fabs(new_x_min / resolution -
round(new_x_min / resolution)) > 0.05f)
247 new_x_min = resolution *
round(new_x_min / resolution);
248 if (fabs(new_y_min / resolution -
round(new_y_min / resolution)) > 0.05f)
249 new_y_min = resolution *
round(new_y_min / resolution);
250 if (fabs(new_x_max / resolution -
round(new_x_max / resolution)) > 0.05f)
251 new_x_max = resolution *
round(new_x_max / resolution);
252 if (fabs(new_y_max / resolution -
round(new_y_max / resolution)) > 0.05f)
253 new_y_max = resolution *
round(new_y_max / resolution);
256 extra_x_izq =
round((x_min - new_x_min) / resolution);
257 extra_y_arr =
round((y_min - new_y_min) / resolution);
259 new_size_x =
round((new_x_max - new_x_min) / resolution);
260 new_size_y =
round((new_y_max - new_y_min) / resolution);
262 assert(new_size_x >= size_x + extra_x_izq);
264 #ifdef ROWSIZE_MULTIPLE_16
266 size_t old_new_size_x = new_size_x;
267 if (0 != (new_size_x % 16))
269 int size_x_incr = 16 - (new_size_x % 16);
271 new_x_max += size_x_incr * resolution;
273 new_size_x =
round((new_x_max - new_x_min) / resolution);
274 assert(0 == (new_size_x % 16));
278 new_map.resize(new_size_x * new_size_y, p2l(new_cells_default_value));
282 cellType* dest_ptr = &new_map[extra_x_izq + extra_y_arr * new_size_x];
284 size_t row_size = size_x *
sizeof(
cellType);
286 for (
size_t y = 0; y < size_y; y++)
288 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
289 assert(dest_ptr + row_size - 1 <= &new_map[new_map.size() - 1]);
290 assert(src_ptr + row_size - 1 <= &map[map.size() - 1]);
292 memcpy(dest_ptr, src_ptr, row_size);
293 dest_ptr += new_size_x;
312 m_voronoi_diagram.clear();
354 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
365 for (
size_t i = 0; i < N; i++)
368 auto h =
d2f(
H(p) +
H(1 - p));
371 if (i == 0 || i == (N - 1)) h = 0;
384 for (
signed char it :
map)
389 if (h < (
MAX_H - 0.001f))
432 for (
auto it =
map.begin(); it <
map.end(); ++it) *it = defValue;
443 if (
static_cast<unsigned int>(x) >=
size_x ||
444 static_cast<unsigned int>(y) >=
size_y)
453 float old =
l2p(theCell);
454 float new_v = 1 / (1 + (1 - v) * (1 - old) / (old * v));
484 std::vector<cellType> newMap;
493 newMap.resize(newSizeX * newSizeY);
495 for (
int x = 0; x < newSizeX; x++)
497 for (
int y = 0; y < newSizeY; y++)
501 for (
int xx = 0; xx < downRatio; xx++)
502 for (
int yy = 0; yy < downRatio; yy++)
503 newCell +=
getCell(x * downRatio + xx, y * downRatio + yy);
505 newCell /= (downRatio * downRatio);
507 newMap[x + y * newSizeX] =
p2l(newCell);
529 params.offset_other_map_points,
params.decimation_other_map_points);
532 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
536 const size_t nLocalPoints = otherMap->
size();
537 std::vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
538 z_locals(nLocalPoints);
540 const float sin_phi = sin(otherMapPose.
phi);
541 const float cos_phi = cos(otherMapPose.
phi);
543 size_t nOtherMapPointsWithCorrespondence =
545 size_t nTotalCorrespondences = 0;
546 float _sumSqrDist = 0;
549 const int cellsSearchRange =
553 correspondences.clear();
556 if (!nLocalPoints)
return;
561 float local_x_min = std::numeric_limits<float>::max();
562 float local_x_max = -std::numeric_limits<float>::max();
563 float local_y_min = std::numeric_limits<float>::max();
564 float local_y_max = -std::numeric_limits<float>::max();
566 const auto& otherMap_pxs = otherMap->getPointsBufferRef_x();
567 const auto& otherMap_pys = otherMap->getPointsBufferRef_y();
568 const auto& otherMap_pzs = otherMap->getPointsBufferRef_z();
571 for (
unsigned int localIdx =
params.offset_other_map_points;
572 localIdx < nLocalPoints;
573 localIdx +=
params.decimation_other_map_points)
576 const float xx = x_locals[localIdx] = otherMapPose.
x +
577 cos_phi * otherMap_pxs[localIdx] -
578 sin_phi * otherMap_pys[localIdx];
579 const float yy = y_locals[localIdx] = otherMapPose.
y +
580 sin_phi * otherMap_pxs[localIdx] +
581 cos_phi * otherMap_pys[localIdx];
582 z_locals[localIdx] = otherMap_pzs[localIdx];
585 local_x_min = min(local_x_min, xx);
586 local_x_max = max(local_x_max, xx);
587 local_y_min = min(local_y_min, yy);
588 local_y_max = max(local_y_max, yy);
593 if (local_x_min >
x_max || local_x_max < x_min || local_y_min >
y_max ||
600 for (
unsigned int localIdx =
params.offset_other_map_points;
601 localIdx < nLocalPoints;
602 localIdx +=
params.decimation_other_map_points)
605 float maxDistForCorrespondenceSquared =
609 const float x_local = x_locals[localIdx];
610 const float y_local = y_locals[localIdx];
611 const float z_local = z_locals[localIdx];
614 float min_dist = 1e6;
618 const int cx0 =
x2idx(x_local);
619 const int cy0 =
y2idx(y_local);
622 const int cx_min = max(0, cx0 - cellsSearchRange);
624 min(
static_cast<int>(
size_x) - 1, cx0 + cellsSearchRange);
625 const int cy_min = max(0, cy0 - cellsSearchRange);
627 min(
static_cast<int>(
size_y) - 1, cy0 + cellsSearchRange);
630 bool thisLocalHasCorr =
false;
633 for (
int cx = cx_min; cx <= cx_max; cx++)
635 for (
int cy = cy_min; cy <= cy_max; cy++)
641 const float residual_x =
idx2x(cx) - x_local;
642 const float residual_y =
idx2y(cy) - y_local;
645 maxDistForCorrespondenceSquared =
square(
646 params.maxAngularDistForCorrespondence *
647 params.angularDistPivotPoint.distanceTo(
649 params.maxDistForCorrespondence);
652 const float this_dist =
655 if (this_dist < maxDistForCorrespondenceSquared)
657 if (!
params.onlyKeepTheClosest)
660 nTotalCorrespondences++;
667 mp.
other_x = otherMap_pxs[localIdx];
668 mp.
other_y = otherMap_pys[localIdx];
669 mp.
other_z = otherMap_pzs[localIdx];
670 correspondences.push_back(mp);
675 if (this_dist < min_dist)
677 min_dist = this_dist;
682 closestCorr.
this_z = z_local;
684 closestCorr.
other_x = otherMap_pxs[localIdx];
685 closestCorr.
other_y = otherMap_pys[localIdx];
686 closestCorr.
other_z = otherMap_pzs[localIdx];
691 thisLocalHasCorr =
true;
698 if (
params.onlyKeepTheClosest &&
699 (min_dist < maxDistForCorrespondenceSquared))
701 nTotalCorrespondences++;
702 correspondences.push_back(closestCorr);
706 if (thisLocalHasCorr)
708 nOtherMapPointsWithCorrespondence++;
711 _sumSqrDist += min_dist;
716 extraResults.correspondencesRatio =
717 nOtherMapPointsWithCorrespondence /
718 d2f(nLocalPoints /
params.decimation_other_map_points);
719 extraResults.sumSqrDist = _sumSqrDist;
735 return e1.first > e2.first;
742 float x1,
float y1,
float x2,
float y2)
const
749 for (
int i = 0; i < nSteps; i++)
751 float x = x1 + (x2 - x1) * i /
d2f(nSteps);
752 float y = y1 + (y2 - y1) * i /
d2f(nSteps);
757 return sumCost /
d2f(nSteps);