CRandomFieldGridMap2D.cpp

Go to the documentation of this file.

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

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

#include <mrpt/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:
    // -------------------------------------------------
    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
}

/*---------------------------------------------------------------
                        getAs3DObject
---------------------------------------------------------------*/
void CRandomFieldGridMap2D::getAs3DObject(
    mrpt::opengl::CSetOfObjects::Ptr& outObj) const
{
    if (!genericMapParams.enableSaveAs3DObject) return;

    // Returns only the mean map
    mrpt::opengl::CSetOfObjects::Ptr other_obj =
        mrpt::opengl::CSetOfObjects::Create();
    getAs3DObject(outObj, other_obj);
}

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

    recoverMeanAndCov();  // Only works for KF2 method

    opengl::CSetOfTriangles::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:
            // ----------------------------------
            opengl::CSetOfTriangles::Ptr obj_m =
                std::make_shared<opengl::CSetOfTriangles>();
            obj_m->enableTransparency(true);
            opengl::CSetOfTriangles::Ptr obj_v =
                std::make_shared<opengl::CSetOfTriangles>();
            obj_v->enableTransparency(true);

            //  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
            // ---------------------------------------
            triag.a[0] = triag.a[1] = triag.a[2] =
                0.75f;  // alpha (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(
                        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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);
                    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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
                    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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);
                    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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
                    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:
            // ----------------------------------
            opengl::CSetOfTriangles::Ptr obj_m =
                std::make_shared<opengl::CSetOfTriangles>();
            obj_m->enableTransparency(true);
            opengl::CSetOfTriangles::Ptr obj_v =
                std::make_shared<opengl::CSetOfTriangles>();
            obj_v->enableTransparency(true);

            //  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] =
                0.75f;  // alpha (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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);

                    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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
                    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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);

                    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;
                    jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
                    jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
                    jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
                    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();
    out_cov = 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(
    const mrpt::maps::CMetricMap* otherMap,
    const mrpt::poses::CPose3D& otherMapPose,
    const TMatchingRatioParams& params) const
{
    MRPT_UNUSED_PARAM(otherMap);
    MRPT_UNUSED_PARAM(otherMapPose);
    MRPT_UNUSED_PARAM(params);
    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;