COccupancyGridMap2D_likelihood.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "maps-precomp.h"  // Precomp header

#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace std;

/*---------------------------------------------------------------
 Computes the likelihood that a given observation was taken from a given pose in
 the world being modeled with this map.
    takenFrom The robot's pose the observation is supposed to be taken from.
    obs The observation.
 This method returns a likelihood in the range [0,1].
 ---------------------------------------------------------------*/
double COccupancyGridMap2D::internal_computeObservationLikelihood(
    const CObservation& obs, const CPose3D& takenFrom3D)
{
    // Ignore laser scans if they are not planar or they are not
    //  at the altitude of this grid map:
    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        const auto& scan = static_cast<const CObservation2DRangeScan&>(obs);
        if (!scan.isPlanarScan(insertionOptions.horizontalTolerance))
            return -10;
        if (insertionOptions.useMapAltitude &&
            fabs(insertionOptions.mapAltitude - scan.sensorPose.z()) > 0.01)
            return -10;

        // OK, go on...
    }

    // Execute according to the selected method:
    // --------------------------------------------
    CPose2D takenFrom =
        CPose2D(takenFrom3D);  // 3D -> 2D, we are in a gridmap...

    switch (likelihoodOptions.likelihoodMethod)
    {
        default:
        case lmRayTracing:
            return computeObservationLikelihood_rayTracing(obs, takenFrom);

        case lmMeanInformation:
            return computeObservationLikelihood_MI(obs, takenFrom);

        case lmConsensus:
            return computeObservationLikelihood_Consensus(obs, takenFrom);

        case lmCellsDifference:
            return computeObservationLikelihood_CellsDifference(obs, takenFrom);

        case lmLikelihoodField_Thrun:
            return computeObservationLikelihood_likelihoodField_Thrun(
                obs, takenFrom);

        case lmLikelihoodField_II:
            return computeObservationLikelihood_likelihoodField_II(
                obs, takenFrom);

        case lmConsensusOWA:
            return computeObservationLikelihood_ConsensusOWA(obs, takenFrom);
    };
}

/*---------------------------------------------------------------
            computeObservationLikelihood_Consensus
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_Consensus(
    const CObservation& obs, const CPose2D& takenFrom)
{
    double likResult = 0;

    // This function depends on the observation type:
    // -----------------------------------------------------
    if (!IS_CLASS(obs, CObservation2DRangeScan))
    {
        // THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan'
        // classes only.");
        return 1e-3;
    }
    // Observation is a laser range scan:
    // -------------------------------------------
    const auto& o = static_cast<const CObservation2DRangeScan&>(obs);

    // Insert only HORIZONTAL scans, since the grid is supposed to
    //  be a horizontal representation of space.
    if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
        return 0.5f;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

    // Assure we have a 2D points-map representation of the points from the
    // scan:
    const auto* compareMap = o.buildAuxPointsMap<mrpt::maps::CPointsMap>();

    // Observation is a points map:
    // -------------------------------------------
    size_t Denom = 0;
    //  int         Acells = 1;
    TPoint2D pointGlobal, pointLocal;

    // Get the points buffers:

    //  compareMap.getPointsBuffer( n, xs, ys, zs );
    const size_t n = compareMap->size();

    for (size_t i = 0; i < n; i += likelihoodOptions.consensus_takeEachRange)
    {
        // Get the point and pass it to global coordinates:
        compareMap->getPoint(i, pointLocal);
        takenFrom.composePoint(pointLocal, pointGlobal);

        int cx0 = x2idx(pointGlobal.x);
        int cy0 = y2idx(pointGlobal.y);

        likResult += 1 - getCell_nocheck(cx0, cy0);
        Denom++;
    }
    if (Denom) likResult /= Denom;
    likResult =
        pow(likResult, static_cast<double>(likelihoodOptions.consensus_pow));

    return log(likResult);
}

/*---------------------------------------------------------------
            computeObservationLikelihood_ConsensusOWA
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_ConsensusOWA(
    const CObservation& obs, const CPose2D& takenFrom)
{
    double likResult = 0;

    // This function depends on the observation type:
    // -----------------------------------------------------
    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        // THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan'
        // classes only.");
        return 1e-3;
    }
    // Observation is a laser range scan:
    // -------------------------------------------
    const auto& o = static_cast<const CObservation2DRangeScan&>(obs);

    // Insert only HORIZONTAL scans, since the grid is supposed to
    //  be a horizontal representation of space.
    if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
        return 0.5;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

    // Assure we have a 2D points-map representation of the points from the
    // scan:
    CPointsMap::TInsertionOptions insOpt;
    insOpt.minDistBetweenLaserPoints = -1;  // ALL the laser points

    const auto* compareMap =
        o.buildAuxPointsMap<mrpt::maps::CPointsMap>(&insOpt);

    // Observation is a points map:
    // -------------------------------------------
    int Acells = 1;
    TPoint2D pointGlobal, pointLocal;

    // Get the points buffers:
    const size_t n = compareMap->size();

    // Store the likelihood values in this vector:
    likelihoodOutputs.OWA_pairList.clear();
    for (size_t i = 0; i < n; i++)
    {
        // Get the point and pass it to global coordinates:
        compareMap->getPoint(i, pointLocal);
        takenFrom.composePoint(pointLocal, pointGlobal);

        int cx0 = x2idx(pointGlobal.x);
        int cy0 = y2idx(pointGlobal.y);

        int cxMin = max(0, cx0 - Acells);
        int cxMax = min(static_cast<int>(size_x) - 1, cx0 + Acells);
        int cyMin = max(0, cy0 - Acells);
        int cyMax = min(static_cast<int>(size_y) - 1, cy0 + Acells);

        double lik = 0;

        for (int cx = cxMin; cx <= cxMax; cx++)
            for (int cy = cyMin; cy <= cyMax; cy++)
                lik += 1 - getCell_nocheck(cx, cy);

        int nCells = (cxMax - cxMin + 1) * (cyMax - cyMin + 1);
        ASSERT_(nCells > 0);
        lik /= nCells;

        TPairLikelihoodIndex element;
        element.first = lik;
        element.second = pointGlobal;
        likelihoodOutputs.OWA_pairList.push_back(element);
    }  // for each range point

    // Sort the list of likelihood values, in descending order:
    // ------------------------------------------------------------
    std::sort(
        likelihoodOutputs.OWA_pairList.begin(),
        likelihoodOutputs.OWA_pairList.end());

    // Cut the vector to the highest "likelihoodOutputs.OWA_length" elements:
    size_t M = likelihoodOptions.OWA_weights.size();
    ASSERT_(likelihoodOutputs.OWA_pairList.size() >= M);

    likelihoodOutputs.OWA_pairList.resize(M);
    likelihoodOutputs.OWA_individualLikValues.resize(M);
    likResult = 0;
    for (size_t k = 0; k < M; k++)
    {
        likelihoodOutputs.OWA_individualLikValues[k] =
            likelihoodOutputs.OWA_pairList[k].first;
        likResult += likelihoodOptions.OWA_weights[k] *
                     likelihoodOutputs.OWA_individualLikValues[k];
    }

    return log(likResult);
}

/*---------------------------------------------------------------
            computeObservationLikelihood_CellsDifference
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_CellsDifference(
    const CObservation& obs, const CPose2D& takenFrom)
{
    double ret = 0.5;

    // This function depends on the observation type:
    // -----------------------------------------------------
    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        // Observation is a laser range scan:
        // -------------------------------------------
        const auto& o = static_cast<const CObservation2DRangeScan&>(obs);

        // Insert only HORIZONTAL scans, since the grid is supposed to
        //  be a horizontal representation of space.
        if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
            return 0.5;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

        // Build a copy of this occupancy grid:
        COccupancyGridMap2D compareGrid(
            takenFrom.x() - 10, takenFrom.x() + 10, takenFrom.y() - 10,
            takenFrom.y() + 10, resolution);
        CPose3D robotPose(takenFrom);
        int Ax, Ay;

        // Insert in this temporary grid:
        compareGrid.insertionOptions.maxDistanceInsertion =
            insertionOptions.maxDistanceInsertion;
        compareGrid.insertionOptions.maxOccupancyUpdateCertainty = 0.95f;
        o.insertObservationInto(&compareGrid, &robotPose);

        // Save Cells offset between the two grids:
        Ax = round((x_min - compareGrid.x_min) / resolution);
        Ay = round((y_min - compareGrid.y_min) / resolution);

        int nCellsCompared = 0;
        float cellsDifference = 0;
        int x0 = max(0, Ax);
        int y0 = max(0, Ay);
        int x1 = min(compareGrid.size_x, size_x + Ax);
        int y1 = min(compareGrid.size_y, size_y + Ay);

        for (int x = x0; x < x1; x += 1)
        {
            for (int y = y0; y < y1; y += 1)
            {
                float xx = compareGrid.idx2x(x);
                float yy = compareGrid.idx2y(y);

                float c1 = getPos(xx, yy);
                float c2 = compareGrid.getCell(x, y);
                if (c2 < 0.45f || c2 > 0.55f)
                {
                    nCellsCompared++;
                    if ((c1 > 0.5 && c2 < 0.5) || (c1 < 0.5 && c2 > 0.5))
                        cellsDifference++;
                }
            }
        }
        ret = 1 - cellsDifference / (nCellsCompared);
    }
    return log(ret);
}

/*---------------------------------------------------------------
            computeObservationLikelihood_MI
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_MI(
    const CObservation& obs, const CPose2D& takenFrom)
{
    MRPT_START

    CPose3D poseRobot(takenFrom);
    double res;

    // Dont modify the grid, only count the changes in Information
    updateInfoChangeOnly.enabled = true;
    insertionOptions.maxDistanceInsertion *=
        likelihoodOptions.MI_ratio_max_distance;

    // Reset the new information counters:
    updateInfoChangeOnly.cellsUpdated = 0;
    updateInfoChangeOnly.I_change = 0;
    updateInfoChangeOnly.laserRaysSkip = likelihoodOptions.MI_skip_rays;

    // Insert the observation (It will not be really inserted, only the
    // information counted)
    insertObservation(obs, &poseRobot);

    // Compute the change in I aported by the observation:
    double newObservation_mean_I;
    if (updateInfoChangeOnly.cellsUpdated)
        newObservation_mean_I =
            updateInfoChangeOnly.I_change / updateInfoChangeOnly.cellsUpdated;
    else
        newObservation_mean_I = 0;

    // Let the normal mode enabled, i.e. the grid can be updated
    updateInfoChangeOnly.enabled = false;
    insertionOptions.maxDistanceInsertion /=
        likelihoodOptions.MI_ratio_max_distance;

    res =
        pow(newObservation_mean_I,
            static_cast<double>(likelihoodOptions.MI_exponent));

    return log(res);

    MRPT_END
}

double COccupancyGridMap2D::computeObservationLikelihood_rayTracing(
    const CObservation& obs, const CPose2D& takenFrom)
{
    double ret = 0;

    // This function depends on the observation type:
    // -----------------------------------------------------
    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        // Observation is a laser range scan:
        // -------------------------------------------
        const auto& o = static_cast<const CObservation2DRangeScan&>(obs);
        CObservation2DRangeScan simulatedObs;

        // Insert only HORIZONTAL scans, since the grid is supposed to
        //  be a horizontal representation of space.
        if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
            return 0.5;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

        // The number of simulated rays will be original range scan rays /
        // DOWNRATIO
        int decimation = likelihoodOptions.rayTracing_decimation;
        int nRays = o.scan.size();

        // Perform simulation using same parameters than real observation:
        simulatedObs.aperture = o.aperture;
        simulatedObs.maxRange = o.maxRange;
        simulatedObs.rightToLeft = o.rightToLeft;
        simulatedObs.sensorPose = o.sensorPose;

        // Performs the scan simulation:
        laserScanSimulator(
            simulatedObs,  // The in/out observation
            takenFrom,  // robot pose
            0.45f,  // Cells threshold
            nRays,  // Scan length
            0, decimation);

        double stdLaser = likelihoodOptions.rayTracing_stdHit;
        double stdSqrt2 = sqrt(2.0f) * stdLaser;

        // Compute likelihoods:
        ret = 1;
        // bool     useDF = likelihoodOptions.rayTracing_useDistanceFilter;
        float r_sim, r_obs;
        double likelihood;

        for (int j = 0; j < nRays; j += decimation)
        {
            // Simulated and measured ranges:
            r_sim = simulatedObs.scan[j];
            r_obs = o.scan[j];

            // Is a valid range?
            if (o.validRange[j])
            {
                likelihood =
                    0.1 / o.maxRange +
                    0.9 *
                        exp(-square(
                            min((float)fabs(r_sim - r_obs), 2.0f) / stdSqrt2));
                ret += log(likelihood);
                // printf("Sim=%f\tReal=%f\tlik=%f\n",r_sim,r_obs,likelihood);
            }
        }
    }

    return ret;
}
/**/

/*---------------------------------------------------------------
            computeObservationLikelihood_likelihoodField_Thrun
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_Thrun(
    const CObservation& obs, const CPose2D& takenFrom)
{
    MRPT_START

    double ret = 0;

    // This function depends on the observation type:
    // -----------------------------------------------------
    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        // Observation is a laser range scan:
        // -------------------------------------------
        const auto& o = static_cast<const CObservation2DRangeScan&>(obs);

        // Insert only HORIZONTAL scans, since the grid is supposed to
        //  be a horizontal representation of space.
        if (!o.isPlanarScan(insertionOptions.horizontalTolerance)) return -10;

        // Assure we have a 2D points-map representation of the points from the
        // scan:
        CPointsMap::TInsertionOptions opts;
        opts.minDistBetweenLaserPoints = resolution * 0.5f;
        opts.isPlanarMap = true;  // Already filtered above!
        opts.horizontalTolerance = insertionOptions.horizontalTolerance;

        // Compute the likelihood of the points in this grid map:
        ret = computeLikelihoodField_Thrun(
            o.buildAuxPointsMap<mrpt::maps::CPointsMap>(&opts), &takenFrom);

    }  // end of observation is a scan range 2D
    else if (IS_CLASS(obs, CObservationRange))
    {
        // Sonar-like observations:
        // ---------------------------------------
        const auto& o = static_cast<const CObservationRange&>(obs);

        // Create a point map representation of the observation:
        CSimplePointsMap pts;
        pts.insertionOptions.minDistBetweenLaserPoints = resolution * 0.5f;
        pts.insertObservation(o);

        // Compute the likelihood of the points in this grid map:
        ret = computeLikelihoodField_Thrun(&pts, &takenFrom);
    }

    return ret;

    MRPT_END
}

/*---------------------------------------------------------------
        computeObservationLikelihood_likelihoodField_II
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_II(
    const CObservation& obs, const CPose2D& takenFrom)
{
    MRPT_START

    double ret = 0;

    // This function depends on the observation type:
    // -----------------------------------------------------
    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        // Observation is a laser range scan:
        // -------------------------------------------
        const auto& o = static_cast<const CObservation2DRangeScan&>(obs);

        // Insert only HORIZONTAL scans, since the grid is supposed to
        //  be a horizontal representation of space.
        if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
            return 0.5f;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

        // Assure we have a 2D points-map representation of the points from the
        // scan:

        // Compute the likelihood of the points in this grid map:
        ret = computeLikelihoodField_II(
            o.buildAuxPointsMap<mrpt::maps::CPointsMap>(), &takenFrom);

    }  // end of observation is a scan range 2D

    return ret;

    MRPT_END
}

/*---------------------------------------------------------------
                    computeLikelihoodField_Thrun
 ---------------------------------------------------------------*/
double COccupancyGridMap2D::computeLikelihoodField_Thrun(
    const CPointsMap* pm, const CPose2D* relativePose)
{
    MRPT_START

    double ret;
    size_t N = pm->size();
    int K = (int)ceil(
        likelihoodOptions.LF_maxCorrsDistance /*m*/ /
        resolution);  // The size of the checking area for matchings:

    bool Product_T_OrSum_F = !likelihoodOptions.LF_alternateAverageMethod;

    if (!N)
    {
        return -100;  // No way to estimate this likelihood!!
    }

    // Compute the likelihoods for each point:
    ret = 0;

    float stdHit = likelihoodOptions.LF_stdHit;
    float zHit = likelihoodOptions.LF_zHit;
    float zRandom = likelihoodOptions.LF_zRandom;
    float zRandomMaxRange = likelihoodOptions.LF_maxRange;
    float zRandomTerm = zRandom / zRandomMaxRange;
    float Q = -0.5f / square(stdHit);
    int M = 0;

    unsigned int size_x_1 = size_x - 1;
    unsigned int size_y_1 = size_y - 1;

#define LIK_LF_CACHE_INVALID (66)

    // Aux. variables for the "for j" loop:
    double thisLik = LIK_LF_CACHE_INVALID;
    double maxCorrDist_sq = square(likelihoodOptions.LF_maxCorrsDistance);
    double minimumLik = zRandomTerm + zHit * exp(Q * maxCorrDist_sq);
    double ccos, ssin;
    float occupiedMinDist;

    if (likelihoodOptions.enableLikelihoodCache)
    {
        // Reset the precomputed likelihood values map
        if (m_likelihoodCacheOutDated)
        {
            if (!map.empty())
                precomputedLikelihood.assign(map.size(), LIK_LF_CACHE_INVALID);
            else
                precomputedLikelihood.clear();

            m_likelihoodCacheOutDated = false;
        }
    }

    cellType thresholdCellValue = p2l(0.5f);
    int decimation = likelihoodOptions.LF_decimation;

    const double _resolution = this->resolution;
    const double constDist2DiscrUnits = 100 / (_resolution * _resolution);
    const double constDist2DiscrUnits_INV = 1.0 / constDist2DiscrUnits;

    if (N < 10) decimation = 1;

    TPoint2D pointLocal;
    TPoint2D pointGlobal;

    for (size_t j = 0; j < N; j += decimation)
    {
        occupiedMinDist = maxCorrDist_sq;  // The max.distance

        // Get the point and pass it to global coordinates:
        if (relativePose)
        {
            pm->getPoint(j, pointLocal);
// pointGlobal = *relativePose + pointLocal;
#ifdef HAVE_SINCOS
            ::sincos(relativePose->phi(), &ssin, &ccos);
#else
            ccos = cos(relativePose->phi());
            ssin = sin(relativePose->phi());
#endif
            pointGlobal.x =
                relativePose->x() + pointLocal.x * ccos - pointLocal.y * ssin;
            pointGlobal.y =
                relativePose->y() + pointLocal.x * ssin + pointLocal.y * ccos;
        }
        else
        {
            pm->getPoint(j, pointGlobal);
        }

        // Point to cell indixes
        int cx = x2idx(pointGlobal.x);
        int cy = y2idx(pointGlobal.y);

        // Precomputed table:
        // Tip: Comparison cx<0 is implicit in (unsigned)(x)>size...
        if (static_cast<unsigned>(cx) >= size_x_1 ||
            static_cast<unsigned>(cy) >= size_y_1)
        {
            // We are outside of the map: Assign the likelihood for the max.
            // correspondence distance:
            thisLik = minimumLik;
        }
        else
        {
            // We are into the map limits:
            if (likelihoodOptions.enableLikelihoodCache)
            {
                thisLik = precomputedLikelihood[cx + cy * size_x];
            }

            if (!likelihoodOptions.enableLikelihoodCache ||
                thisLik == LIK_LF_CACHE_INVALID)
            {
                // Compute now:
                // -------------
                // Find the closest occupied cell in a certain range, given by
                // K:
                int xx1 = max(0, cx - K);
                int xx2 = min(size_x_1, (unsigned)(cx + K));
                int yy1 = max(0, cy - K);
                int yy2 = min(size_y_1, (unsigned)(cy + K));

                // Optimized code: this part will be invoked a *lot* of times:
                {
                    cellType* mapPtr =
                        &map[xx1 + yy1 * size_x];  // Initial pointer position
                    unsigned incrAfterRow = size_x - ((xx2 - xx1) + 1);

                    signed int Ax0 = 10 * (xx1 - cx);
                    signed int Ay = 10 * (yy1 - cy);

                    unsigned int occupiedMinDistInt =
                        mrpt::round(maxCorrDist_sq * constDist2DiscrUnits);

                    for (int yy = yy1; yy <= yy2; yy++)
                    {
                        unsigned int Ay2 =
                            square((unsigned int)(Ay));  // Square is faster
                        // with unsigned.
                        signed short Ax = Ax0;
                        cellType cell;

                        for (int xx = xx1; xx <= xx2; xx++)
                        {
                            if ((cell = *mapPtr++) < thresholdCellValue)
                            {
                                unsigned int d =
                                    square((unsigned int)(Ax)) + Ay2;
                                keep_min(occupiedMinDistInt, d);
                            }
                            Ax += 10;
                        }
                        // Go to (xx1,yy++)
                        mapPtr += incrAfterRow;
                        Ay += 10;
                    }

                    occupiedMinDist =
                        occupiedMinDistInt * constDist2DiscrUnits_INV;
                }

                if (likelihoodOptions.LF_useSquareDist)
                    occupiedMinDist *= occupiedMinDist;

                thisLik = zRandomTerm + zHit * exp(Q * occupiedMinDist);

                if (likelihoodOptions.enableLikelihoodCache)
                    // And save it into the table and into "thisLik":
                    precomputedLikelihood[cx + cy * size_x] = thisLik;
            }
        }

        // Update the likelihood:
        if (Product_T_OrSum_F)
        {
            ret += log(thisLik);
        }
        else
        {
            ret += thisLik;
            M++;
        }
    }  // end of for each point in the scan

    if (!Product_T_OrSum_F) ret = log(ret / M);

    return ret;

    MRPT_END
}

/*---------------------------------------------------------------
                    computeLikelihoodField_II
 ---------------------------------------------------------------*/
double COccupancyGridMap2D::computeLikelihoodField_II(
    const CPointsMap* pm, const CPose2D* relativePose)
{
    MRPT_START

    double ret;
    size_t N = pm->size();

    if (!N) return 1e-100;  // No way to estimate this likelihood!!

    // Compute the likelihoods for each point:
    ret = 0;
    //  if (likelihoodOptions.LF_alternateAverageMethod)
    //          ret = 0;
    //  else    ret = 1;

    TPoint2D pointLocal, pointGlobal;

    float zRandomTerm = 1.0f / likelihoodOptions.LF_maxRange;
    float Q = -0.5f / square(likelihoodOptions.LF_stdHit);

    // Aux. cell indixes variables:
    int cx, cy;
    size_t j;
    int cx0, cy0;
    int cx_min, cx_max;
    int cy_min, cy_max;
    int maxRangeInCells =
        (int)ceil(likelihoodOptions.LF_maxCorrsDistance / resolution);
    int nCells = 0;

    // -----------------------------------------------------
    // Compute around a window of neigbors around each point
    // -----------------------------------------------------
    for (j = 0; j < N; j += likelihoodOptions.LF_decimation)
    {
        // Get the point and pass it to global coordinates:
        // ---------------------------------------------
        if (relativePose)
        {
            pm->getPoint(j, pointLocal);
            pointGlobal = *relativePose + pointLocal;
        }
        else
        {
            pm->getPoint(j, pointGlobal);
        }

        // Point to cell indixes:
        // ---------------------------------------------
        cx0 = x2idx(pointGlobal.x);
        cy0 = y2idx(pointGlobal.y);

        // Compute the range of cells to compute:
        // ---------------------------------------------
        cx_min = max(cx0 - maxRangeInCells, 0);
        cx_max = min(cx0 + maxRangeInCells, static_cast<int>(size_x));
        cy_min = max(cy0 - maxRangeInCells, 0);
        cy_max = min(cy0 + maxRangeInCells, static_cast<int>(size_y));

        //      debugImg.rectangle(cx_min,cy_min,cx_max,cy_max,0xFF0000 );

        // Compute over the window of cells:
        // ---------------------------------------------
        double lik = 0;
        for (cx = cx_min; cx <= cx_max; cx++)
        {
            for (cy = cy_min; cy <= cy_max; cy++)
            {
                float P_free = getCell(cx, cy);
                float termDist =
                    exp(Q * (square(idx2x(cx) - pointGlobal.x) +
                             square(idx2y(cy) - pointGlobal.y)));

                lik += P_free * zRandomTerm + (1 - P_free) * termDist;
            }  // end for cy
        }  // end for cx

        // Update the likelihood:
        if (likelihoodOptions.LF_alternateAverageMethod)
            ret += lik;
        else
            ret += log(lik / ((cy_max - cy_min + 1) * (cx_max - cx_min + 1)));
        nCells++;

    }  // end of for each point in the scan

    if (likelihoodOptions.LF_alternateAverageMethod && nCells > 0)
        ret = log(ret / nCells);
    else
        ret = ret / nCells;

    return ret;

    MRPT_END
}

/*---------------------------------------------------------------
    Initilization of values, don't needed to be called directly.
  ---------------------------------------------------------------*/
COccupancyGridMap2D::TLikelihoodOptions::TLikelihoodOptions()
    : OWA_weights(100, 1 / 100.0f)

{
}

/*---------------------------------------------------------------
                    loadFromConfigFile
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::TLikelihoodOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
    likelihoodMethod =
        iniFile.read_enum(section, "likelihoodMethod", likelihoodMethod);

    enableLikelihoodCache = iniFile.read_bool(
        section, "enableLikelihoodCache", enableLikelihoodCache);

    LF_stdHit = iniFile.read_float(section, "LF_stdHit", LF_stdHit);
    LF_zHit = iniFile.read_float(section, "LF_zHit", LF_zHit);
    LF_zRandom = iniFile.read_float(section, "LF_zRandom", LF_zRandom);
    LF_maxRange = iniFile.read_float(section, "LF_maxRange", LF_maxRange);
    LF_decimation = iniFile.read_int(section, "LF_decimation", LF_decimation);
    LF_maxCorrsDistance =
        iniFile.read_float(section, "LF_maxCorrsDistance", LF_maxCorrsDistance);
    LF_useSquareDist =
        iniFile.read_bool(section, "LF_useSquareDist", LF_useSquareDist);
    LF_alternateAverageMethod = iniFile.read_bool(
        section, "LF_alternateAverageMethod", LF_alternateAverageMethod);

    MI_exponent = iniFile.read_float(section, "MI_exponent", MI_exponent);
    MI_skip_rays = iniFile.read_int(section, "MI_skip_rays", MI_skip_rays);
    MI_ratio_max_distance = iniFile.read_float(
        section, "MI_ratio_max_distance", MI_ratio_max_distance);

    rayTracing_useDistanceFilter = iniFile.read_bool(
        section, "rayTracing_useDistanceFilter", rayTracing_useDistanceFilter);
    rayTracing_stdHit =
        iniFile.read_float(section, "rayTracing_stdHit", rayTracing_stdHit);

    consensus_takeEachRange = iniFile.read_int(
        section, "consensus_takeEachRange", consensus_takeEachRange);
    consensus_pow = iniFile.read_float(section, "consensus_pow", consensus_pow);

    iniFile.read_vector(section, "OWA_weights", OWA_weights, OWA_weights);
}

/*---------------------------------------------------------------
                    dumpToTextStream
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::TLikelihoodOptions::dumpToTextStream(
    std::ostream& out) const
{
    out << mrpt::format(
        "\n----------- [COccupancyGridMap2D::TLikelihoodOptions] ------------ "
        "\n\n");

    out << mrpt::format("likelihoodMethod                        = ");
    switch (likelihoodMethod)
    {
        case lmMeanInformation:
            out << mrpt::format("lmMeanInformation");
            break;
        case lmRayTracing:
            out << mrpt::format("lmRayTracing");
            break;
        case lmConsensus:
            out << mrpt::format("lmConsensus");
            break;
        case lmCellsDifference:
            out << mrpt::format("lmCellsDifference");
            break;
        case lmLikelihoodField_Thrun:
            out << mrpt::format("lmLikelihoodField_Thrun");
            break;
        case lmLikelihoodField_II:
            out << mrpt::format("lmLikelihoodField_II");
            break;
        case lmConsensusOWA:
            out << mrpt::format("lmConsensusOWA");
            break;
        default:
            out << mrpt::format("UNKNOWN!!!");
            break;
    }
    out << mrpt::format("\n");

    out << mrpt::format(
        "enableLikelihoodCache                   = %c\n",
        enableLikelihoodCache ? 'Y' : 'N');

    out << mrpt::format(
        "LF_stdHit                               = %f\n", LF_stdHit);
    out << mrpt::format(
        "LF_zHit                                 = %f\n", LF_zHit);
    out << mrpt::format(
        "LF_zRandom                              = %f\n", LF_zRandom);
    out << mrpt::format(
        "LF_maxRange                             = %f\n", LF_maxRange);
    out << mrpt::format(
        "LF_decimation                           = %u\n", LF_decimation);
    out << mrpt::format(
        "LF_maxCorrsDistance                     = %f\n", LF_maxCorrsDistance);
    out << mrpt::format(
        "LF_useSquareDist                        = %c\n",
        LF_useSquareDist ? 'Y' : 'N');
    out << mrpt::format(
        "LF_alternateAverageMethod               = %c\n",
        LF_alternateAverageMethod ? 'Y' : 'N');
    out << mrpt::format(
        "MI_exponent                             = %f\n", MI_exponent);
    out << mrpt::format(
        "MI_skip_rays                            = %u\n", MI_skip_rays);
    out << mrpt::format(
        "MI_ratio_max_distance                   = %f\n",
        MI_ratio_max_distance);
    out << mrpt::format(
        "rayTracing_useDistanceFilter            = %c\n",
        rayTracing_useDistanceFilter ? 'Y' : 'N');
    out << mrpt::format(
        "rayTracing_decimation                   = %u\n",
        rayTracing_decimation);
    out << mrpt::format(
        "rayTracing_stdHit                       = %f\n", rayTracing_stdHit);
    out << mrpt::format(
        "consensus_takeEachRange                 = %u\n",
        consensus_takeEachRange);
    out << mrpt::format(
        "consensus_pow                           = %.02f\n", consensus_pow);
    out << mrpt::format("OWA_weights   = [");
    for (size_t i = 0; i < OWA_weights.size(); i++)
    {
        if (i < 3 || i > (OWA_weights.size() - 3))
            out << mrpt::format("%.03f ", OWA_weights[i]);
        else if (i == 3 && OWA_weights.size() > 6)
            out << mrpt::format(" ... ");
    }
    out << mrpt::format("] (size=%u)\n", (unsigned)OWA_weights.size());
    out << mrpt::format("\n");
}

/** Returns true if this map is able to compute a sensible likelihood function
 * for this observation (i.e. an occupancy grid map cannot with an image).
 * \param obs The observation.
 * \sa computeObservationLikelihood
 */
bool COccupancyGridMap2D::internal_canComputeObservationLikelihood(
    const mrpt::obs::CObservation& obs) const
{
    // Ignore laser scans if they are not planar or they are not
    //  at the altitude of this grid map:
    if (IS_CLASS(obs, CObservation2DRangeScan))
    {
        const auto& scan = static_cast<const CObservation2DRangeScan&>(obs);

        if (!scan.isPlanarScan(insertionOptions.horizontalTolerance))
            return false;
        if (insertionOptions.useMapAltitude &&
            fabs(insertionOptions.mapAltitude - scan.sensorPose.z()) > 0.01)
            return false;

        // OK, go on...
        return true;
    }
    else  // Is not a laser scanner...
    {
        return false;
    }
}