/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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;

double COccupancyGridMap2D::internal_computeObservationLikelihood(
	const CObservation& obs, const CPose3D& takenFrom3D) 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 = dynamic_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) const
{
	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 = dynamic_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) const
{
	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 = dynamic_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>(m_size_x) - 1, cx0 + Acells);
		int cyMin = max(0, cy0 - Acells);
		int cyMax = min(static_cast<int>(m_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) const
{
	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 = dynamic_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, m_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((m_xMin - compareGrid.m_xMin) / m_resolution);
		Ay = round((m_yMin - compareGrid.m_yMin) / m_resolution);

		int nCellsCompared = 0;
		float cellsDifference = 0;
		int x0 = max(0, Ax);
		int y0 = max(0, Ay);
		int x1 = min(compareGrid.m_size_x, m_size_x + Ax);
		int y1 = min(compareGrid.m_size_y, m_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) const
{
	MRPT_START

	CPose3D poseRobot(takenFrom);
	double res;

	// Dont modify the grid, only count the changes in Information
	updateInfoChangeOnly.enabled = true;
	const_cast<COccupancyGridMap2D*>(this)
		->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)
	const_cast<COccupancyGridMap2D*>(this)->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;
	const_cast<COccupancyGridMap2D*>(this)
		->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) const
{
	double ret = 0;

	// This function depends on the observation type:
	// -----------------------------------------------------
	if (IS_CLASS(obs, CObservation2DRangeScan))
	{
		// Observation is a laser range scan:
		// -------------------------------------------
		const auto& o = dynamic_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.getScanSize();

		// 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.getScanRange(j);
			r_obs = o.getScanRange(j);

			// Is a valid range?
			if (o.getScanRangeValidity(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) const
{
	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 = dynamic_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 = m_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 = dynamic_cast<const CObservationRange&>(obs);

		// Create a point map representation of the observation:
		CSimplePointsMap pts;
		pts.insertionOptions.minDistBetweenLaserPoints = m_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) const
{
	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 = dynamic_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) const
{
	MRPT_START

	double ret;
	size_t N = pm->size();
	int K = (int)ceil(
		likelihoodOptions.LF_maxCorrsDistance /*m*/ /
		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 = m_size_x - 1;
	unsigned int size_y_1 = m_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;

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

			m_likelihoodCacheOutDated = false;
		}
	}

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

	const double _resolution = this->m_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)
	{
		// 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 = m_precomputedLikelihood[cx + cy * m_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:
				float occupiedMinDist;
				{
					// Initial pointer position
					const cellType* mapPtr = &m_map[xx1 + yy1 * m_size_x];
					unsigned incrAfterRow = m_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":
					m_precomputedLikelihood[cx + cy * m_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) const
{
	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 / m_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>(m_size_x));
		cy_min = max(cy0 - maxRangeInCells, 0);
		cy_max = min(cy0 + maxRangeInCells, static_cast<int>(m_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 << "\n----------- [COccupancyGridMap2D::TLikelihoodOptions] "
		   "------------ "
		   "\n\n";

	out << "likelihoodMethod                        = ";
	switch (likelihoodMethod)
	{
		case lmMeanInformation: out << "lmMeanInformation"; break;
		case lmRayTracing: out << "lmRayTracing"; break;
		case lmConsensus: out << "lmConsensus"; break;
		case lmCellsDifference: out << "lmCellsDifference"; break;
		case lmLikelihoodField_Thrun: out << "lmLikelihoodField_Thrun"; break;
		case lmLikelihoodField_II: out << "lmLikelihoodField_II"; break;
		case lmConsensusOWA: out << "lmConsensusOWA"; break;
		default: out << "UNKNOWN!!!"; break;
	}
	out << "\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 << "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 << " ... ";
	}
	out << mrpt::format("] (size=%u)\n", (unsigned)OWA_weights.size());
	out << "\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 = dynamic_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;
	}
}
