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);
267 compareGrid.insertionOptions.maxDistanceInsertion =
268 insertionOptions.maxDistanceInsertion;
269 compareGrid.insertionOptions.maxOccupancyUpdateCertainty = 0.95f;
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)
402 r_sim = simulatedObs.
scan[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);
556 float occupiedMinDist;
558 if (likelihoodOptions.enableLikelihoodCache)
561 if (m_likelihoodCacheOutDated)
566 precomputedLikelihood.clear();
568 m_likelihoodCacheOutDated =
false;
572 cellType thresholdCellValue = p2l(0.5f);
573 int decimation = likelihoodOptions.LF_decimation;
575 const double _resolution = this->resolution;
576 const double constDist2DiscrUnits = 100 / (_resolution * _resolution);
577 const double constDist2DiscrUnits_INV = 1.0 / constDist2DiscrUnits;
579 if (N < 10) decimation = 1;
584 for (
size_t j = 0; j < N; j += decimation)
586 occupiedMinDist = maxCorrDist_sq;
594 ::sincos(relativePose->
phi(), &ssin, &ccos);
596 ccos = cos(relativePose->
phi());
597 ssin = sin(relativePose->
phi());
600 relativePose->
x() + pointLocal.
x * ccos - pointLocal.
y * ssin;
602 relativePose->
y() + pointLocal.
x * ssin + pointLocal.
y * ccos;
610 int cx = x2idx(pointGlobal.
x);
611 int cy = y2idx(pointGlobal.
y);
615 if (static_cast<unsigned>(cx) >= size_x_1 ||
616 static_cast<unsigned>(cy) >= size_y_1)
620 thisLik = minimumLik;
625 if (likelihoodOptions.enableLikelihoodCache)
627 thisLik = precomputedLikelihood[cx + cy * size_x];
630 if (!likelihoodOptions.enableLikelihoodCache ||
637 int xx1 = max(0, cx - K);
638 int xx2 =
min(size_x_1, (
unsigned)(cx + K));
639 int yy1 = max(0, cy - K);
640 int yy2 =
min(size_y_1, (
unsigned)(cy + K));
645 &map[xx1 + yy1 * size_x];
646 unsigned incrAfterRow = size_x - ((xx2 - xx1) + 1);
648 signed int Ax0 = 10 * (xx1 - cx);
649 signed int Ay = 10 * (yy1 - cy);
651 unsigned int occupiedMinDistInt =
652 mrpt::round(maxCorrDist_sq * constDist2DiscrUnits);
654 for (
int yy = yy1; yy <= yy2; yy++)
657 square((
unsigned int)(Ay));
659 signed short Ax = Ax0;
662 for (
int xx = xx1; xx <= xx2; xx++)
664 if ((cell = *mapPtr++) < thresholdCellValue)
667 square((
unsigned int)(Ax)) + Ay2;
673 mapPtr += incrAfterRow;
678 occupiedMinDistInt * constDist2DiscrUnits_INV;
681 if (likelihoodOptions.LF_useSquareDist)
682 occupiedMinDist *= occupiedMinDist;
684 thisLik = zRandomTerm + zHit * exp(Q * occupiedMinDist);
686 if (likelihoodOptions.enableLikelihoodCache)
688 precomputedLikelihood[cx + cy * size_x] = thisLik;
693 if (Product_T_OrSum_F)
704 if (!Product_T_OrSum_F) ret = log(ret / M);
714 double COccupancyGridMap2D::computeLikelihoodField_II(
720 size_t N = pm->
size();
722 if (!N)
return 1e-100;
732 float zRandomTerm = 1.0f / likelihoodOptions.LF_maxRange;
733 float Q = -0.5f /
square(likelihoodOptions.LF_stdHit);
741 int maxRangeInCells =
742 (int)ceil(likelihoodOptions.LF_maxCorrsDistance / resolution);
748 for (j = 0; j < N; j += likelihoodOptions.LF_decimation)
755 pointGlobal = *relativePose + pointLocal;
764 cx0 = x2idx(pointGlobal.
x);
765 cy0 = y2idx(pointGlobal.
y);
769 cx_min = max(cx0 - maxRangeInCells, 0);
770 cx_max =
min(cx0 + maxRangeInCells, static_cast<int>(size_x));
771 cy_min = max(cy0 - maxRangeInCells, 0);
772 cy_max =
min(cy0 + maxRangeInCells, static_cast<int>(size_y));
779 for (cx = cx_min; cx <= cx_max; cx++)
781 for (cy = cy_min; cy <= cy_max; cy++)
783 float P_free = getCell(cx, cy);
785 exp(Q * (
square(idx2x(cx) - pointGlobal.
x) +
786 square(idx2y(cy) - pointGlobal.
y)));
788 lik += P_free * zRandomTerm + (1 - P_free) * termDist;
793 if (likelihoodOptions.LF_alternateAverageMethod)
796 ret += log(lik / ((cy_max - cy_min + 1) * (cx_max - cx_min + 1)));
801 if (likelihoodOptions.LF_alternateAverageMethod && nCells > 0)
802 ret = log(ret / nCells);
814 COccupancyGridMap2D::TLikelihoodOptions::TLikelihoodOptions()
815 : OWA_weights(100, 1 / 100.0f)
827 iniFile.read_enum(section,
"likelihoodMethod", likelihoodMethod);
829 enableLikelihoodCache =
iniFile.read_bool(
830 section,
"enableLikelihoodCache", enableLikelihoodCache);
832 LF_stdHit =
iniFile.read_float(section,
"LF_stdHit", LF_stdHit);
833 LF_zHit =
iniFile.read_float(section,
"LF_zHit", LF_zHit);
834 LF_zRandom =
iniFile.read_float(section,
"LF_zRandom", LF_zRandom);
835 LF_maxRange =
iniFile.read_float(section,
"LF_maxRange", LF_maxRange);
836 LF_decimation =
iniFile.read_int(section,
"LF_decimation", LF_decimation);
837 LF_maxCorrsDistance =
838 iniFile.read_float(section,
"LF_maxCorrsDistance", LF_maxCorrsDistance);
840 iniFile.read_bool(section,
"LF_useSquareDist", LF_useSquareDist);
841 LF_alternateAverageMethod =
iniFile.read_bool(
842 section,
"LF_alternateAverageMethod", LF_alternateAverageMethod);
844 MI_exponent =
iniFile.read_float(section,
"MI_exponent", MI_exponent);
845 MI_skip_rays =
iniFile.read_int(section,
"MI_skip_rays", MI_skip_rays);
846 MI_ratio_max_distance =
iniFile.read_float(
847 section,
"MI_ratio_max_distance", MI_ratio_max_distance);
849 rayTracing_useDistanceFilter =
iniFile.read_bool(
850 section,
"rayTracing_useDistanceFilter", rayTracing_useDistanceFilter);
852 iniFile.read_float(section,
"rayTracing_stdHit", rayTracing_stdHit);
854 consensus_takeEachRange =
iniFile.read_int(
855 section,
"consensus_takeEachRange", consensus_takeEachRange);
856 consensus_pow =
iniFile.read_float(section,
"consensus_pow", consensus_pow);
858 iniFile.read_vector(section,
"OWA_weights", OWA_weights, OWA_weights);
865 std::ostream& out)
const 868 "\n----------- [COccupancyGridMap2D::TLikelihoodOptions] ------------ " 872 switch (likelihoodMethod)
902 "enableLikelihoodCache = %c\n",
903 enableLikelihoodCache ?
'Y' :
'N');
906 "LF_stdHit = %f\n", LF_stdHit);
908 "LF_zHit = %f\n", LF_zHit);
910 "LF_zRandom = %f\n", LF_zRandom);
912 "LF_maxRange = %f\n", LF_maxRange);
914 "LF_decimation = %u\n", LF_decimation);
916 "LF_maxCorrsDistance = %f\n", LF_maxCorrsDistance);
918 "LF_useSquareDist = %c\n",
919 LF_useSquareDist ?
'Y' :
'N');
921 "LF_alternateAverageMethod = %c\n",
922 LF_alternateAverageMethod ?
'Y' :
'N');
924 "MI_exponent = %f\n", MI_exponent);
926 "MI_skip_rays = %u\n", MI_skip_rays);
928 "MI_ratio_max_distance = %f\n",
929 MI_ratio_max_distance);
931 "rayTracing_useDistanceFilter = %c\n",
932 rayTracing_useDistanceFilter ?
'Y' :
'N');
934 "rayTracing_decimation = %u\n",
935 rayTracing_decimation);
937 "rayTracing_stdHit = %f\n", rayTracing_stdHit);
939 "consensus_takeEachRange = %u\n",
940 consensus_takeEachRange);
942 "consensus_pow = %.02f\n", consensus_pow);
944 for (
size_t i = 0; i < OWA_weights.size(); i++)
946 if (i < 3 || i > (OWA_weights.size() - 3))
948 else if (i == 3 && OWA_weights.size() > 6)
951 out <<
mrpt::format(
"] (size=%u)\n", (
unsigned)OWA_weights.size());
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation &obs) const override
Returns true if this map is able to compute a sensible likelihood function for this observation (i...
float maxRange
The maximum range allowed by the device, in meters (e.g.
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
With this struct options are provided to the observation insertion process.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
#define LIK_LF_CACHE_INVALID
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
double x() const
Common members of all points & poses classes.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini" file.
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
constexpr std::size_t size() const
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, then processed by calls to this class (default=0).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true...
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map! ...
OccGridCellTraits::cellType cellType
The type of the map cells:
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
size_t size() const
Returns the number of stored points in the map.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
int round(const T value)
Returns the closer integer (int) to x.