COccupancyGridMap2D_likelihood.cpp

Go to the documentation of this file.

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

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

#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/utils/CStream.h>


using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::utils;
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 ( obs->GetRuntimeClass() == CLASS_ID(CObservation2DRangeScan) )
        {
                const CObservation2DRangeScan   *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 ( obs->GetRuntimeClass() != CLASS_ID(CObservation2DRangeScan) )
        {
                //THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan' classes only.");
                return 1e-3;
        }
        // Observation is a laser range scan:
        // -------------------------------------------
        const CObservation2DRangeScan           *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 CPointsMap *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 ( obs->GetRuntimeClass() == CLASS_ID(CObservation2DRangeScan) )
        {
                //THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan' classes only.");
                return 1e-3;
        }
        // Observation is a laser range scan:
        // -------------------------------------------
        const CObservation2DRangeScan           *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 CPointsMap *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 ( obs->GetRuntimeClass() == CLASS_ID(CObservation2DRangeScan) )
         {
                 // Observation is a laser range scan:
                 // -------------------------------------------
                const CObservation2DRangeScan           *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 ( obs->GetRuntimeClass() == CLASS_ID(CObservation2DRangeScan) )
         {
                 // Observation is a laser range scan:
                 // -------------------------------------------
                const CObservation2DRangeScan           *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 CObservation2DRangeScan           *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 CObservationRange         *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 ( obs->GetRuntimeClass() == CLASS_ID(CObservation2DRangeScan) )
        {
                // Observation is a laser range scan:
                // -------------------------------------------
                const CObservation2DRangeScan           *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;

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

#define LIK_LF_CACHE_INVALID    (66)

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

                        precomputedLikelihoodToBeRecomputed = 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::utils::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() :
        likelihoodMethod                                ( lmLikelihoodField_Thrun),

        LF_stdHit                                               ( 0.35f ),
        LF_zHit                                                 ( 0.95f ),
        LF_zRandom                                              ( 0.05f ),
        LF_maxRange                                             ( 81.0f ),
        LF_decimation                                   ( 5 ),
        LF_maxCorrsDistance                             ( 0.3f ),
        LF_useSquareDist                                ( false ),
        LF_alternateAverageMethod               ( false ),

        MI_exponent                                             ( 2.5f ),
        MI_skip_rays                                    ( 10 ),
        MI_ratio_max_distance                   ( 1.5f ),

        rayTracing_useDistanceFilter    ( true ),
        rayTracing_decimation                   ( 10 ),
        rayTracing_stdHit                               ( 1.0f ),

        consensus_takeEachRange                 ( 1 ),
        consensus_pow                                   ( 5 ),
        OWA_weights                                             (100,1/100.0f),

        enableLikelihoodCache           ( true )
{
}

/*---------------------------------------------------------------
                                        loadFromConfigFile
  ---------------------------------------------------------------*/
void  COccupancyGridMap2D::TLikelihoodOptions::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase  &iniFile,
        const std::string &section)
{
        MRPT_LOAD_CONFIG_VAR_CAST(likelihoodMethod, int, TLikelihoodMethod, iniFile, section);

    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(mrpt::utils::CStream    &out) const
{
        out.printf("\n----------- [COccupancyGridMap2D::TLikelihoodOptions] ------------ \n\n");

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

        out.printf("enableLikelihoodCache                   = %c\n",    enableLikelihoodCache ? 'Y':'N');

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