/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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/core/round.h>
#include <mrpt/img/color_maps.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CRandomFieldGridMap2D.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/utils.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CSetOfTriangles.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/os.h>

#include <numeric>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace mrpt::io;
using namespace mrpt::img;
using namespace std;

IMPLEMENTS_VIRTUAL_SERIALIZABLE(CRandomFieldGridMap2D, CMetricMap, mrpt::maps)

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CRandomFieldGridMap2D::CRandomFieldGridMap2D(
	TMapRepresentation mapType, double x_min, double x_max, double y_min,
	double y_max, double resolution)
	: CDynamicGrid<TRandomFieldCell>(x_min, x_max, y_min, y_max, resolution),
	  COutputLogger("CRandomFieldGridMap2D"),

	  m_mapType(mapType),
	  m_cov(0, 0)

{
	// We can't set "m_insertOptions_common" here via "getCommonInsertOptions()"
	// since
	//  it's a pure virtual method and we're at the constructor.
	// We need all derived classes to call ::clear() in their constructors so we
	// reach internal_clear()
	//  and set there that variable...
}

CRandomFieldGridMap2D::~CRandomFieldGridMap2D() = default;
/** Changes the size of the grid, erasing previous contents. \sa resize */
void CRandomFieldGridMap2D::setSize(
	const double x_min, const double x_max, const double y_min,
	const double y_max, const double resolution,
	const TRandomFieldCell* fill_value)
{
	CDynamicGrid<TRandomFieldCell>::setSize(
		x_min, x_max, y_min, y_max, resolution, fill_value);
	CMetricMap::clear();
}

void CRandomFieldGridMap2D::setCellsConnectivity(
	const ConnectivityDescriptor::Ptr& new_connectivity_descriptor)
{
	m_gmrf_connectivity = new_connectivity_descriptor;
}

/*---------------------------------------------------------------
						clear
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::internal_clear()
{
	// (Read the note in the constructor above)
	m_insertOptions_common =
		getCommonInsertOptions();  // Get the pointer from child class

	m_average_normreadings_mean = 0;
	m_average_normreadings_var = 0;
	m_average_normreadings_count = 0;

	switch (m_mapType)
	{
		case mrKernelDM:
		case mrKernelDMV:
		{
			// Set the grid to initial values:
			TRandomFieldCell def(0, 0);
			fill(def);
		}
		break;

		case mrKalmanFilter:
		{
			MRPT_LOG_DEBUG_FMT(
				"[clear] Setting covariance matrix to %ux%u\n",
				(unsigned int)(m_size_y * m_size_x),
				(unsigned int)(m_size_y * m_size_x));

			TRandomFieldCell def(
				m_insertOptions_common->KF_defaultCellMeanValue,  // mean
				m_insertOptions_common->KF_initialCellStd  // std
			);

			fill(def);

			// Reset the covariance matrix:
			m_cov.setSize(m_size_y * m_size_x, m_size_y * m_size_x);

			// And load its default values:
			const double KF_covSigma2 =
				square(m_insertOptions_common->KF_covSigma);
			const double res2 = square(m_resolution);
			const double std0sqr =
				square(m_insertOptions_common->KF_initialCellStd);

			for (int i = 0; i < m_cov.rows(); i++)
			{
				int cx1 = (i % m_size_x);
				int cy1 = (i / m_size_x);

				for (int j = i; j < m_cov.cols(); j++)
				{
					int cx2 = (j % m_size_x);
					int cy2 = (j / m_size_x);

					if (i == j) { m_cov(i, j) = std0sqr; }
					else
					{
						m_cov(i, j) = std0sqr *
							exp(-0.5 *
								(res2 *
								 static_cast<double>(
									 square(cx1 - cx2) + square(cy1 - cy2))) /
								KF_covSigma2);
						m_cov(j, i) = m_cov(i, j);
					}
				}  // for j
			}  // for i

			// m_cov.saveToTextFile("cov_init.txt",1);
		}
		break;
		// and continue with:
		case mrKalmanApproximate:
		{
			m_hasToRecoverMeanAndCov = true;

			CTicTac tictac;
			tictac.Tic();

			MRPT_LOG_DEBUG(
				"[CRandomFieldGridMap2D::clear] Resetting compressed cov. "
				"matrix and cells\n");
			TRandomFieldCell def(
				m_insertOptions_common->KF_defaultCellMeanValue,  // mean
				m_insertOptions_common->KF_initialCellStd  // std
			);

			fill(def);

			// Reset the covariance matrix:
			// --------------------------------------
			const signed W = m_insertOptions_common->KF_W_size;
			const size_t N = m_map.size();
			const size_t K = 2 * W * (W + 1) + 1;

			const double KF_covSigma2 =
				square(m_insertOptions_common->KF_covSigma);
			const double std0sqr =
				square(m_insertOptions_common->KF_initialCellStd);
			const double res2 = square(m_resolution);

			m_stackedCov.setSize(N, K);

			// Populate it with the initial cov. values:
			// ------------------------------------------
			signed Acx, Acy;
			const double* ptr_first_row = &m_stackedCov(0, 0);

			for (size_t i = 0; i < N; i++)
			{
				double* ptr = &m_stackedCov(i, 0);

				if (i == 0)
				{
					// 1) current cell
					*ptr++ = std0sqr;

					// 2) W rest of the first row:
					Acy = 0;
					for (Acx = 1; Acx <= W; Acx++)
						*ptr++ = std0sqr *
							exp(-0.5 *
								(res2 *
								 static_cast<double>(
									 square(Acx) + square(Acy))) /
								KF_covSigma2);

					// 3) The others W rows:
					for (Acy = 1; Acy <= W; Acy++)
						for (Acx = -W; Acx <= W; Acx++)
							*ptr++ = std0sqr *
								exp(-0.5 *
									(res2 *
									 static_cast<double>(
										 square(Acx) + square(Acy))) /
									KF_covSigma2);
				}
				else
				{
					// Just copy the same:
					memcpy(ptr, ptr_first_row, sizeof(double) * K);
				}
			}

			MRPT_LOG_DEBUG_FMT(
				"[clear] %ux%u cells done in %.03fms\n", unsigned(m_size_x),
				unsigned(m_size_y), 1000 * tictac.Tac());
		}
		break;

		case mrGMRF_SD:
		{
			CTicTac tictac;
			tictac.Tic();

			MRPT_LOG_DEBUG(
				"[clear] Generating Prior based on 'Squared Differences'\n");
			MRPT_LOG_DEBUG_FMT(
				"[clear] Initial map dimension: %u cells, x=(%.2f,%.2f) "
				"y=(%.2f,%.2f)\n",
				static_cast<unsigned int>(m_map.size()), getXMin(), getXMax(),
				getYMin(), getYMax());

			// Set the gridmap (m_map) to initial values:
			TRandomFieldCell def(0, 0);	 // mean, std
			fill(def);

			mrpt::maps::COccupancyGridMap2D m_Ocgridmap;
			float res_coef = 1.f;  // Default value

			if (this->m_insertOptions_common->GMRF_use_occupancy_information)
			{  // Load Occupancy Gridmap and resize
				if (!m_insertOptions_common->GMRF_simplemap_file.empty())
				{
					mrpt::maps::CSimpleMap simpleMap;
					CFileGZInputStream fi(
						this->m_insertOptions_common->GMRF_simplemap_file);
					mrpt::serialization::archiveFrom(fi) >> simpleMap;
					ASSERT_(!simpleMap.empty());
					m_Ocgridmap.loadFromSimpleMap(simpleMap);
					res_coef =
						this->getResolution() / m_Ocgridmap.getResolution();
				}
				else if (!m_insertOptions_common->GMRF_gridmap_image_file
							  .empty())
				{
					// Load from image
					const bool grid_loaded_ok = m_Ocgridmap.loadFromBitmapFile(
						this->m_insertOptions_common->GMRF_gridmap_image_file,
						this->m_insertOptions_common->GMRF_gridmap_image_res,
						TPoint2D(
							this->m_insertOptions_common->GMRF_gridmap_image_cx,
							this->m_insertOptions_common
								->GMRF_gridmap_image_cy));
					ASSERT_(grid_loaded_ok);
					res_coef = this->getResolution() /
						this->m_insertOptions_common->GMRF_gridmap_image_res;
				}
				else
				{
					THROW_EXCEPTION(
						"Neither `simplemap_file` nor `gridmap_image_file` "
						"found in config/mission file. Quitting.");
				}

				// Resize MRF Map to match Occupancy Gridmap dimmensions
				MRPT_LOG_DEBUG(
					"Resizing m_map to match Occupancy Gridmap dimensions");

				resize(
					m_Ocgridmap.getXMin(), m_Ocgridmap.getXMax(),
					m_Ocgridmap.getYMin(), m_Ocgridmap.getYMax(), def, 0.0);

				MRPT_LOG_DEBUG_FMT(
					"Occupancy Gridmap dimensions: x=(%.2f,%.2f)m "
					"y=(%.2f,%.2f)m \n",
					m_Ocgridmap.getXMin(), m_Ocgridmap.getXMax(),
					m_Ocgridmap.getYMin(), m_Ocgridmap.getYMax());
				MRPT_LOG_DEBUG_FMT(
					"Occupancy Gridmap dimensions: %u cells, cx=%i cy=%i\n\n",
					static_cast<unsigned int>(
						m_Ocgridmap.getSizeX() * m_Ocgridmap.getSizeY()),
					m_Ocgridmap.getSizeX(), m_Ocgridmap.getSizeY());
				MRPT_LOG_DEBUG_FMT(
					"New map dimensions: %u cells, x=(%.2f,%.2f) "
					"y=(%.2f,%.2f)\n",
					static_cast<unsigned int>(m_map.size()), getXMin(),
					getXMax(), getYMin(), getYMax());
				MRPT_LOG_DEBUG_FMT(
					"New map dimensions: %u cells, cx=%u cy=%u\n",
					static_cast<unsigned int>(m_map.size()),
					static_cast<unsigned int>(getSizeX()),
					static_cast<unsigned int>(getSizeY()));

				m_Ocgridmap.saveAsBitmapFile(
					"./obstacle_map_from_MRPT_for_GMRF.png");
			}

			// m_map number of cells
			const size_t nodeCount = m_map.size();

			// Set initial factors: L "prior factors" + 0 "Observation factors"
			const size_t nPriorFactors =
				(this->getSizeX() - 1) * this->getSizeY() +
				this->getSizeX() *
					(this->getSizeY() -
					 1);  // L = (Nr-1)*Nc + Nr*(Nc-1); Full connected

			MRPT_LOG_DEBUG_STREAM(
				"[clear] Generating "
				<< nPriorFactors
				<< " prior factors for a map size of N=" << nodeCount << endl);

			m_gmrf.clear();
			m_gmrf.initialize(nodeCount);

			m_mrf_factors_activeObs.clear();
			m_mrf_factors_activeObs.resize(
				nodeCount);	 // All cells, no observation

			m_mrf_factors_priors.clear();

			//-------------------------------------
			// Load default values for H_prior:
			//-------------------------------------
			if (!m_gmrf_connectivity &&
				this->m_insertOptions_common->GMRF_use_occupancy_information)
			{
				MRPT_LOG_DEBUG("LOADING PRIOR BASED ON OCCUPANCY GRIDMAP \n");
				MRPT_LOG_DEBUG_FMT(
					"MRF Map Dimmensions: %u x %u cells \n",
					static_cast<unsigned int>(m_size_x),
					static_cast<unsigned int>(m_size_y));
				MRPT_LOG_DEBUG_FMT(
					"Occupancy map Dimmensions: %i x %i cells \n",
					m_Ocgridmap.getSizeX(), m_Ocgridmap.getSizeY());
				MRPT_LOG_DEBUG_FMT("Res_Coeff = %f pixels/celda", res_coef);

				// Use region growing algorithm to determine the cell
				// interconnections (Factors)
				size_t cx = 0;
				size_t cy = 0;

				size_t cxoj_min, cxoj_max, cyoj_min, cyoj_max, seed_cxo,
					seed_cyo;  // Cell j limits in the Occupancy
				size_t cxoi_min, cxoi_max, cyoi_min, cyoi_max, objective_cxo,
					objective_cyo;	// Cell i limits in the Occupancy
				size_t cxo_min, cxo_max, cyo_min,
					cyo_max;  // Cell i+j limits in the Occupancy
				// bool first_obs = false;

				std::multimap<size_t, size_t>
					cell_interconnections;	// Store the interconnections
				// (relations) of each cell with its
				// neighbourds

				for (size_t j = 0; j < nodeCount;
					 j++)  // For each cell in the map
				{
					// Get cell_j indx-limits in Occuppancy gridmap
					cxoj_min = floor(cx * res_coef);
					cxoj_max = cxoj_min + ceil(res_coef - 1);
					cyoj_min = floor(cy * res_coef);
					cyoj_max = cyoj_min + ceil(res_coef - 1);

					seed_cxo = cxoj_min + ceil(res_coef / 2 - 1);
					seed_cyo = cyoj_min + ceil(res_coef / 2 - 1);

					// If cell occpuped then add fake observation: to allow all
					// cells having a solution
					if (m_Ocgridmap.getCell(seed_cxo, seed_cyo) < 0.5)
					{
						TObservationGMRF new_obs(*this);
						new_obs.node_id = j;
						new_obs.obsValue = 0.0;
						new_obs.Lambda = 10e-5;
						new_obs.time_invariant =
							true;  // Obs that will not dissapear with time.
						m_mrf_factors_activeObs[j].push_back(new_obs);
						m_gmrf.addConstraint(*m_mrf_factors_activeObs[j]
												  .rbegin());  // add to graph
					}

					// Factor with the right node: H_ji = - Lamda_prior
					// Factor with the upper node: H_ji = - Lamda_prior
					//-------------------------------------------------
					for (int neighbor = 0; neighbor < 2; neighbor++)
					{
						size_t i, cxi, cyi;

						if (neighbor == 0)
						{
							if (cx >= (m_size_x - 1)) continue;
							i = j + 1;
							cxi = cx + 1;
							cyi = cy;
						}
						else if (neighbor == 1)
						{
							if (cy >= (m_size_y - 1)) continue;
							i = j + m_size_x;
							cxi = cx;
							cyi = cy + 1;
						}
						else
							throw std::runtime_error("Shouldn't reach here!");

						// Get cell_i indx-limits in Occuppancy gridmap
						cxoi_min = floor(cxi * res_coef);
						cxoi_max = cxoi_min + ceil(res_coef - 1);
						cyoi_min = floor(cyi * res_coef);
						cyoi_max = cyoi_min + ceil(res_coef - 1);

						objective_cxo = cxoi_min + ceil(res_coef / 2 - 1);
						objective_cyo = cyoi_min + ceil(res_coef / 2 - 1);

						// Get overall indx of both cells together
						cxo_min = min(cxoj_min, cxoi_min);
						cxo_max = max(cxoj_max, cxoi_max);
						cyo_min = min(cyoj_min, cyoi_min);
						cyo_max = max(cyoj_max, cyoi_max);

						// Check using Region growing if cell j is connected to
						// cell i (Occupancy gridmap)
						if (exist_relation_between2cells(
								&m_Ocgridmap, cxo_min, cxo_max, cyo_min,
								cyo_max, seed_cxo, seed_cyo, objective_cxo,
								objective_cyo))
						{
							TPriorFactorGMRF new_prior(*this);
							new_prior.node_id_i = i;
							new_prior.node_id_j = j;
							new_prior.Lambda =
								m_insertOptions_common->GMRF_lambdaPrior;

							m_mrf_factors_priors.push_back(new_prior);
							m_gmrf.addConstraint(
								*m_mrf_factors_priors
									 .rbegin());  // add to graph

							// Save relation between cells
							cell_interconnections.insert(
								std::pair<size_t, size_t>(j, i));
							cell_interconnections.insert(
								std::pair<size_t, size_t>(i, j));
						}

					}  // end for 2 neighbors

					// Increment j coordinates (row(x), col(y))
					if (++cx >= m_size_x)
					{
						cx = 0;
						cy++;
					}
				}  // end for "j"
			}
			else
			{
				ConnectivityDescriptor* custom_connectivity =
					m_gmrf_connectivity.get();	// Use a raw ptr to avoid the
				// cost in the inner loops
				if (custom_connectivity != nullptr)
					MRPT_LOG_DEBUG(
						"[CRandomFieldGridMap2D::clear] Initiating prior "
						"(using user-supplied connectivity pattern)");
				else
					MRPT_LOG_DEBUG(
						"[CRandomFieldGridMap2D::clear] Initiating prior "
						"(fully connected)");

				//---------------------------------------------------------------
				// Load default values for H_prior without Occupancy
				// information:
				//---------------------------------------------------------------
				size_t cx = 0, cy = 0;
				for (size_t j = 0; j < nodeCount; j++)
				{
					// add factors between this node and:
					// 1) the right node: j +1
					// 2) the bottom node:  j+m_size_x
					//-------------------------------------------------
					const size_t c_idx_to_check[2] = {cx, cy};
					const size_t c_idx_to_check_limits[2] = {
						m_size_x - 1, m_size_y - 1};
					const size_t c_neighbor_idx_incr[2] = {1, m_size_x};

					for (int dir = 0; dir < 2; dir++)
					{
						if (c_idx_to_check[dir] >= c_idx_to_check_limits[dir])
							continue;

						const size_t i = j + c_neighbor_idx_incr[dir];
						ASSERT_(i < nodeCount);

						double edge_lamdba = .0;
						if (custom_connectivity != nullptr)
						{
							const bool is_connected =
								custom_connectivity->getEdgeInformation(
									this, cx, cy, cx + (dir == 0 ? 1 : 0),
									cy + (dir == 1 ? 1 : 0), edge_lamdba);
							if (!is_connected) continue;
						}
						else
						{
							edge_lamdba =
								m_insertOptions_common->GMRF_lambdaPrior;
						}
						TPriorFactorGMRF new_prior(*this);
						new_prior.node_id_i = i;
						new_prior.node_id_j = j;
						new_prior.Lambda = edge_lamdba;

						m_mrf_factors_priors.push_back(new_prior);
						m_gmrf.addConstraint(
							*m_mrf_factors_priors.rbegin());  // add to graph
					}

					// Increment j coordinates (row(x), col(y))
					if (++cx >= m_size_x)
					{
						cx = 0;
						cy++;
					}
				}  // end for "j"
			}  // end if_use_Occupancy

			MRPT_LOG_DEBUG_STREAM(
				"[clear] Prior built in " << tictac.Tac() << " s ----------");

			if (m_rfgm_run_update_upon_clear)
			{
				// Solve system and update map estimation
				updateMapEstimation_GMRF();
			}

		}  // end case
		break;
		default: cerr << "MAP TYPE NOT RECOGNIZED... QUITTING" << endl; break;
	}  // end switch
}

/*---------------------------------------------------------------
						isEmpty
  ---------------------------------------------------------------*/
bool CRandomFieldGridMap2D::isEmpty() const { return false; }
/*---------------------------------------------------------------
					insertObservation_KernelDM_DMV
  ---------------------------------------------------------------*/
/** The implementation of "insertObservation" for Achim Lilienthal's map models
 * DM & DM+V.
 * \param normReading Is a [0,1] normalized concentration reading.
 * \param is_DMV = false -> map type is Kernel DM; true -> map type is DM+V
 */
void CRandomFieldGridMap2D::insertObservation_KernelDM_DMV(
	double normReading, const mrpt::math::TPoint2D& point, bool is_DMV)
{
	MRPT_START

	static const TRandomFieldCell defCell(0, 0);

	// Assure we have room enough in the grid!
	resize(
		point.x - m_insertOptions_common->cutoffRadius * 2,
		point.x + m_insertOptions_common->cutoffRadius * 2,
		point.y - m_insertOptions_common->cutoffRadius * 2,
		point.y + m_insertOptions_common->cutoffRadius * 2, defCell);

	// Compute the "parzen Gaussian" once only:
	// -------------------------------------------------
	ASSERT_LT_(m_resolution, 0.5 * m_insertOptions_common->cutoffRadius);
	int Ac_cutoff = round(m_insertOptions_common->cutoffRadius / m_resolution);
	unsigned Ac_all = 1 + 2 * Ac_cutoff;
	double minWinValueAtCutOff = exp(-square(
		m_insertOptions_common->cutoffRadius / m_insertOptions_common->sigma));

	if (m_DM_lastCutOff != m_insertOptions_common->cutoffRadius ||
		m_DM_gaussWindow.size() != square(Ac_all))
	{
		MRPT_LOG_DEBUG_FMT(
			"[CRandomFieldGridMap2D::insertObservation_KernelDM_DMV] "
			"Precomputing window %ux%u\n",
			Ac_all, Ac_all);

		double dist;
		double std = m_insertOptions_common->sigma;

		// Compute the window:
		m_DM_gaussWindow.resize(Ac_all * Ac_all);
		m_DM_lastCutOff = m_insertOptions_common->cutoffRadius;

		// Actually the array could be 1/4 of this size, but this
		// way it's easier and it's late night now :-)
		auto it = m_DM_gaussWindow.begin();
		for (unsigned cx = 0; cx < Ac_all; cx++)
		{
			for (unsigned cy = 0; cy < Ac_all; cy++)
			{
				dist = m_resolution *
					sqrt(static_cast<double>(
						square(Ac_cutoff + 1 - cx) +
						square(Ac_cutoff + 1 - cy)));
				*(it++) = std::exp(-square(dist / std));
			}
		}

		MRPT_LOG_DEBUG(
			"[CRandomFieldGridMap2D::insertObservation_KernelDM_DMV] Done!");
	}  // end of computing the gauss. window.

	//	Fuse with current content of grid (the MEAN of each cell):
	// --------------------------------------------------------------
	const int sensor_cx = x2idx(point.x);
	const int sensor_cy = y2idx(point.y);
	TRandomFieldCell* cell;
	auto windowIt = m_DM_gaussWindow.begin();

	for (int Acx = -Ac_cutoff; Acx <= Ac_cutoff; Acx++)
	{
		for (int Acy = -Ac_cutoff; Acy <= Ac_cutoff; ++Acy, ++windowIt)
		{
			const double windowValue = *windowIt;

			if (windowValue > minWinValueAtCutOff)
			{
				cell = cellByIndex(sensor_cx + Acx, sensor_cy + Acy);
				ASSERT_(cell != nullptr);
				cell->dm_mean_w() += windowValue;
				cell->dm_mean() += windowValue * normReading;
				if (is_DMV)
				{
					const double cell_var =
						square(normReading - computeMeanCellValue_DM_DMV(cell));
					cell->dmv_var_mean += windowValue * cell_var;
				}
			}
		}
	}

	MRPT_END
}

/*---------------------------------------------------------------
					TInsertionOptionsCommon
 ---------------------------------------------------------------*/
CRandomFieldGridMap2D::TInsertionOptionsCommon::TInsertionOptionsCommon()
	: cutoffRadius(sigma * 3.0),

	  GMRF_simplemap_file(""),
	  GMRF_gridmap_image_file(""),

	  GMRF_saturate_min(-std::numeric_limits<double>::max()),
	  GMRF_saturate_max(std::numeric_limits<double>::max())

{
}

/*---------------------------------------------------------------
					internal_dumpToTextStream_common
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::TInsertionOptionsCommon::
	internal_dumpToTextStream_common(std::ostream& out) const
{
	out << mrpt::format(
		"sigma                                   = %f\n", sigma);
	out << mrpt::format(
		"cutoffRadius                            = %f\n", cutoffRadius);
	out << mrpt::format(
		"R_min                                   = %f\n", R_min);
	out << mrpt::format(
		"R_max                                   = %f\n", R_max);
	out << mrpt::format(
		"dm_sigma_omega                          = %f\n", dm_sigma_omega);

	out << mrpt::format(
		"KF_covSigma                             = %f\n", KF_covSigma);
	out << mrpt::format(
		"KF_initialCellStd                       = %f\n", KF_initialCellStd);
	out << mrpt::format(
		"KF_observationModelNoise                = %f\n",
		KF_observationModelNoise);
	out << mrpt::format(
		"KF_defaultCellMeanValue                 = %f\n",
		KF_defaultCellMeanValue);
	out << mrpt::format(
		"KF_W_size                               = %u\n", (unsigned)KF_W_size);

	out << mrpt::format(
		"GMRF_lambdaPrior                        = %f\n", GMRF_lambdaPrior);
	out << mrpt::format(
		"GMRF_lambdaObs                          = %f\n", GMRF_lambdaObs);
	out << mrpt::format(
		"GMRF_lambdaObsLoss                      = %f\n", GMRF_lambdaObs);

	out << mrpt::format(
		"GMRF_use_occupancy_information          = %s\n",
		GMRF_use_occupancy_information ? "YES" : "NO");
	out << mrpt::format(
		"GMRF_simplemap_file                     = %s\n",
		GMRF_simplemap_file.c_str());
	out << mrpt::format(
		"GMRF_gridmap_image_file                 = %s\n",
		GMRF_gridmap_image_file.c_str());
	out << mrpt::format(
		"GMRF_gridmap_image_res                  = %f\n",
		GMRF_gridmap_image_res);
	out << mrpt::format(
		"GMRF_gridmap_image_cx                   = %u\n",
		static_cast<unsigned int>(GMRF_gridmap_image_cx));
	out << mrpt::format(
		"GMRF_gridmap_image_cy                   = %u\n",
		static_cast<unsigned int>(GMRF_gridmap_image_cy));
}

/*---------------------------------------------------------------
					internal_loadFromConfigFile_common
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::TInsertionOptionsCommon::
	internal_loadFromConfigFile_common(
		const mrpt::config::CConfigFileBase& iniFile,
		const std::string& section)
{
	sigma = iniFile.read_float(section.c_str(), "sigma", sigma);
	cutoffRadius =
		iniFile.read_float(section.c_str(), "cutoffRadius", cutoffRadius);
	R_min = iniFile.read_float(section.c_str(), "R_min", R_min);
	R_max = iniFile.read_float(section.c_str(), "R_max", R_max);
	MRPT_LOAD_CONFIG_VAR(dm_sigma_omega, double, iniFile, section);

	KF_covSigma =
		iniFile.read_float(section.c_str(), "KF_covSigma", KF_covSigma);
	KF_initialCellStd = iniFile.read_float(
		section.c_str(), "KF_initialCellStd", KF_initialCellStd);
	KF_observationModelNoise = iniFile.read_float(
		section.c_str(), "KF_observationModelNoise", KF_observationModelNoise);
	KF_defaultCellMeanValue = iniFile.read_float(
		section.c_str(), "KF_defaultCellMeanValue", KF_defaultCellMeanValue);
	MRPT_LOAD_CONFIG_VAR(KF_W_size, int, iniFile, section);

	GMRF_lambdaPrior = iniFile.read_float(
		section.c_str(), "GMRF_lambdaPrior", GMRF_lambdaPrior);
	GMRF_lambdaObs =
		iniFile.read_float(section.c_str(), "GMRF_lambdaObs", GMRF_lambdaObs);
	GMRF_lambdaObsLoss = iniFile.read_float(
		section.c_str(), "GMRF_lambdaObsLoss", GMRF_lambdaObsLoss);

	GMRF_use_occupancy_information = iniFile.read_bool(
		section.c_str(), "GMRF_use_occupancy_information", false, false);
	GMRF_simplemap_file =
		iniFile.read_string(section.c_str(), "simplemap_file", "", false);
	GMRF_gridmap_image_file =
		iniFile.read_string(section.c_str(), "gridmap_image_file", "", false);
	GMRF_gridmap_image_res =
		iniFile.read_float(section.c_str(), "gridmap_image_res", 0.01f, false);
	GMRF_gridmap_image_cx =
		iniFile.read_int(section.c_str(), "gridmap_image_cx", 0, false);
	GMRF_gridmap_image_cy =
		iniFile.read_int(section.c_str(), "gridmap_image_cy", 0, false);
}

/*---------------------------------------------------------------
					saveAsBitmapFile
 ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::saveAsBitmapFile(const std::string& filName) const
{
	MRPT_START

	mrpt::img::CImage img;
	getAsBitmapFile(img);
	img.saveToFile(filName);

	MRPT_END
}

/** Like saveAsBitmapFile(), but returns the data in matrix form */
void CRandomFieldGridMap2D::getAsMatrix(
	mrpt::math::CMatrixDouble& cells_mat) const
{
	MRPT_START
	cells_mat.resize(m_size_y, m_size_x);
	recoverMeanAndCov();  // Only has effects for KF2 method

	for (unsigned int y = 0; y < m_size_y; y++)
	{
		for (unsigned int x = 0; x < m_size_x; x++)
		{
			const TRandomFieldCell* cell = cellByIndex(x, y);
			ASSERT_(cell != nullptr);
			double c;

			switch (m_mapType)
			{
				case mrKernelDM:
				case mrKernelDMV: c = computeMeanCellValue_DM_DMV(cell); break;

				case mrKalmanFilter:
				case mrKalmanApproximate: c = cell->kf_mean(); break;
				case mrGMRF_SD: c = cell->gmrf_mean(); break;

				default: THROW_EXCEPTION("Unknown m_mapType!!");
			};
			mrpt::saturate(
				c, m_insertOptions_common->GMRF_saturate_min,
				m_insertOptions_common->GMRF_saturate_max);
			cells_mat(m_size_y - 1 - y, x) = c;
		}
	}
	MRPT_END
}

/*---------------------------------------------------------------
					getAsBitmapFile
 ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::getAsBitmapFile(mrpt::img::CImage& out_img) const
{
	MRPT_START
	mrpt::math::CMatrixDouble cells_mat;
	getAsMatrix(cells_mat);
	out_img.setFromMatrix(
		cells_mat, false /* vals are not normalized in by default [0,1] */);
	MRPT_END
}

/*---------------------------------------------------------------
					resize
 ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::resize(
	double new_x_min, double new_x_max, double new_y_min, double new_y_max,
	const TRandomFieldCell& defaultValueNewCells, double additionalMarginMeters)
{
	MRPT_START

	size_t old_sizeX = m_size_x;
	size_t old_sizeY = m_size_y;
	double old_x_min = m_x_min;
	double old_y_min = m_y_min;

	// The parent class method:
	CDynamicGrid<TRandomFieldCell>::resize(
		new_x_min, new_x_max, new_y_min, new_y_max, defaultValueNewCells,
		additionalMarginMeters);

	// Do we really resized?
	if (m_size_x != old_sizeX || m_size_y != old_sizeY)
	{
		// YES:
		// If we are in a Kalman Filter representation, also build the new
		// covariance matrix:
		if (m_mapType == mrKalmanFilter)
		{
			// ------------------------------------------
			//		Update the covariance matrix
			// ------------------------------------------
			size_t i, j, N = m_size_y * m_size_x;  // The new number of cells
			CMatrixD oldCov(m_cov);	 // Make a copy

			// m_cov.saveToTextFile("__debug_cov_before_resize.txt");

			printf(
				"[CRandomFieldGridMap2D::resize] Resizing from %ux%u to %ux%u "
				"(%u cells)\n",
				static_cast<unsigned>(old_sizeX),
				static_cast<unsigned>(old_sizeY),
				static_cast<unsigned>(m_size_x),
				static_cast<unsigned>(m_size_y),
				static_cast<unsigned>(m_size_x * m_size_y));

			m_cov.setSize(N, N);

			// Compute the new cells at the left and the bottom:
			size_t Acx_left = round((old_x_min - m_x_min) / m_resolution);
			size_t Acy_bottom = round((old_y_min - m_y_min) / m_resolution);

			// -------------------------------------------------------
			// STEP 1: Copy the old map values:
			// -------------------------------------------------------
			for (i = 0; i < N; i++)
			{
				size_t cx1 = i % m_size_x;
				size_t cy1 = i / m_size_x;

				bool C1_isFromOldMap = Acx_left <= cx1 &&
					cx1 < (Acx_left + old_sizeX) && Acy_bottom <= cy1 &&
					cy1 < (Acy_bottom + old_sizeY);

				if (C1_isFromOldMap)
				{
					for (j = i; j < N; j++)
					{
						size_t cx2 = j % m_size_x;
						size_t cy2 = j / m_size_x;

						bool C2_isFromOldMap = Acx_left <= cx2 &&
							cx2 < (Acx_left + old_sizeX) && Acy_bottom <= cy2 &&
							cy2 < (Acy_bottom + old_sizeY);

						// Were both cells in the old map??? --> Copy it!
						if (C1_isFromOldMap && C2_isFromOldMap)
						{
							// Copy values for the old matrix:
							unsigned int idx_c1 =
								((cx1 - Acx_left) +
								 old_sizeX * (cy1 - Acy_bottom));
							unsigned int idx_c2 =
								((cx2 - Acx_left) +
								 old_sizeX * (cy2 - Acy_bottom));

							MRPT_START

							ASSERT_(cx1 >= Acx_left);
							ASSERT_(cy1 >= Acy_bottom);
							ASSERT_((cx1 - Acx_left) < old_sizeX);
							ASSERT_((cy1 - Acy_bottom) < old_sizeY);

							ASSERT_(cx2 >= Acx_left);
							ASSERT_(cy2 >= Acy_bottom);
							ASSERT_((cx2 - Acx_left) < old_sizeX);
							ASSERT_((cy2 - Acy_bottom) < old_sizeY);

							ASSERT_(idx_c1 < old_sizeX * old_sizeY);
							ASSERT_(idx_c2 < old_sizeX * old_sizeY);

							MRPT_END_WITH_CLEAN_UP(
								printf("cx1=%u\n", static_cast<unsigned>(cx1));
								printf("cy1=%u\n", static_cast<unsigned>(cy1));
								printf("cx2=%u\n", static_cast<unsigned>(cx2));
								printf("cy2=%u\n", static_cast<unsigned>(cy2));
								printf(
									"Acx_left=%u\n",
									static_cast<unsigned>(Acx_left));
								printf(
									"Acy_bottom=%u\n",
									static_cast<unsigned>(Acy_bottom));
								printf(
									"idx_c1=%u\n",
									static_cast<unsigned>(idx_c1));
								printf(
									"idx_c2=%u\n",
									static_cast<unsigned>(idx_c2)););

							m_cov(i, j) = oldCov(idx_c1, idx_c2);
							m_cov(j, i) = m_cov(i, j);

							if (i == j) ASSERT_(idx_c1 == idx_c2);

							if (i == j && m_cov(i, i) < 0)
							{
								printf(
									"\ni=%u \nj=%i \ncx1,cy1 = %u,%u \n "
									"cx2,cy2=%u,%u \nidx_c1=%u \nidx_c2=%u "
									"\nAcx_left=%u \nAcy_bottom=%u "
									"\nold_sizeX=%u \n",
									static_cast<unsigned>(i),
									static_cast<unsigned>(j),
									static_cast<unsigned>(cx1),
									static_cast<unsigned>(cy1),
									static_cast<unsigned>(cx2),
									static_cast<unsigned>(cy2),
									static_cast<unsigned>(idx_c1),
									static_cast<unsigned>(idx_c2),
									static_cast<unsigned>(Acx_left),
									static_cast<unsigned>(Acy_bottom),
									static_cast<unsigned>(old_sizeX));
							}
						}

						ASSERT_(!std::isnan(m_cov(i, j)));

					}  // for j
				}
			}  // for i

			// -------------------------------------------------------
			// STEP 2: Set default values for new cells
			// -------------------------------------------------------
			for (i = 0; i < N; i++)
			{
				size_t cx1 = i % m_size_x;
				size_t cy1 = i / m_size_x;

				bool C1_isFromOldMap = Acx_left <= cx1 &&
					cx1 < (Acx_left + old_sizeX) && Acy_bottom <= cy1 &&
					cy1 < (Acy_bottom + old_sizeY);
				for (j = i; j < N; j++)
				{
					size_t cx2 = j % m_size_x;
					size_t cy2 = j / m_size_x;

					bool C2_isFromOldMap = Acx_left <= cx2 &&
						cx2 < (Acx_left + old_sizeX) && Acy_bottom <= cy2 &&
						cy2 < (Acy_bottom + old_sizeY);
					double dist = 0;

					// If both cells were NOT in the old map??? --> Introduce
					// default values:
					if (!C1_isFromOldMap || !C2_isFromOldMap)
					{
						// Set a new starting value:
						if (i == j)
						{
							m_cov(i, i) = square(
								m_insertOptions_common->KF_initialCellStd);
						}
						else
						{
							dist = m_resolution *
								sqrt(static_cast<double>(
									square(cx1 - cx2) + square(cy1 - cy2)));
							double K = sqrt(m_cov(i, i) * m_cov(j, j));

							if (std::isnan(K))
							{
								printf(
									"c(i,i)=%e   c(j,j)=%e\n", m_cov(i, i),
									m_cov(j, j));
								ASSERT_(!std::isnan(K));
							}

							m_cov(i, j) = K *
								exp(-0.5 *
									square(
										dist /
										m_insertOptions_common->KF_covSigma));
							m_cov(j, i) = m_cov(i, j);
						}

						ASSERT_(!std::isnan(m_cov(i, j)));
					}
				}  // for j
			}  // for i

			// m_cov.saveToTextFile("__debug_cov_after_resize.txt");
			// Resize done!
			printf("[CRandomFieldGridMap2D::resize] Done\n");

		}  // end of Kalman Filter map
		else if (m_mapType == mrKalmanApproximate)
		{
			// ------------------------------------------
			//		Approximate-Kalman filter
			// ------------------------------------------

			// Cells with "std" == -1 are new ones, we have to change their std
			//   to "m_insertOptions_common->KF_initialCellStd", then adapt
			//   appropriately
			//   the compressed cov. matrix:

			MRPT_LOG_DEBUG_FMT(
				"[resize] Resizing from %ux%u to %ux%u (%u cells)\n",
				static_cast<unsigned>(old_sizeX),
				static_cast<unsigned>(old_sizeY),
				static_cast<unsigned>(m_size_x),
				static_cast<unsigned>(m_size_y),
				static_cast<unsigned>(m_size_x * m_size_y));

			// Adapt the size of the cov. matrix:
			const signed W = m_insertOptions_common->KF_W_size;
			const size_t N = m_map.size();
			const size_t K = 2 * W * (W + 1) + 1;
			ASSERT_(int(K) == m_stackedCov.cols());
			ASSERT_(int(old_sizeX * old_sizeY) == m_stackedCov.rows());

			// Compute the new cells at the left and the bottom:
			size_t Acx_left = round((old_x_min - m_x_min) / m_resolution);
			size_t Acy_bottom = round((old_y_min - m_y_min) / m_resolution);

			m_stackedCov.setSize(N, K);

			// Prepare the template for new cells:
			CVectorDouble template_row(K);
			{
				const double std0sqr =
					square(m_insertOptions_common->KF_initialCellStd);
				double* ptr = &template_row[0];
				const double res2 = square(m_resolution);
				const double KF_covSigma2 =
					square(m_insertOptions_common->KF_covSigma);

				// 1) current cell
				*ptr++ = std0sqr;

				// 2) W rest of the first row:
				int Acx, Acy = 0;
				for (Acx = 1; Acx <= W; Acx++)
					*ptr++ = std0sqr *
						exp(-0.5 *
							(res2 *
							 static_cast<double>(square(Acx) + square(Acy))) /
							KF_covSigma2);

				// 3) The others W rows:
				for (Acy = 1; Acy <= W; Acy++)
					for (Acx = -W; Acx <= W; Acx++)
						*ptr++ = std0sqr *
							exp(-0.5 *
								(res2 *
								 static_cast<double>(
									 square(Acx) + square(Acy))) /
								KF_covSigma2);
			}

			// Go thru all the cells, from the bottom to the top so
			//  we don't need to make a temporary copy of the covariances:
			// i<N will become false for "i=-1" ;-)
			for (size_t i = N - 1; i < N; i--)
			{
				int cx, cy;
				idx2cxcy(i, cx, cy);

				const int old_idx_of_i =
					(cx - Acx_left) + (cy - Acy_bottom) * old_sizeX;

				TRandomFieldCell& cell = m_map[i];
				if (cell.kf_std() < 0)
				{
					// "i" is a new cell, fix its std and fill out the
					// compressed covariance:
					cell.kf_std() = m_insertOptions_common->KF_initialCellStd;

					double* new_row = &m_stackedCov(i, 0);
					memcpy(new_row, &template_row[0], sizeof(double) * K);
				}
				else
				{
					// "i" is an existing old cell: just copy the "m_stackedCov"
					// row:
					ASSERT_(int(old_idx_of_i) < m_stackedCov.rows());
					if (old_idx_of_i != int(i))	 // Copy row only if it's moved
					{
						const double* ptr_old = &m_stackedCov(old_idx_of_i, 0);
						double* ptr_new = &m_stackedCov(i, 0);
						memcpy(ptr_new, ptr_old, sizeof(double) * K);
					}
				}
			}  // end for i
		}  // end of Kalman-Approximate map
	}

	MRPT_END
}

/*---------------------------------------------------------------
					insertObservation_KF
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::insertObservation_KF(
	double normReading, const mrpt::math::TPoint2D& point)
{
	MRPT_START

	const TRandomFieldCell defCell(
		m_insertOptions_common->KF_defaultCellMeanValue,  // mean
		m_insertOptions_common->KF_initialCellStd  // std
	);

	// Assure we have room enough in the grid!
	resize(point.x - 1, point.x + 1, point.y - 1, point.y + 1, defCell);

	// --------------------------------------------------------
	// The Kalman-Filter estimation of the MRF grid-map:
	// --------------------------------------------------------

	// Prediction stage of KF:
	// ------------------------------------
	// Nothing to do here (static map)

	// Update stage of KF:
	//  We directly apply optimized formulas arising
	//   from our concrete sensor model.
	// -------------------------------------------------
	int cellIdx = xy2idx(point.x, point.y);
	TRandomFieldCell* cell = cellByPos(point.x, point.y);
	ASSERT_(cell != nullptr);
	size_t N, i, j;

	double yk = normReading - cell->kf_mean();	// Predicted observation mean
	double sk =
		m_cov(cellIdx, cellIdx) +
		square(
			m_insertOptions_common
				->KF_observationModelNoise);  // Predicted observation variance
	double sk_1 = 1.0 / sk;

	// The kalman gain:
	std::vector<TRandomFieldCell>::iterator it;

	static CTicTac tictac;
	MRPT_LOG_DEBUG("[insertObservation_KF] Updating mean values...");
	tictac.Tic();

	// Update mean values:
	// ---------------------------------------------------------
	for (i = 0, it = m_map.begin(); it != m_map.end(); ++it, ++i)
		// it->kf_mean =  it->kf_mean + yk * sk_1 * m_cov(i,cellIdx);
		it->kf_mean() += yk * sk_1 * m_cov(i, cellIdx);

	MRPT_LOG_DEBUG_FMT("Done in %.03fms\n", tictac.Tac() * 1000);

	// Update covariance matrix values:
	// ---------------------------------------------------------
	N = m_cov.rows();

	MRPT_LOG_DEBUG("[insertObservation_KF] Updating covariance matrix...");
	tictac.Tic();

	// We need to refer to the old cov: make an efficient copy of it:
	auto* oldCov = (double*)/*mrpt_alloca*/ malloc(sizeof(double) * N * N);
	double* oldCov_ptr = oldCov;
	for (i = 0; i < N; i++)
	{
		memcpy(oldCov_ptr, &m_cov(i, 0), sizeof(double) * N);
		oldCov_ptr += N;
	}

	MRPT_LOG_DEBUG_FMT(
		"Copy matrix %ux%u: %.06fms\n", (unsigned)m_cov.rows(),
		(unsigned)m_cov.cols(), tictac.Tac() * 1000);

	// The following follows from the expansion of Kalman Filter matrix
	// equations
	// TODO: Add references to some paper (if any)?
	const double* oldCov_row_c = oldCov + cellIdx * N;
	for (i = 0; i < N; i++)
	{
		const double oldCov_i_c = oldCov[i * N + cellIdx];
		const double sk_1_oldCov_i_c = sk_1 * oldCov_i_c;

		const double* oldCov_row_i = oldCov + i * N;
		for (j = i; j < N; j++)
		{
			double new_cov_ij =
				oldCov_row_i[j] - sk_1_oldCov_i_c * oldCov_row_c[j];

			// Make symmetric:
			m_cov(i, j) = new_cov_ij;
			m_cov(j, i) = new_cov_ij;

			// Update the "std" in the cell as well:
			if (i == j)
			{
				if (m_cov(i, i) < 0)
				{
					printf(
						"Wrong insertion in KF! m_cov(%u,%u) = %.5f",
						static_cast<unsigned int>(i),
						static_cast<unsigned int>(i), m_cov(i, i));
				}

				ASSERT_(m_cov(i, i) >= 0);
				m_map[i].kf_std() = sqrt(new_cov_ij);
			}
		}  // j
	}  // i

	// Free mem:
	/*mrpt_alloca_*/ free(oldCov);

	MRPT_LOG_DEBUG_FMT("Done! %.03fms\n", tictac.Tac() * 1000);

	MRPT_END
}

/*---------------------------------------------------------------
					saveMetricMapRepresentationToFile
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::saveMetricMapRepresentationToFile(
	const std::string& filNamePrefix) const
{
	std::string fil;

// Save as a bitmap:
#if MRPT_HAS_OPENCV
	fil = filNamePrefix + std::string("_mean.png");
	saveAsBitmapFile(fil);
#endif

	// Save dimensions of the grid (for any mapping algorithm):
	CMatrixF DIMs(1, 4);
	DIMs(0, 0) = m_x_min;
	DIMs(0, 1) = m_x_max;
	DIMs(0, 2) = m_y_min;
	DIMs(0, 3) = m_y_max;

	DIMs.saveToTextFile(
		filNamePrefix + std::string("_grid_limits.txt"), MATRIX_FORMAT_FIXED,
		false /* add mrpt header */,
		"% Grid limits: [x_min x_max y_min y_max]\n");

	switch (m_mapType)
	{
		case mrKernelDM:
		case mrKernelDMV:
		{
			CMatrixF all_means(m_size_y, m_size_x);
			CMatrixF all_vars(m_size_y, m_size_x);
			CMatrixF all_confs(m_size_y, m_size_x);

			for (size_t y = 0; y < m_size_y; y++)
				for (size_t x = 0; x < m_size_x; x++)
				{
					const TRandomFieldCell* cell = cellByIndex(x, y);
					all_means(y, x) = computeMeanCellValue_DM_DMV(cell);
					all_vars(y, x) = computeVarCellValue_DM_DMV(cell);
					all_confs(y, x) = computeConfidenceCellValue_DM_DMV(cell);
				}

			all_means.saveToTextFile(
				filNamePrefix + std::string("_mean.txt"), MATRIX_FORMAT_FIXED);
			if (m_mapType == mrKernelDMV)
			{
				all_vars.saveToTextFile(
					filNamePrefix + std::string("_var.txt"),
					MATRIX_FORMAT_FIXED);
				all_confs.saveToTextFile(
					filNamePrefix + std::string("_confidence.txt"),
					MATRIX_FORMAT_FIXED);
			}
		}
		break;

		case mrKalmanFilter:
		case mrKalmanApproximate:
		{
			recoverMeanAndCov();

			// Save the mean and std matrix:
			CMatrixF MEAN(m_size_y, m_size_x);
			CMatrixF STDs(m_size_y, m_size_x);

			for (size_t i = 0; i < m_size_y; i++)
				for (size_t j = 0; j < m_size_x; j++)
				{
					MEAN(i, j) = cellByIndex(j, i)->kf_mean();
					STDs(i, j) = cellByIndex(j, i)->kf_std();
				}

			MEAN.saveToTextFile(
				filNamePrefix + std::string("_mean.txt"), MATRIX_FORMAT_FIXED);
			STDs.saveToTextFile(
				filNamePrefix + std::string("_cells_std.txt"),
				MATRIX_FORMAT_FIXED);

			if (m_mapType == mrKalmanApproximate)
			{
				m_stackedCov.saveToTextFile(
					filNamePrefix + std::string("_mean_compressed_cov.txt"),
					MATRIX_FORMAT_FIXED);
			}
			if (m_mapType == mrKalmanFilter)
			{
				// Save the covariance matrix:
				m_cov.saveToTextFile(
					filNamePrefix + std::string("_mean_cov.txt"));
			}

			// And also as bitmap:
			CImage img_cov;
			img_cov.setFromMatrix(STDs, false /*it's not normalized*/);
			img_cov.saveToFile(
				filNamePrefix + std::string("_cells_std.png"),
				true /* vertical flip */);

			// Save the 3D graphs:
			saveAsMatlab3DGraph(filNamePrefix + std::string("_3D.m"));
		}
		break;

		case mrGMRF_SD:
		{
			// Save the mean and std matrix:
			CMatrixF MEAN(m_size_y, m_size_x);
			CMatrixF STDs(m_size_y, m_size_x);
			CMatrixD XYZ(m_size_y * m_size_x, 4);

			size_t idx = 0;
			for (size_t i = 0; i < m_size_y; ++i)
			{
				for (size_t j = 0; j < m_size_x; ++j, ++idx)
				{
					MEAN(i, j) = cellByIndex(j, i)->gmrf_mean();
					STDs(i, j) = cellByIndex(j, i)->gmrf_std();

					XYZ(idx, 0) = idx2x(j);
					XYZ(idx, 1) = idx2y(i);
					XYZ(idx, 2) = cellByIndex(j, i)->gmrf_mean();
					XYZ(idx, 3) = cellByIndex(j, i)->gmrf_std();
				}
			}

			MEAN.saveToTextFile(
				filNamePrefix + std::string("_mean.txt"), MATRIX_FORMAT_FIXED);
			STDs.saveToTextFile(
				filNamePrefix + std::string("_cells_std.txt"),
				MATRIX_FORMAT_FIXED);
			XYZ.saveToTextFile(
				filNamePrefix + std::string("_xyz_and_std.txt"),
				MATRIX_FORMAT_FIXED, false,
				"% Columns: GRID_X   GRID_Y   ESTIMATED_Z   "
				"STD_DEV_OF_ESTIMATED_Z \n");
		}
		break;

		default: THROW_EXCEPTION("Unknown method!");
	};	// end switch
}

/*---------------------------------------------------------------
					saveAsMatlab3DGraph
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::saveAsMatlab3DGraph(
	const std::string& filName) const
{
	MRPT_START

	const double std_times = 3;

	ASSERT_(
		m_mapType == mrKalmanFilter || m_mapType == mrKalmanApproximate ||
		m_mapType == mrGMRF_SD);

	recoverMeanAndCov();

	FILE* f = os::fopen(filName.c_str(), "wt");
	if (!f) THROW_EXCEPTION("Couldn't create output file!");

	os::fprintf(
		f, "%%-------------------------------------------------------\n");
	os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
	os::fprintf(f, "%%'CRandomFieldGridMap2D::saveAsMatlab3DGraph'\n");
	os::fprintf(f, "%%\n");
	os::fprintf(f, "%%                        ~ MRPT ~\n");
	os::fprintf(
		f, "%%  Jose Luis Blanco Claraco, University of Malaga @ 2006-2007\n");
	os::fprintf(f, "%%  http://www.isa.uma.es/ \n");
	os::fprintf(
		f, "%%-------------------------------------------------------\n\n");

	unsigned int cx, cy;
	vector<double> xs, ys;

	// xs: array of X-axis values
	os::fprintf(f, "xs = [");
	xs.resize(m_size_x);
	for (cx = 0; cx < m_size_x; cx++)
	{
		xs[cx] = m_x_min + m_resolution * cx;
		os::fprintf(f, "%f ", xs[cx]);
	}
	os::fprintf(f, "];\n");

	// ys: array of X-axis values
	os::fprintf(f, "ys = [");
	ys.resize(m_size_y);
	for (cy = 0; cy < m_size_y; cy++)
	{
		ys[cy] = m_y_min + m_resolution * cy;
		os::fprintf(f, "%f ", ys[cy]);
	}
	os::fprintf(f, "];\n");

	// z_mean: matrix with mean concentration values
	os::fprintf(f, "z_mean = [\n");
	for (cy = 0; cy < m_size_y; cy++)
	{
		for (cx = 0; cx < m_size_x; cx++)
		{
			const TRandomFieldCell* cell = cellByIndex(cx, cy);
			ASSERT_(cell != nullptr);
			os::fprintf(f, "%e ", cell->kf_mean());
		}  // for cx

		if (cy < (m_size_y - 1)) os::fprintf(f, "; ...\n");
	}  // for cy
	os::fprintf(f, "];\n\n");

	// z_upper: matrix with upper confidence levels for concentration values
	os::fprintf(f, "z_upper = [\n");
	for (cy = 0; cy < m_size_y; cy++)
	{
		for (cx = 0; cx < m_size_x; cx++)
		{
			const TRandomFieldCell* cell = cellByIndex(cx, cy);
			ASSERT_(cell != nullptr);
			os::fprintf(
				f, "%e ",
				mrpt::saturate_val(
					cell->kf_mean() + std_times * cell->kf_std(),
					m_insertOptions_common->GMRF_saturate_min,
					m_insertOptions_common->GMRF_saturate_max));
		}  // for cx

		if (cy < (m_size_y - 1)) os::fprintf(f, "; ...\n");
	}  // for cy
	os::fprintf(f, "];\n\n");

	// z_lowe: matrix with lower confidence levels for concentration values
	os::fprintf(f, "z_lower = [\n");
	for (cy = 0; cy < m_size_y; cy++)
	{
		for (cx = 0; cx < m_size_x; cx++)
		{
			const TRandomFieldCell* cell = cellByIndex(cx, cy);
			ASSERT_(cell != nullptr);
			os::fprintf(
				f, "%e ",
				mrpt::saturate_val(
					cell->kf_mean() - std_times * cell->kf_std(),
					m_insertOptions_common->GMRF_saturate_min,
					m_insertOptions_common->GMRF_saturate_max));
		}  // for cx

		if (cy < (m_size_y - 1)) os::fprintf(f, "; ...\n");
	}  // for cy
	os::fprintf(f, "];\n\n");

	// Plot them:
	os::fprintf(f, "hold off;\n");
	os::fprintf(f, "S1 = surf(xs,ys,z_mean);\n");
	os::fprintf(f, "hold on;\n");
	os::fprintf(f, "S2 = surf(xs,ys,z_upper);\n");
	os::fprintf(f, "S3 = surf(xs,ys,z_lower);\n");
	os::fprintf(f, "\n");
	os::fprintf(f, "set(S1,'FaceAlpha',0.9,'EdgeAlpha',0.9);\n");
	os::fprintf(f, "set(S2,'FaceAlpha',0.4,'EdgeAlpha',0.4);\n");
	os::fprintf(f, "set(S3,'FaceAlpha',0.2,'EdgeAlpha',0.2);\n");
	os::fprintf(f, "\n");
	os::fprintf(
		f, "set(gca,'PlotBoxAspectRatio',[%f %f %f]);\n", m_x_max - m_x_min,
		m_y_max - m_y_min, 4.0);
	os::fprintf(f, "view(-40,30)\n");
	os::fprintf(
		f, "axis([%f %f %f %f 0 1]);\n", m_x_min, m_x_max, m_y_min, m_y_max);

	os::fprintf(
		f,
		"\nfigure; imagesc(xs,ys,z_mean);axis equal;title('Mean "
		"value');colorbar;");
	os::fprintf(
		f,
		"\nfigure; imagesc(xs,ys,(z_upper-z_mean)/%f);axis equal;title('Std "
		"dev of estimated value');colorbar;",
		std_times);

	fclose(f);

	MRPT_END
}

void CRandomFieldGridMap2D::getVisualizationInto(
	mrpt::opengl::CSetOfObjects& o) const
{
	if (!genericMapParams.enableSaveAs3DObject) return;

	// Returns only the mean map
	mrpt::opengl::CSetOfObjects other_obj;
	getAs3DObject(o, other_obj);
}

void CRandomFieldGridMap2D::getAs3DObject(
	mrpt::opengl::CSetOfObjects& meanObj,
	mrpt::opengl::CSetOfObjects& varObj) const
{
	if (!genericMapParams.enableSaveAs3DObject) return;

	recoverMeanAndCov();  // Only works for KF2 method

	mrpt::opengl::TTriangle triag;

	unsigned int cx, cy;
	vector<double> xs, ys;

	// xs: array of X-axis values
	xs.resize(m_size_x);
	for (cx = 0; cx < m_size_x; cx++)
		xs[cx] = m_x_min + m_resolution * cx;

	// ys: array of Y-axis values
	ys.resize(m_size_y);
	for (cy = 0; cy < m_size_y; cy++)
		ys[cy] = m_y_min + m_resolution * cy;

	// Draw the surfaces:
	switch (m_mapType)
	{
		case mrKalmanFilter:
		case mrKalmanApproximate:
		case mrGMRF_SD:
		{
			// for Kalman models:
			// ----------------------------------
			auto obj_m = std::make_shared<opengl::CSetOfTriangles>();
			auto obj_v = std::make_shared<opengl::CSetOfTriangles>();

			//  Compute mean max/min values:
			// ---------------------------------------
			double maxVal_m = 0, minVal_m = 1, AMaxMin_m, maxVal_v = 0,
				   minVal_v = 1, AMaxMin_v;
			double c, v;
			for (cy = 1; cy < m_size_y; cy++)
			{
				for (cx = 1; cx < m_size_x; cx++)
				{
					const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
					ASSERT_(cell_xy != nullptr);
					// mean
					c = cell_xy->kf_mean();
					minVal_m = min(minVal_m, c);
					maxVal_m = max(maxVal_m, c);
					// variance
					v = square(cell_xy->kf_std());
					minVal_v = min(minVal_v, v);
					maxVal_v = max(maxVal_v, v);
				}
			}

			AMaxMin_m = maxVal_m - minVal_m;
			if (AMaxMin_m == 0) AMaxMin_m = 1;
			AMaxMin_v = maxVal_v - minVal_v;
			if (AMaxMin_v == 0) AMaxMin_v = 1;

			// ---------------------------------------
			//  Compute Maps
			// ---------------------------------------
			// alpha (transparency)
			triag.a(0) = triag.a(1) = triag.a(2) = 0xA0;
			for (cy = 1; cy < m_size_y; cy++)
			{
				for (cx = 1; cx < m_size_x; cx++)
				{
					// Cell values:
					const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
					ASSERT_(cell_xy != nullptr);
					const TRandomFieldCell* cell_x_1y = cellByIndex(cx - 1, cy);
					ASSERT_(cell_x_1y != nullptr);
					const TRandomFieldCell* cell_xy_1 = cellByIndex(cx, cy - 1);
					ASSERT_(cell_xy_1 != nullptr);
					const TRandomFieldCell* cell_x_1y_1 =
						cellByIndex(cx - 1, cy - 1);
					ASSERT_(cell_x_1y_1 != nullptr);

					// MEAN values
					//-----------------
					double c_xy = mrpt::saturate_val(
						cell_xy->kf_mean(),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);
					double c_x_1y = mrpt::saturate_val(
						cell_x_1y->kf_mean(),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);
					double c_xy_1 = mrpt::saturate_val(
						cell_xy_1->kf_mean(),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);
					double c_x_1y_1 = mrpt::saturate_val(
						cell_x_1y_1->kf_mean(),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);

					double col_xy = c_xy;
					double col_x_1y = c_x_1y;
					double col_xy_1 = c_xy_1;
					double col_x_1y_1 = c_x_1y_1;

					// Triangle #1:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = c_xy;
					triag.x(1) = xs[cx];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = c_xy_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy - 1];
					triag.z(2) = c_x_1y_1;

					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_xy_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y_1));

					obj_m->insertTriangle(triag);

					// Triangle #2:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = c_xy;
					triag.x(1) = xs[cx - 1];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = c_x_1y_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy];
					triag.z(2) = c_x_1y;
					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_x_1y_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y));
					obj_m->insertTriangle(triag);

					// VARIANCE values
					//------------------
					double v_xy =
						min(10.0, max(0.0, square(cell_xy->kf_std())));
					double v_x_1y =
						min(10.0, max(0.0, square(cell_x_1y->kf_std())));
					double v_xy_1 =
						min(10.0, max(0.0, square(cell_xy_1->kf_std())));
					double v_x_1y_1 =
						min(10.0, max(0.0, square(cell_x_1y_1->kf_std())));

					col_xy = v_xy /
						10;	 // min(1.0,max(0.0, (v_xy-minVal_v)/AMaxMin_v ) );
					col_x_1y = v_x_1y / 10;	 // min(1.0,max(0.0,
					// (v_x_1y-minVal_v)/AMaxMin_v ) );
					col_xy_1 = v_xy_1 / 10;	 // min(1.0,max(0.0,
					// (v_xy_1-minVal_v)/AMaxMin_v ) );
					col_x_1y_1 = v_x_1y_1 / 10;	 // min(1.0,max(0.0,
					// (v_x_1y_1-minVal_v)/AMaxMin_v ) );

					// Triangle #1:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = c_xy + v_xy;
					triag.x(1) = xs[cx];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = c_xy_1 + v_xy_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy - 1];
					triag.z(2) = c_x_1y_1 + v_x_1y_1;

					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_xy_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y_1));

					obj_v->insertTriangle(triag);

					// Triangle #2:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = c_xy + v_xy;
					triag.x(1) = xs[cx - 1];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = c_x_1y_1 + v_x_1y_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy];
					triag.z(2) = c_x_1y + v_x_1y;

					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_x_1y_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y));

					obj_v->insertTriangle(triag);

				}  // for cx
			}  // for cy
			meanObj.insert(obj_m);
			varObj.insert(obj_v);
		}
		break;	// end KF models

		case mrKernelDM:
		case mrKernelDMV:
		{
			// Draw for Kernel model:
			// ----------------------------------
			auto obj_m = std::make_shared<opengl::CSetOfTriangles>();
			auto obj_v = std::make_shared<opengl::CSetOfTriangles>();

			//  Compute mean max/min values:
			// ---------------------------------------
			double maxVal_m = 0, minVal_m = 1, AMaxMin_m, maxVal_v = 0,
				   minVal_v = 1, AMaxMin_v;
			double c, v;
			for (cy = 1; cy < m_size_y; cy++)
			{
				for (cx = 1; cx < m_size_x; cx++)
				{
					const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
					ASSERT_(cell_xy != nullptr);
					// mean
					c = computeMeanCellValue_DM_DMV(cell_xy);
					minVal_m = min(minVal_m, c);
					maxVal_m = max(maxVal_m, c);
					// variance
					v = computeVarCellValue_DM_DMV(cell_xy);
					minVal_v = min(minVal_v, v);
					maxVal_v = max(maxVal_v, v);
				}
			}

			AMaxMin_m = maxVal_m - minVal_m;
			if (AMaxMin_m == 0) AMaxMin_m = 1;
			AMaxMin_v = maxVal_v - minVal_v;
			if (AMaxMin_v == 0) AMaxMin_v = 1;

			// ---------------------------------------
			//  Compute Maps
			// ---------------------------------------
			triag.a(0) = triag.a(1) = triag.a(2) = 0xA0;  // (transparency)
			for (cy = 1; cy < m_size_y; cy++)
			{
				for (cx = 1; cx < m_size_x; cx++)
				{
					// Cell values:
					const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
					ASSERT_(cell_xy != nullptr);
					const TRandomFieldCell* cell_x_1y = cellByIndex(cx - 1, cy);
					ASSERT_(cell_x_1y != nullptr);
					const TRandomFieldCell* cell_xy_1 = cellByIndex(cx, cy - 1);
					ASSERT_(cell_xy_1 != nullptr);
					const TRandomFieldCell* cell_x_1y_1 =
						cellByIndex(cx - 1, cy - 1);
					ASSERT_(cell_x_1y_1 != nullptr);

					// MEAN values
					//-----------------
					double c_xy = mrpt::saturate_val(
						computeMeanCellValue_DM_DMV(cell_xy),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);
					double c_x_1y = mrpt::saturate_val(
						computeMeanCellValue_DM_DMV(cell_x_1y),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);
					double c_xy_1 = mrpt::saturate_val(
						computeMeanCellValue_DM_DMV(cell_xy_1),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);
					double c_x_1y_1 = mrpt::saturate_val(
						computeMeanCellValue_DM_DMV(cell_x_1y_1),
						m_insertOptions_common->GMRF_saturate_min,
						m_insertOptions_common->GMRF_saturate_max);

					double col_xy = c_xy;
					double col_x_1y = c_x_1y;
					double col_xy_1 = c_xy_1;
					double col_x_1y_1 = c_x_1y_1;

					// Triangle #1:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = c_xy;
					triag.x(1) = xs[cx];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = c_xy_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy - 1];
					triag.z(2) = c_x_1y_1;

					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_xy_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y_1));

					obj_m->insertTriangle(triag);

					// Triangle #2:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = c_xy;
					triag.x(1) = xs[cx - 1];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = c_x_1y_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy];
					triag.z(2) = c_x_1y;

					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_x_1y_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y));

					obj_m->insertTriangle(triag);

					// VARIANCE values
					//------------------
					double v_xy =
						min(1.0, max(0.0, computeVarCellValue_DM_DMV(cell_xy)));
					double v_x_1y = min(
						1.0, max(0.0, computeVarCellValue_DM_DMV(cell_x_1y)));
					double v_xy_1 = min(
						1.0, max(0.0, computeVarCellValue_DM_DMV(cell_xy_1)));
					double v_x_1y_1 = min(
						1.0, max(0.0, computeVarCellValue_DM_DMV(cell_x_1y_1)));

					col_xy = v_xy;
					col_x_1y = v_x_1y;
					col_xy_1 = v_xy_1;
					col_x_1y_1 = v_x_1y_1;

					// Triangle #1:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = v_xy;
					triag.x(1) = xs[cx];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = v_xy_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy - 1];
					triag.z(2) = v_x_1y_1;

					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_xy_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y_1));

					obj_v->insertTriangle(triag);

					// Triangle #2:
					triag.x(0) = xs[cx];
					triag.y(0) = ys[cy];
					triag.z(0) = v_xy;
					triag.x(1) = xs[cx - 1];
					triag.y(1) = ys[cy - 1];
					triag.z(1) = v_x_1y_1;
					triag.x(2) = xs[cx - 1];
					triag.y(2) = ys[cy];
					triag.z(2) = v_x_1y;

					triag.vertices[0].setColor(colormap(cmJET, col_xy));
					triag.vertices[1].setColor(colormap(cmJET, col_x_1y_1));
					triag.vertices[2].setColor(colormap(cmJET, col_x_1y));

					obj_v->insertTriangle(triag);

				}  // for cx
			}  // for cy
			meanObj.insert(obj_m);
			varObj.insert(obj_v);
		}
		break;	// end Kernel models
	};	// end switch maptype
}

/*---------------------------------------------------------------
					computeConfidenceCellValue_DM_DMV
 ---------------------------------------------------------------*/
double CRandomFieldGridMap2D::computeConfidenceCellValue_DM_DMV(
	const TRandomFieldCell* cell) const
{
	// A confidence measure:
	return 1.0 -
		std::exp(-square(
			cell->dm_mean_w() / m_insertOptions_common->dm_sigma_omega));
}

/*---------------------------------------------------------------
					computeMeanCellValue_DM_DMV
 ---------------------------------------------------------------*/
double CRandomFieldGridMap2D::computeMeanCellValue_DM_DMV(
	const TRandomFieldCell* cell) const
{
	// A confidence measure:
	const double alpha = 1.0 -
		std::exp(-square(
			cell->dm_mean_w() / m_insertOptions_common->dm_sigma_omega));
	const double r_val =
		(cell->dm_mean_w() > 0) ? (cell->dm_mean() / cell->dm_mean_w()) : 0;
	return alpha * r_val + (1 - alpha) * m_average_normreadings_mean;
}

/*---------------------------------------------------------------
					computeVarCellValue_DM_DMV
 ---------------------------------------------------------------*/
double CRandomFieldGridMap2D::computeVarCellValue_DM_DMV(
	const TRandomFieldCell* cell) const
{
	// A confidence measure:
	const double alpha = 1.0 -
		std::exp(-square(
			cell->dm_mean_w() / m_insertOptions_common->dm_sigma_omega));
	const double r_val =
		(cell->dm_mean_w() > 0) ? (cell->dmv_var_mean / cell->dm_mean_w()) : 0;
	return alpha * r_val + (1 - alpha) * m_average_normreadings_var;
}

struct TInterpQuery
{
	int cx, cy;
	double val, var;
	double coef;
};

/*---------------------------------------------------------------
					predictMeasurement
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::predictMeasurement(
	const double x, const double y, double& out_predict_response,
	double& out_predict_response_variance, bool do_sensor_normalization,
	const TGridInterpolationMethod interp_method)
{
	MRPT_START

	vector<TInterpQuery> queries;
	switch (interp_method)
	{
		case gimNearest:
			queries.resize(1);
			queries[0].cx = x2idx(x);
			queries[0].cy = y2idx(y);
			queries[0].coef = 1.0;
			break;

		case gimBilinear:
			if (x <= m_x_min + m_resolution * 0.5 ||
				y <= m_y_min + m_resolution * 0.5 ||
				x >= m_x_max - m_resolution * 0.5 ||
				x >= m_x_max - m_resolution * 0.5)
			{
				// Too close to a border:
				queries.resize(1);
				queries[0].cx = x2idx(x);
				queries[0].cy = y2idx(y);
				queries[0].coef = 1.0;
			}
			else
			{
				queries.resize(4);
				const double K_1 = 1.0 / (m_resolution * m_resolution);
				// 11
				queries[0].cx = x2idx(x - m_resolution * 0.5);
				queries[0].cy = y2idx(y - m_resolution * 0.5);
				// 12
				queries[1].cx = x2idx(x - m_resolution * 0.5);
				queries[1].cy = y2idx(y + m_resolution * 0.5);
				// 21
				queries[2].cx = x2idx(x + m_resolution * 0.5);
				queries[2].cy = y2idx(y - m_resolution * 0.5);
				// 22
				queries[3].cx = x2idx(x + m_resolution * 0.5);
				queries[3].cy = y2idx(y + m_resolution * 0.5);
				// Weights:
				queries[0].coef = K_1 * (idx2x(queries[3].cx) - x) *
					(idx2y(queries[3].cy) - y);
				queries[1].coef = K_1 * (idx2x(queries[3].cx) - x) *
					(y - idx2y(queries[0].cy));
				queries[2].coef = K_1 * (x - idx2x(queries[0].cx)) *
					(idx2y(queries[3].cy) - y);
				queries[3].coef = K_1 * (x - idx2x(queries[0].cx)) *
					(y - idx2y(queries[0].cy));
			}
			break;
		default: THROW_EXCEPTION("Unknown interpolation method!");
	};

	// Run queries:
	for (auto& q : queries)
	{
		const TRandomFieldCell* cell = cellByIndex(q.cx, q.cy);
		switch (m_mapType)
		{
			case mrKernelDM:
			{
				if (!cell)
				{
					q.val = m_average_normreadings_mean;
					q.var = square(m_insertOptions_common->KF_initialCellStd);
				}
				else
				{
					q.val = computeMeanCellValue_DM_DMV(cell);
					q.var = square(m_insertOptions_common->KF_initialCellStd);
				}
			}
			break;
			case mrKernelDMV:
			{
				if (!cell)
				{
					q.val = m_average_normreadings_mean;
					q.var = square(m_insertOptions_common->KF_initialCellStd);
				}
				else
				{
					q.val = computeMeanCellValue_DM_DMV(cell);
					q.var = computeVarCellValue_DM_DMV(cell);
				}
			}
			break;

			case mrKalmanFilter:
			case mrKalmanApproximate:
			case mrGMRF_SD:
			{
				if (m_mapType == mrKalmanApproximate &&
					m_hasToRecoverMeanAndCov)
					recoverMeanAndCov();  // Just for KF2

				if (!cell)
				{
					q.val = m_insertOptions_common->KF_defaultCellMeanValue;
					q.var =
						square(m_insertOptions_common->KF_initialCellStd) +
						square(
							m_insertOptions_common->KF_observationModelNoise);
				}
				else
				{
					q.val = cell->kf_mean();
					q.var =
						square(cell->kf_std()) +
						square(
							m_insertOptions_common->KF_observationModelNoise);
				}
			}
			break;

			default: THROW_EXCEPTION("Invalid map type.");
		};
	}

	// Sum coeffs:
	out_predict_response = 0;
	out_predict_response_variance = 0;
	for (auto& querie : queries)
	{
		out_predict_response += querie.val * querie.coef;
		out_predict_response_variance += querie.var * querie.coef;
	}

	// Un-do the sensor normalization:
	if (do_sensor_normalization)
		out_predict_response = m_insertOptions_common->R_min +
			out_predict_response *
				(m_insertOptions_common->R_max - m_insertOptions_common->R_min);

	MRPT_END
}

/*---------------------------------------------------------------
					insertObservation_KF2
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::insertObservation_KF2(
	double normReading, const mrpt::math::TPoint2D& point)
{
	MRPT_START

	MRPT_LOG_DEBUG_STREAM(
		"Inserting KF2: (" << normReading << ") at Position" << point << endl);

	const signed W = m_insertOptions_common->KF_W_size;
	const size_t K = 2 * W * (W + 1) + 1;
	const size_t W21 = 2 * W + 1;
	const size_t W21sqr = W21 * W21;

	ASSERT_(W >= 2);

	m_hasToRecoverMeanAndCov = true;

	const TRandomFieldCell defCell(
		m_insertOptions_common->KF_defaultCellMeanValue,  // mean
		-1	// Just to indicate that cells are new, next changed to:
		// m_insertOptions_common->KF_initialCellStd			// std
	);

	// Assure we have room enough in the grid!
	const double Aspace = (W + 1) * m_resolution;

	resize(
		point.x - Aspace, point.x + Aspace, point.y - Aspace, point.y + Aspace,
		defCell, Aspace);

	// --------------------------------------------------------
	// The Kalman-Filter estimation of the MRF grid-map:
	// --------------------------------------------------------
	const size_t N = m_map.size();

	ASSERT_(int(K) == m_stackedCov.cols());
	ASSERT_(int(N) == m_stackedCov.rows());

	// Prediction stage of KF:
	// ------------------------------------
	// Nothing to do here (static map)

	// Update stage of KF:
	//  We directly apply optimized formulas arising
	//   from our concrete sensor model.
	// -------------------------------------------------

	// const double	KF_covSigma2 = square(m_insertOptions_common->KF_covSigma);
	// const double	std0 = m_insertOptions_common->KF_initialCellStd;
	// const double	res2 = square(m_resolution);
	const int cellIdx = xy2idx(point.x, point.y);
	TRandomFieldCell* cell = cellByPos(point.x, point.y);
	ASSERT_(cell != nullptr);

	// Predicted observation mean
	double yk = normReading - cell->kf_mean();

	// Predicted observation variance
	double sk = m_stackedCov(cellIdx, 0) +	// Variance of that cell: cov(i,i)
		square(m_insertOptions_common->KF_observationModelNoise);

	double sk_1 = 1.0 / sk;

	static CTicTac tictac;
	MRPT_LOG_DEBUG("[insertObservation_KF2] Updating mean values...");
	tictac.Tic();

	// ------------------------------------------------------------
	// Update mean values:
	//  Process only those cells in the vecinity of the cell (c):
	//
	//   What follows is *** REALLY UGLY *** for efficiency, sorry!!  :-)
	// ------------------------------------------------------------
	const int cx_c = x2idx(point.x);
	const int cy_c = y2idx(point.y);

	const int Acx0 = max(-W, -cx_c);
	const int Acy0 = max(-W, -cy_c);
	const int Acx1 = min(W, int(m_size_x) - 1 - cx_c);
	const int Acy1 = min(W, int(m_size_y) - 1 - cy_c);

	// We will fill this now, so we already have it for updating the
	//  covariances next:
	CVectorDouble cross_covs_c_i(W21sqr, 0);  // Indexes are relative to the
	// (2W+1)x(2W+1) window centered
	// at "cellIdx".
	std::vector<int> window_idxs(W21sqr, -1);  // The real-map indexes for each
	// element in the window, or -1 if
	// there are out of the map (for cells
	// close to the border)

	// 1) First, the cells before "c":
	for (int Acy = Acy0; Acy <= 0; Acy++)
	{
		int limit_cx = Acy < 0 ? Acx1 : -1;

		size_t idx = cx_c + Acx0 + m_size_x * (cy_c + Acy);
		int idx_c_in_idx = -Acy * W21 - Acx0;

		int window_idx = Acx0 + W + (Acy + W) * W21;

		for (int Acx = Acx0; Acx <= limit_cx; Acx++)
		{
			ASSERT_(idx_c_in_idx > 0);
			const double cov_i_c = m_stackedCov(idx, idx_c_in_idx);
			// JGmonroy - review m_stackedCov

			m_map[idx].kf_mean() += yk * sk_1 * cov_i_c;

			// Save window indexes:
			cross_covs_c_i[window_idx] = cov_i_c;
			window_idxs[window_idx++] = idx;

			idx_c_in_idx--;
			idx++;
		}
	}

	// 2) The cell "c" itself, and the rest within the window:
	for (int Acy = 0; Acy <= Acy1; Acy++)
	{
		int start_cx = Acy > 0 ? Acx0 : 0;

		size_t idx = cx_c + start_cx + m_size_x * (cy_c + Acy);
		int idx_i_in_c;
		if (Acy > 0)
			idx_i_in_c =
				(W + 1) + (Acy - 1) * W21 + (start_cx + W);	 // Rest of rows
		else
			idx_i_in_c = 0;	 // First row.

		int window_idx = start_cx + W + (Acy + W) * W21;

		for (int Acx = start_cx; Acx <= Acx1; Acx++)
		{
			ASSERT_(idx_i_in_c >= 0 && idx_i_in_c < int(K));

			double cov_i_c = m_stackedCov(cellIdx, idx_i_in_c);
			m_map[idx].kf_mean() += yk * sk_1 * cov_i_c;

			// Save window indexes:
			cross_covs_c_i[window_idx] = cov_i_c;
			window_idxs[window_idx++] = idx;

			idx_i_in_c++;
			idx++;
		}
	}

	// ------------------------------------------------------------
	// Update cross-covariances values:
	//  Process only those cells in the vecinity of the cell (c)
	// ------------------------------------------------------------

	// First, we need the cross-covariances between each cell (i) and
	//  (c) in the window around (c). We have this in "cross_covs_c_i[k]"
	//  for k=[0,(2K+1)^2-1], and the indexes in "window_idxs[k]".
	for (size_t i = 0; i < W21sqr; i++)
	{
		const int idx_i = window_idxs[i];
		if (idx_i < 0) continue;  // out of the map

		// Extract the x,y indexes:
		int cx_i, cy_i;
		idx2cxcy(idx_i, cx_i, cy_i);

		const double cov_c_i = cross_covs_c_i[i];

		for (size_t j = i; j < W21sqr; j++)
		{
			const int idx_j = window_idxs[j];
			if (idx_j < 0) continue;  // out of the map

			int cx_j, cy_j;
			idx2cxcy(idx_j, cx_j, cy_j);

			// The cells (i,j) may be too far from each other:
			const int Ax = cx_j - cx_i;
			if (Ax > W)
				continue;  // Next cell (may be more rows after the current
			// one...)

			const int Ay = cy_j - cy_i;
			if (Ay > W) break;	// We are absolutely sure out of i's window.

			const double cov_c_j = cross_covs_c_i[j];

			int idx_j_in_i;
			if (Ay > 0) idx_j_in_i = Ax + W + (Ay - 1) * W21 + W + 1;
			else
				idx_j_in_i = Ax;  // First row:

			// Kalman update of the cross-covariances:
			double& cov_to_change = m_stackedCov(idx_i, idx_j_in_i);
			double Delta_cov = cov_c_j * cov_c_i * sk_1;
			if (i == j && cov_to_change < Delta_cov)
				THROW_EXCEPTION_FMT(
					"Negative variance value appeared! Please increase the "
					"size of the window "
					"(W).\n(m_insertOptions_common->KF_covSigma=%f)",
					m_insertOptions_common->KF_covSigma);

			cov_to_change -= Delta_cov;

		}  // end for j
	}  // end for i

	MRPT_LOG_DEBUG_FMT("Done in %.03fms\n", tictac.Tac() * 1000);

	MRPT_END
}
/*---------------------------------------------------------------
					recoverMeanAndCov
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::recoverMeanAndCov() const
{
	if (!m_hasToRecoverMeanAndCov || (m_mapType != mrKalmanApproximate)) return;
	m_hasToRecoverMeanAndCov = false;

	// Just recover the std of each cell:
	const size_t N = m_map.size();
	for (size_t i = 0; i < N; i++)
		m_map_castaway_const()[i].kf_std() = sqrt(m_stackedCov(i, 0));
}

/*---------------------------------------------------------------
					getMeanAndCov
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::getMeanAndCov(
	CVectorDouble& out_means, CMatrixDouble& out_cov) const
{
	const size_t N = BASE::m_map.size();
	out_means.resize(N);
	for (size_t i = 0; i < N; ++i)
		out_means[i] = BASE::m_map[i].kf_mean();

	recoverMeanAndCov();
	// Avoid warning on object slicing: we are OK with that.
	out_cov = static_cast<const CMatrixDouble&>(m_cov);
}

/*---------------------------------------------------------------
					getMeanAndSTD
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::getMeanAndSTD(
	CVectorDouble& out_means, CVectorDouble& out_STD) const
{
	const size_t N = BASE::m_map.size();
	out_means.resize(N);
	out_STD.resize(N);

	for (size_t i = 0; i < N; ++i)
	{
		out_means[i] = BASE::m_map[i].kf_mean();
		out_STD[i] = sqrt(m_stackedCov(i, 0));
	}
}

/*---------------------------------------------------------------
					setMeanAndSTD
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::setMeanAndSTD(
	CVectorDouble& in_means, CVectorDouble& in_std)
{
	// Assure dimmensions match
	const size_t N = BASE::m_map.size();
	ASSERT_(N == size_t(in_means.size()));
	ASSERT_(N == size_t(in_std.size()));

	m_hasToRecoverMeanAndCov = true;
	for (size_t i = 0; i < N; ++i)
	{
		m_map[i].kf_mean() = in_means[i];  // update mean values
		m_stackedCov(i, 0) = square(in_std[i]);	 // update variance values
	}
	recoverMeanAndCov();  // update STD values from cov matrix
}

CRandomFieldGridMap2D::TMapRepresentation CRandomFieldGridMap2D::getMapType()
{
	return m_mapType;
}

void CRandomFieldGridMap2D::updateMapEstimation()
{
	switch (m_mapType)
	{
		case mrKernelDM:
		case mrKernelDMV:
		case mrKalmanFilter:
		case mrKalmanApproximate:
			// Nothing to do, already done in the insert method...
			break;

		case mrGMRF_SD: this->updateMapEstimation_GMRF(); break;
		default:
			THROW_EXCEPTION(
				"insertObservation() isn't implemented for selected 'mapType'");
	};
}

void CRandomFieldGridMap2D::insertIndividualReading(
	const double sensorReading, const mrpt::math::TPoint2D& point,
	const bool update_map, const bool time_invariant,
	const double reading_stddev)
{
	switch (m_mapType)
	{
		case mrKernelDM:
			insertObservation_KernelDM_DMV(sensorReading, point, false);
			break;
		case mrKernelDMV:
			insertObservation_KernelDM_DMV(sensorReading, point, true);
			break;
		case mrKalmanFilter: insertObservation_KF(sensorReading, point); break;
		case mrKalmanApproximate:
			insertObservation_KF2(sensorReading, point);
			break;
		case mrGMRF_SD:
			insertObservation_GMRF(
				sensorReading, point, update_map, time_invariant,
				reading_stddev == .0
					? m_insertOptions_common
						  ->GMRF_lambdaObs	// default information
					: 1.0 / mrpt::square(reading_stddev));
			break;
		default:
			THROW_EXCEPTION(
				"insertObservation() isn't implemented for selected 'mapType'");
	};
}

/*---------------------------------------------------------------
					insertObservation_GMRF
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::insertObservation_GMRF(
	double normReading, const mrpt::math::TPoint2D& point,
	const bool update_map, const bool time_invariant,
	const double reading_information)
{
	try
	{
		// Get index of observed cell
		const int cellIdx = xy2idx(point.x, point.y);
		TRandomFieldCell* cell = cellByPos(point.x, point.y);
		ASSERT_(cell != nullptr);

		// Insert observation in the vector of Active Observations
		TObservationGMRF new_obs(*this);
		new_obs.node_id = cellIdx;
		new_obs.obsValue = normReading;
		new_obs.Lambda = reading_information;
		new_obs.time_invariant = time_invariant;

		m_mrf_factors_activeObs[cellIdx].push_back(new_obs);
		m_gmrf.addConstraint(
			*m_mrf_factors_activeObs[cellIdx].rbegin());  // add to graph
	}
	catch (const std::exception& e)
	{
		cerr << "Exception while Inserting new Observation: " << e.what()
			 << endl;
	}

	// Solve system and update map estimation
	if (update_map) updateMapEstimation_GMRF();
}

/*---------------------------------------------------------------
					updateMapEstimation_GMRF
  ---------------------------------------------------------------*/
void CRandomFieldGridMap2D::updateMapEstimation_GMRF()
{
	mrpt::math::CVectorDouble x_incr, x_var;
	m_gmrf.updateEstimation(
		x_incr, m_insertOptions_common->GMRF_skip_variance ? nullptr : &x_var);

	ASSERT_(size_t(m_map.size()) == size_t(x_incr.size()));
	ASSERT_(
		m_insertOptions_common->GMRF_skip_variance ||
		size_t(m_map.size()) == size_t(x_var.size()));

	// Update Mean-Variance in the base grid class
	for (size_t j = 0; j < m_map.size(); j++)
	{
		m_map[j].gmrf_std() = m_insertOptions_common->GMRF_skip_variance
			? .0
			: std::sqrt(x_var[j]);
		m_map[j].gmrf_mean() += x_incr[j];

		mrpt::saturate(
			m_map[j].gmrf_mean(), m_insertOptions_common->GMRF_saturate_min,
			m_insertOptions_common->GMRF_saturate_max);
	}

	// Update Information/Strength of Active Observations
	//---------------------------------------------------------
	if (m_insertOptions_common->GMRF_lambdaObsLoss != 0)
	{
		for (auto& obs : m_mrf_factors_activeObs)
		{
			for (auto ito = obs.begin(); ito != obs.end();)
			{
				if (!ito->time_invariant)
				{
					++ito;
					continue;
				}

				ito->Lambda -= m_insertOptions_common->GMRF_lambdaObsLoss;
				if (ito->Lambda < 0)
				{
					m_gmrf.eraseConstraint(*ito);
					ito = obs.erase(ito);
				}
				else
					++ito;
			}
		}
	}
}

bool CRandomFieldGridMap2D::exist_relation_between2cells(
	const mrpt::maps::COccupancyGridMap2D* m_Ocgridmap, size_t cxo_min,
	size_t cxo_max, size_t cyo_min, size_t cyo_max, const size_t seed_cxo,
	const size_t seed_cyo, const size_t objective_cxo,
	const size_t objective_cyo)
{
	// printf("Checking relation between cells (%i,%i) and (%i,%i)",
	// seed_cxo,seed_cyo,objective_cxo,objective_cyo);

	// Ensure delimited region is within the Occupancy map
	cxo_min = max(cxo_min, (size_t)0);
	cxo_max = min(cxo_max, (size_t)m_Ocgridmap->getSizeX() - 1);
	cyo_min = max(cyo_min, (size_t)0);
	cyo_max = min(cyo_max, (size_t)m_Ocgridmap->getSizeY() - 1);

	// printf("Under gridlimits cx=(%i,%i) and cy=(%i,%i) \n",
	// cxo_min,cxo_max,cyo_min,cyo_max);

	// Check that seed and objective are inside the delimited Occupancy gridmap
	if ((seed_cxo < cxo_min) || (seed_cxo >= cxo_max) || (seed_cyo < cyo_min) ||
		(seed_cyo >= cyo_max))
	{
		// cout << "Seed out of bounds (false)" << endl;
		return false;
	}
	if ((objective_cxo < cxo_min) || (objective_cxo >= cxo_max) ||
		(objective_cyo < cyo_min) || (objective_cyo >= cyo_max))
	{
		// cout << "Objective out of bounds (false)" << endl;
		return false;
	}

	// Check that seed and obj have similar occupancy (0,1)
	if ((m_Ocgridmap->getCell(seed_cxo, seed_cyo) < 0.5) !=
		(m_Ocgridmap->getCell(objective_cxo, objective_cyo) < 0.5))
	{
		// cout << "Seed and objective have diff occupation (false)" << endl;
		return false;
	}

	// Create Matrix for region growing (row,col)
	mrpt::math::CMatrixUInt matExp(
		cxo_max - cxo_min + 1, cyo_max - cyo_min + 1);
	// cout << "Matrix creted with dimension:" << matExp.rows() << " x "
	// << matExp.cols() << endl;
	// CMatrixF matExp(cxo_max-cxo_min+1, cyo_max-cyo_min+1);
	matExp.fill(0);

	// Add seed
	matExp(seed_cxo - cxo_min, seed_cyo - cyo_min) = 1;
	int seedsOld = 0;
	int seedsNew = 1;

	// NOT VERY EFFICIENT!!
	while (seedsOld < seedsNew)
	{
		seedsOld = seedsNew;

		for (int col = 0; col < matExp.cols(); col++)
		{
			for (int row = 0; row < matExp.rows(); row++)
			{
				// test if cell needs to be expanded
				if (matExp(row, col) == 1)
				{
					matExp(row, col) = 2;  // mark as expanded
					// check if neighbourds have similar occupancy (expand)
					for (int i = -1; i <= 1; i++)
					{
						for (int j = -1; j <= 1; j++)
						{
							// check that neighbour is inside the map
							if ((int(row) + j >= 0) &&
								(int(row) + j <= int(matExp.rows() - 1)) &&
								(int(col) + i >= 0) &&
								(int(col) + i <= int(matExp.cols()) - 1))
							{
								if (!((i == 0 && j == 0) ||
									  !(matExp(row + j, col + i) == 0)))
								{
									// check if expand
									if ((m_Ocgridmap->getCell(
											 row + cxo_min, col + cyo_min) <
										 0.5) ==
										(m_Ocgridmap->getCell(
											 row + j + cxo_min,
											 col + i + cyo_min) < 0.5))
									{
										if ((row + j + cxo_min ==
											 objective_cxo) &&
											(col + i + cyo_min ==
											 objective_cyo))
										{
											// cout << "Connection Success
											// (true)" << endl;
											return true;  // Objective connected
										}
										matExp(row + j, col + i) = 1;
										seedsNew++;
									}
								}
							}
						}
					}
				}
			}
		}
	}
	// if not connection to he objective is found, then return false
	// cout << "Connection not found (false)" << endl;
	return false;
}

float CRandomFieldGridMap2D::compute3DMatchingRatio(
	[[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
	[[maybe_unused]] const mrpt::poses::CPose3D& otherMapPose,
	[[maybe_unused]] const TMatchingRatioParams& params) const
{
	return 0;
}

// ============ TObservationGMRF ===========
double CRandomFieldGridMap2D::TObservationGMRF::evaluateResidual() const
{
	return m_parent->m_map[this->node_id].gmrf_mean() - this->obsValue;
}
double CRandomFieldGridMap2D::TObservationGMRF::getInformation() const
{
	return this->Lambda;
}
void CRandomFieldGridMap2D::TObservationGMRF::evalJacobian(double& dr_dx) const
{
	dr_dx = 1.0;
}
// ============ TPriorFactorGMRF ===========
double CRandomFieldGridMap2D::TPriorFactorGMRF::evaluateResidual() const
{
	return m_parent->m_map[this->node_id_i].gmrf_mean() -
		m_parent->m_map[this->node_id_j].gmrf_mean();
}
double CRandomFieldGridMap2D::TPriorFactorGMRF::getInformation() const
{
	return this->Lambda;
}
void CRandomFieldGridMap2D::TPriorFactorGMRF::evalJacobian(
	double& dr_dx_i, double& dr_dx_j) const
{
	dr_dx_i = +1.0;
	dr_dx_j = -1.0;
}

CRandomFieldGridMap2D::ConnectivityDescriptor::ConnectivityDescriptor() =
	default;
CRandomFieldGridMap2D::ConnectivityDescriptor::~ConnectivityDescriptor() =
	default;
