32 double COccupancyGridMap2D::internal_computeObservationLikelihood(
40 if (!scan.isPlanarScan(insertionOptions.horizontalTolerance))
42 if (insertionOptions.useMapAltitude &&
43 fabs(insertionOptions.mapAltitude - scan.sensorPose.z()) > 0.01)
54 switch (likelihoodOptions.likelihoodMethod)
58 return computeObservationLikelihood_rayTracing(obs, takenFrom);
60 case lmMeanInformation:
61 return computeObservationLikelihood_MI(obs, takenFrom);
64 return computeObservationLikelihood_Consensus(obs, takenFrom);
66 case lmCellsDifference:
67 return computeObservationLikelihood_CellsDifference(obs, takenFrom);
69 case lmLikelihoodField_Thrun:
70 return computeObservationLikelihood_likelihoodField_Thrun(
73 case lmLikelihoodField_II:
74 return computeObservationLikelihood_likelihoodField_II(
78 return computeObservationLikelihood_ConsensusOWA(obs, takenFrom);
85 double COccupancyGridMap2D::computeObservationLikelihood_Consensus(
104 if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
120 const size_t n = compareMap->
size();
122 for (
size_t i = 0; i < n; i += likelihoodOptions.consensus_takeEachRange)
125 compareMap->getPoint(i, pointLocal);
128 int cx0 = x2idx(pointGlobal.
x);
129 int cy0 = y2idx(pointGlobal.
y);
131 likResult += 1 - getCell_nocheck(cx0, cy0);
134 if (Denom) likResult /= Denom;
136 pow(likResult,
static_cast<double>(likelihoodOptions.consensus_pow));
138 return log(likResult);
144 double COccupancyGridMap2D::computeObservationLikelihood_ConsensusOWA(
147 double likResult = 0;
163 if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
171 const auto* compareMap =
180 const size_t n = compareMap->
size();
183 likelihoodOutputs.OWA_pairList.clear();
184 for (
size_t i = 0; i < n; i++)
187 compareMap->getPoint(i, pointLocal);
190 int cx0 = x2idx(pointGlobal.
x);
191 int cy0 = y2idx(pointGlobal.
y);
193 int cxMin = max(0, cx0 - Acells);
194 int cxMax = min(
static_cast<int>(size_x) - 1, cx0 + Acells);
195 int cyMin = max(0, cy0 - Acells);
196 int cyMax = min(
static_cast<int>(size_y) - 1, cy0 + Acells);
200 for (
int cx = cxMin; cx <= cxMax; cx++)
201 for (
int cy = cyMin; cy <= cyMax; cy++)
202 lik += 1 - getCell_nocheck(cx, cy);
204 int nCells = (cxMax - cxMin + 1) * (cyMax - cyMin + 1);
210 element.second = pointGlobal;
211 likelihoodOutputs.OWA_pairList.push_back(element);
217 likelihoodOutputs.OWA_pairList.begin(),
218 likelihoodOutputs.OWA_pairList.end());
221 size_t M = likelihoodOptions.OWA_weights.size();
222 ASSERT_(likelihoodOutputs.OWA_pairList.size() >= M);
224 likelihoodOutputs.OWA_pairList.resize(M);
225 likelihoodOutputs.OWA_individualLikValues.resize(M);
227 for (
size_t k = 0; k < M; k++)
229 likelihoodOutputs.OWA_individualLikValues[k] =
230 likelihoodOutputs.OWA_pairList[k].first;
231 likResult += likelihoodOptions.OWA_weights[k] *
232 likelihoodOutputs.OWA_individualLikValues[k];
235 return log(likResult);
241 double COccupancyGridMap2D::computeObservationLikelihood_CellsDifference(
256 if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
261 takenFrom.
x() - 10, takenFrom.
x() + 10, takenFrom.
y() - 10,
262 takenFrom.
y() + 10, resolution);
268 insertionOptions.maxDistanceInsertion;
270 o.insertObservationInto(&compareGrid, &robotPose);
273 Ax =
round((x_min - compareGrid.
x_min) / resolution);
274 Ay =
round((y_min - compareGrid.
y_min) / resolution);
276 int nCellsCompared = 0;
277 float cellsDifference = 0;
280 int x1 = min(compareGrid.
size_x, size_x + Ax);
281 int y1 = min(compareGrid.
size_y, size_y + Ay);
283 for (
int x = x0; x < x1; x += 1)
285 for (
int y = y0; y < y1; y += 1)
287 float xx = compareGrid.
idx2x(x);
288 float yy = compareGrid.
idx2y(y);
290 float c1 = getPos(xx, yy);
291 float c2 = compareGrid.
getCell(x, y);
292 if (c2 < 0.45f || c2 > 0.55f)
295 if ((c1 > 0.5 && c2 < 0.5) || (c1 < 0.5 && c2 > 0.5))
300 ret = 1 - cellsDifference / (nCellsCompared);
308 double COccupancyGridMap2D::computeObservationLikelihood_MI(
317 updateInfoChangeOnly.enabled =
true;
318 insertionOptions.maxDistanceInsertion *=
319 likelihoodOptions.MI_ratio_max_distance;
322 updateInfoChangeOnly.cellsUpdated = 0;
323 updateInfoChangeOnly.I_change = 0;
324 updateInfoChangeOnly.laserRaysSkip = likelihoodOptions.MI_skip_rays;
328 insertObservation(obs, &poseRobot);
331 double newObservation_mean_I;
332 if (updateInfoChangeOnly.cellsUpdated)
333 newObservation_mean_I =
334 updateInfoChangeOnly.I_change / updateInfoChangeOnly.cellsUpdated;
336 newObservation_mean_I = 0;
339 updateInfoChangeOnly.enabled =
false;
340 insertionOptions.maxDistanceInsertion /=
341 likelihoodOptions.MI_ratio_max_distance;
344 pow(newObservation_mean_I,
345 static_cast<double>(likelihoodOptions.MI_exponent));
352 double COccupancyGridMap2D::computeObservationLikelihood_rayTracing(
368 if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
373 int decimation = likelihoodOptions.rayTracing_decimation;
390 double stdLaser = likelihoodOptions.rayTracing_stdHit;
391 double stdSqrt2 = sqrt(2.0f) * stdLaser;
399 for (
int j = 0; j < nRays; j += decimation)
403 r_obs = o.getScanRange(j);
406 if (o.getScanRangeValidity(j))
412 min((
float)fabs(r_sim - r_obs), 2.0f) / stdSqrt2));
413 ret += log(likelihood);
426 double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_Thrun(
443 if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
return -10;
453 ret = computeLikelihoodField_Thrun(
469 ret = computeLikelihoodField_Thrun(&pts, &takenFrom);
480 double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_II(
497 if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
504 ret = computeLikelihoodField_II(
517 double COccupancyGridMap2D::computeLikelihoodField_Thrun(
523 size_t N = pm->
size();
525 likelihoodOptions.LF_maxCorrsDistance /
528 bool Product_T_OrSum_F = !likelihoodOptions.LF_alternateAverageMethod;
538 float stdHit = likelihoodOptions.LF_stdHit;
539 float zHit = likelihoodOptions.LF_zHit;
540 float zRandom = likelihoodOptions.LF_zRandom;
541 float zRandomMaxRange = likelihoodOptions.LF_maxRange;
542 float zRandomTerm = zRandom / zRandomMaxRange;
543 float Q = -0.5f /
square(stdHit);
546 unsigned int size_x_1 = size_x - 1;
547 unsigned int size_y_1 = size_y - 1;
549 #define LIK_LF_CACHE_INVALID (66)
553 double maxCorrDist_sq =
square(likelihoodOptions.LF_maxCorrsDistance);
554 double minimumLik = zRandomTerm + zHit * exp(Q * maxCorrDist_sq);
557 if (likelihoodOptions.enableLikelihoodCache)
560 if (m_likelihoodCacheOutDated)
565 precomputedLikelihood.clear();
567 m_likelihoodCacheOutDated =
false;
571 cellType thresholdCellValue = p2l(0.5f);
572 int decimation = likelihoodOptions.LF_decimation;
574 const double _resolution = this->resolution;
575 const double constDist2DiscrUnits = 100 / (_resolution * _resolution);
576 const double constDist2DiscrUnits_INV = 1.0 / constDist2DiscrUnits;
578 if (N < 10) decimation = 1;
583 for (
size_t j = 0; j < N; j += decimation)
591 ::sincos(relativePose->
phi(), &ssin, &ccos);
593 ccos = cos(relativePose->
phi());
594 ssin = sin(relativePose->
phi());
597 relativePose->
x() + pointLocal.
x * ccos - pointLocal.
y * ssin;
599 relativePose->
y() + pointLocal.
x * ssin + pointLocal.
y * ccos;
607 int cx = x2idx(pointGlobal.
x);
608 int cy = y2idx(pointGlobal.
y);
612 if (
static_cast<unsigned>(cx) >= size_x_1 ||
613 static_cast<unsigned>(cy) >= size_y_1)
617 thisLik = minimumLik;
622 if (likelihoodOptions.enableLikelihoodCache)
624 thisLik = precomputedLikelihood[cx + cy * size_x];
627 if (!likelihoodOptions.enableLikelihoodCache ||
634 int xx1 = max(0, cx - K);
635 int xx2 = min(size_x_1, (
unsigned)(cx + K));
636 int yy1 = max(0, cy - K);
637 int yy2 = min(size_y_1, (
unsigned)(cy + K));
640 float occupiedMinDist;
643 &map[xx1 + yy1 * size_x];
644 unsigned incrAfterRow = size_x - ((xx2 - xx1) + 1);
646 signed int Ax0 = 10 * (xx1 - cx);
647 signed int Ay = 10 * (yy1 - cy);
649 unsigned int occupiedMinDistInt =
650 mrpt::round(maxCorrDist_sq * constDist2DiscrUnits);
652 for (
int yy = yy1; yy <= yy2; yy++)
655 square((
unsigned int)(Ay));
657 signed short Ax = Ax0;
660 for (
int xx = xx1; xx <= xx2; xx++)
662 if ((cell = *mapPtr++) < thresholdCellValue)
665 square((
unsigned int)(Ax)) + Ay2;
671 mapPtr += incrAfterRow;
676 occupiedMinDistInt * constDist2DiscrUnits_INV;
679 if (likelihoodOptions.LF_useSquareDist)
680 occupiedMinDist *= occupiedMinDist;
682 thisLik = zRandomTerm + zHit * exp(Q * occupiedMinDist);
684 if (likelihoodOptions.enableLikelihoodCache)
686 precomputedLikelihood[cx + cy * size_x] = thisLik;
691 if (Product_T_OrSum_F)
702 if (!Product_T_OrSum_F) ret = log(ret / M);
712 double COccupancyGridMap2D::computeLikelihoodField_II(
718 size_t N = pm->
size();
720 if (!N)
return 1e-100;
730 float zRandomTerm = 1.0f / likelihoodOptions.LF_maxRange;
731 float Q = -0.5f /
square(likelihoodOptions.LF_stdHit);
739 int maxRangeInCells =
740 (int)ceil(likelihoodOptions.LF_maxCorrsDistance / resolution);
746 for (j = 0; j < N; j += likelihoodOptions.LF_decimation)
753 pointGlobal = *relativePose + pointLocal;
762 cx0 = x2idx(pointGlobal.
x);
763 cy0 = y2idx(pointGlobal.
y);
767 cx_min = max(cx0 - maxRangeInCells, 0);
768 cx_max = min(cx0 + maxRangeInCells,
static_cast<int>(size_x));
769 cy_min = max(cy0 - maxRangeInCells, 0);
770 cy_max = min(cy0 + maxRangeInCells,
static_cast<int>(size_y));
777 for (cx = cx_min; cx <= cx_max; cx++)
779 for (cy = cy_min; cy <= cy_max; cy++)
781 float P_free = getCell(cx, cy);
783 exp(Q * (
square(idx2x(cx) - pointGlobal.
x) +
784 square(idx2y(cy) - pointGlobal.
y)));
786 lik += P_free * zRandomTerm + (1 - P_free) * termDist;
791 if (likelihoodOptions.LF_alternateAverageMethod)
794 ret += log(lik / ((cy_max - cy_min + 1) * (cx_max - cx_min + 1)));
799 if (likelihoodOptions.LF_alternateAverageMethod && nCells > 0)
800 ret = log(ret / nCells);
812 COccupancyGridMap2D::TLikelihoodOptions::TLikelihoodOptions()
813 : OWA_weights(100, 1 / 100.0f)
825 iniFile.read_enum(section,
"likelihoodMethod", likelihoodMethod);
827 enableLikelihoodCache =
iniFile.read_bool(
828 section,
"enableLikelihoodCache", enableLikelihoodCache);
830 LF_stdHit =
iniFile.read_float(section,
"LF_stdHit", LF_stdHit);
831 LF_zHit =
iniFile.read_float(section,
"LF_zHit", LF_zHit);
832 LF_zRandom =
iniFile.read_float(section,
"LF_zRandom", LF_zRandom);
833 LF_maxRange =
iniFile.read_float(section,
"LF_maxRange", LF_maxRange);
834 LF_decimation =
iniFile.read_int(section,
"LF_decimation", LF_decimation);
835 LF_maxCorrsDistance =
836 iniFile.read_float(section,
"LF_maxCorrsDistance", LF_maxCorrsDistance);
838 iniFile.read_bool(section,
"LF_useSquareDist", LF_useSquareDist);
839 LF_alternateAverageMethod =
iniFile.read_bool(
840 section,
"LF_alternateAverageMethod", LF_alternateAverageMethod);
842 MI_exponent =
iniFile.read_float(section,
"MI_exponent", MI_exponent);
843 MI_skip_rays =
iniFile.read_int(section,
"MI_skip_rays", MI_skip_rays);
844 MI_ratio_max_distance =
iniFile.read_float(
845 section,
"MI_ratio_max_distance", MI_ratio_max_distance);
847 rayTracing_useDistanceFilter =
iniFile.read_bool(
848 section,
"rayTracing_useDistanceFilter", rayTracing_useDistanceFilter);
850 iniFile.read_float(section,
"rayTracing_stdHit", rayTracing_stdHit);
852 consensus_takeEachRange =
iniFile.read_int(
853 section,
"consensus_takeEachRange", consensus_takeEachRange);
854 consensus_pow =
iniFile.read_float(section,
"consensus_pow", consensus_pow);
856 iniFile.read_vector(section,
"OWA_weights", OWA_weights, OWA_weights);
863 std::ostream&
out)
const
865 out <<
"\n----------- [COccupancyGridMap2D::TLikelihoodOptions] "
869 out <<
"likelihoodMethod = ";
870 switch (likelihoodMethod)
873 out <<
"lmMeanInformation";
876 out <<
"lmRayTracing";
879 out <<
"lmConsensus";
882 out <<
"lmCellsDifference";
885 out <<
"lmLikelihoodField_Thrun";
888 out <<
"lmLikelihoodField_II";
891 out <<
"lmConsensusOWA";
900 "enableLikelihoodCache = %c\n",
901 enableLikelihoodCache ?
'Y' :
'N');
904 "LF_stdHit = %f\n", LF_stdHit);
906 "LF_zHit = %f\n", LF_zHit);
908 "LF_zRandom = %f\n", LF_zRandom);
910 "LF_maxRange = %f\n", LF_maxRange);
912 "LF_decimation = %u\n", LF_decimation);
914 "LF_maxCorrsDistance = %f\n", LF_maxCorrsDistance);
916 "LF_useSquareDist = %c\n",
917 LF_useSquareDist ?
'Y' :
'N');
919 "LF_alternateAverageMethod = %c\n",
920 LF_alternateAverageMethod ?
'Y' :
'N');
922 "MI_exponent = %f\n", MI_exponent);
924 "MI_skip_rays = %u\n", MI_skip_rays);
926 "MI_ratio_max_distance = %f\n",
927 MI_ratio_max_distance);
929 "rayTracing_useDistanceFilter = %c\n",
930 rayTracing_useDistanceFilter ?
'Y' :
'N');
932 "rayTracing_decimation = %u\n",
933 rayTracing_decimation);
935 "rayTracing_stdHit = %f\n", rayTracing_stdHit);
937 "consensus_takeEachRange = %u\n",
938 consensus_takeEachRange);
940 "consensus_pow = %.02f\n", consensus_pow);
941 out <<
"OWA_weights = [";
942 for (
size_t i = 0; i < OWA_weights.size(); i++)
944 if (i < 3 || i > (OWA_weights.size() - 3))
946 else if (i == 3 && OWA_weights.size() > 6)