/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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 "slam-precomp.h"  // Precompiled headers
//
#include <mrpt/img/CEnhancedMetaFile.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/poses/CPoint2DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/random.h>
#include <mrpt/slam/CGridMapAligner.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/tfest/se2.h>

#include <Eigen/Dense>
#include <chrono>
#include <thread>

using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace mrpt::img;
using namespace mrpt::system;
using namespace mrpt::vision;
using namespace std;

CPosePDF::Ptr CGridMapAligner::AlignPDF(
    const mrpt::maps::CMetricMap* mm1,
    const mrpt::maps::CMetricMap* mm2,
    const CPosePDFGaussian& initialEstimationPDF,
    mrpt::optional_ref<TMetricMapAlignmentResult> info)
{
  MRPT_START

  TReturnInfo infoVal;
  CPosePDF::Ptr ret;

  switch (options.methodSelection)
  {
    case CGridMapAligner::amCorrelation:
      ret = AlignPDF_correlation(mm1, mm2, initialEstimationPDF, infoVal);
      break;

    case CGridMapAligner::amModifiedRANSAC:
    case CGridMapAligner::amRobustMatch:
      // The same function has an internal switch for the specific method:
      ret = AlignPDF_robustMatch(mm1, mm2, initialEstimationPDF, infoVal);
      break;

    default:
      THROW_EXCEPTION("Wrong value found in 'options.methodSelection'!!");
  }

  // Copy the output info if requested:
  if (info)
    if (auto* o = dynamic_cast<TReturnInfo*>(&info.value().get()); o) *o = infoVal;

  return ret;
  MRPT_END
}

namespace
{
bool myVectorOrder(const pair<size_t, float>& o1, const pair<size_t, float>& o2)
{
  return o1.second < o2.second;
}
}  // namespace

/*---------------------------------------------------------------
          AlignPDF_robustMatch
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch(
    const mrpt::maps::CMetricMap* mm1,
    const mrpt::maps::CMetricMap* mm2,
    const CPosePDFGaussian& initialEstimationPDF,
    TReturnInfo& outInfo)
{
  MRPT_START

  ASSERT_(
      options.methodSelection == CGridMapAligner::amRobustMatch ||
      options.methodSelection == CGridMapAligner::amModifiedRANSAC);

  mrpt::tfest::TMatchingPairList& correspondences =
      outInfo.correspondences;  // Use directly this placeholder to save 1
  // variable & 1 copy.
  mrpt::tfest::TMatchingPairList largestConsensusCorrs;

  CLandmarksMap::Ptr lm1(new CLandmarksMap());
  CLandmarksMap::Ptr lm2(new CLandmarksMap());

  std::vector<size_t> idxs1, idxs2;
  std::vector<size_t>::iterator it1, it2;

  // Asserts:
  // -----------------
  const CMultiMetricMap* multimap1 = nullptr;
  const CMultiMetricMap* multimap2 = nullptr;
  const COccupancyGridMap2D* m1 = nullptr;
  const COccupancyGridMap2D* m2 = nullptr;

  if (IS_CLASS(*mm1, CMultiMetricMap) && IS_CLASS(*mm2, CMultiMetricMap))
  {
    multimap1 = dynamic_cast<const CMultiMetricMap*>(mm1);
    multimap2 = dynamic_cast<const CMultiMetricMap*>(mm2);

    ASSERT_(multimap1->countMapsByClass<COccupancyGridMap2D>());
    ASSERT_(multimap2->countMapsByClass<COccupancyGridMap2D>());

    m1 = multimap1->mapByClass<COccupancyGridMap2D>().get();
    m2 = multimap2->mapByClass<COccupancyGridMap2D>().get();
  }
  else if (IS_CLASS(*mm1, COccupancyGridMap2D) && IS_CLASS(*mm2, COccupancyGridMap2D))
  {
    m1 = dynamic_cast<const COccupancyGridMap2D*>(mm1);
    m2 = dynamic_cast<const COccupancyGridMap2D*>(mm2);
  }
  else
    THROW_EXCEPTION(
        "Metric maps must be of classes COccupancyGridMap2D or "
        "CMultiMetricMap");

  ASSERT_(m1);
  ASSERT_(m2);

  ASSERTMSG_(
      m1->getResolution() == m2->getResolution(), mrpt::format(
                                                      "Different resolutions for m1, m2:\n"
                                                      "\tres(m1) = %f\n\tres(m2) = %f\n",
                                                      m1->getResolution(), m2->getResolution()));

  // The algorithm output auxiliar info:
  // -------------------------------------------------
  outInfo.goodness = 1.0f;

  outInfo.landmarks_map1 = lm1;
  outInfo.landmarks_map2 = lm2;

  // The PDF to estimate:
  // ------------------------------------------------------
  CPosePDFSOG::Ptr pdf_SOG = std::make_shared<CPosePDFSOG>();

  // Extract features from grid-maps:
  // ------------------------------------------------------
  const size_t N1 = std::max(40, mrpt::round(m1->getArea() * options.featsPerSquareMeter));
  const size_t N2 = std::max(40, mrpt::round(m2->getArea() * options.featsPerSquareMeter));

  m_grid_feat_extr.extractFeatures(
      *m1, *lm1, N1, options.feature_descriptor, options.feature_detector_options);
  m_grid_feat_extr.extractFeatures(
      *m2, *lm2, N2, options.feature_descriptor, options.feature_detector_options);

  mrpt::system::CTicTac tictac;
  tictac.Tic();

  const size_t nLM1 = lm1->size();
  const size_t nLM2 = lm2->size();

  //  At least two landmarks at each map!
  // ------------------------------------------------------
  if (nLM1 < 2 || nLM2 < 2)
  {
    outInfo.goodness = 0;
  }
  else
  {
    // Compute correlation between landmarks:
    // ---------------------------------------------
    CImage im1, im2;  // Grayscale
    unsigned int corrsCount = 0;
    std::vector<bool> hasCorr(nLM1, false);

    const double thres_max = options.threshold_max;
    const double thres_delta = options.threshold_delta;

    for (size_t idx1 = 0; idx1 < nLM1; idx1++)
    {
      vector<pair<size_t, float>> corrs_indiv;  // (index, distance);
      // Index is used to
      // recover the original
      // index after sorting.
      vector<float> corrs_indiv_only;
      corrs_indiv.reserve(nLM2);
      corrs_indiv_only.reserve(nLM2);

      for (size_t idx2 = 0; idx2 < nLM2; idx2++)
      {
        const float dist = lm1->landmarks.get(idx1)->features[0].descriptorDistanceTo(
            lm2->landmarks.get(idx2)->features[0]);

        corrs_indiv.emplace_back(idx2, dist);
        corrs_indiv_only.push_back(dist);
      }  // end for idx2

      const double corr_best = mrpt::math::minimum(corrs_indiv_only);

      // Sort the list and keep the N best features:
      std::sort(corrs_indiv.begin(), corrs_indiv.end(), myVectorOrder);

      const size_t nBestToKeep = corrs_indiv.size();

      for (size_t w = 0; w < nBestToKeep; w++)
      {
        if (corrs_indiv[w].second <= thres_max &&
            corrs_indiv[w].second <= (corr_best + thres_delta))
        {
          idxs1.push_back(idx1);
          idxs2.push_back(corrs_indiv[w].first);
          outInfo.correspondences_dists_maha.emplace_back(
              idx1, corrs_indiv[w].first, corrs_indiv[w].second);
        }
      }
    }  // end for idx1

    // Save an image with each feature and its matches:
    if (options.save_feat_coors)
    {
      mrpt::system::deleteFilesInDirectory("grid_feats");
      mrpt::system::createDirectory("grid_feats");

      std::map<size_t, std::set<size_t>> corrs_by_idx;
      for (size_t l = 0; l < idxs1.size(); l++) corrs_by_idx[idxs1[l]].insert(idxs2[l]);

      for (auto& it : corrs_by_idx)
      {
        CMatrixFloat descriptor1;
        lm1->landmarks.get(it.first)->features[0].getFirstDescriptorAsMatrix(descriptor1);

        im1.setFromMatrix(descriptor1, true /*normalized*/);

        const size_t FEAT_W = im1.getWidth();
        const size_t FEAT_H = im1.getHeight();
        const size_t nF = it.second.size();

        CImage img_compose(FEAT_W * 2 + 15, 10 + (5 + FEAT_H) * nF);
        img_compose.filledRectangle(
            0, 0, img_compose.getWidth() - 1, img_compose.getHeight() - 1, TColor::black());

        img_compose.drawImage(5, 5, im1);

        size_t j;
        std::set<size_t>::iterator it_j;
        string fil = format("grid_feats/_FEAT_MATCH_%03i", (int)it.first);

        for (j = 0, it_j = it.second.begin(); j < nF; ++j, ++it_j)
        {
          fil += format("_%u", static_cast<unsigned int>(*it_j));

          CMatrixFloat descriptor2;
          lm2->landmarks.get(*it_j)->features[0].getFirstDescriptorAsMatrix(descriptor2);
          im2.setFromMatrix(descriptor2, true);
          img_compose.drawImage(10 + FEAT_W, 5 + j * (FEAT_H + 5), im2);
        }
        fil += ".png";
        bool savedOk = img_compose.saveToFile(fil);
        ASSERT_(savedOk);
      }  // end for map
    }

    // ------------------------------------------------------------------
    // Create the list of correspondences from the lists: idxs1 & idxs2
    // ------------------------------------------------------------------
    correspondences.clear();
    for (it1 = idxs1.begin(), it2 = idxs2.begin(); it1 != idxs1.end(); ++it1, ++it2)
    {
      mrpt::tfest::TMatchingPair mp;
      mp.globalIdx = *it1;
      mp.global.x = lm1->landmarks.get(*it1)->pose_mean.x;
      mp.global.y = lm1->landmarks.get(*it1)->pose_mean.y;
      mp.global.z = lm1->landmarks.get(*it1)->pose_mean.z;

      mp.localIdx = *it2;
      mp.local.x = lm2->landmarks.get(*it2)->pose_mean.x;
      mp.local.y = lm2->landmarks.get(*it2)->pose_mean.y;
      mp.local.z = lm2->landmarks.get(*it2)->pose_mean.z;
      correspondences.push_back(mp);

      if (!hasCorr[*it1])
      {
        hasCorr[*it1] = true;
        corrsCount++;
      }
    }  // end for corres.

    outInfo.goodness = (2.0f * corrsCount) / (nLM1 + nLM2);

    // Compute the estimation using ALL the correspondences (NO ROBUST):
    // ----------------------------------------------------------------------
    mrpt::math::TPose2D noRobustEst;
    if (!mrpt::tfest::se2_l2(correspondences, noRobustEst))
    {
      // There's no way to match the maps! e.g. no correspondences
      outInfo.goodness = 0;
      pdf_SOG->clear();
      outInfo.sog1 = pdf_SOG;
      outInfo.sog2 = pdf_SOG;
      outInfo.sog3 = pdf_SOG;
    }
    else
    {
      outInfo.noRobustEstimation = mrpt::poses::CPose2D(noRobustEst);
      MRPT_LOG_INFO(mrpt::format(
          "[CGridMapAligner] Overall estimation(%u corrs, total: "
          "%u): (%.03f,%.03f,%.03fdeg)\n",
          corrsCount, (unsigned)correspondences.size(), outInfo.noRobustEstimation.x(),
          outInfo.noRobustEstimation.y(), RAD2DEG(outInfo.noRobustEstimation.phi())));

      // The list of SOG modes & their corresponding sub-sets of
      // matchings:
      using TMapMatchingsToPoseMode =
          std::map<mrpt::tfest::TMatchingPairList, CPosePDFSOG::TGaussianMode>;
      TMapMatchingsToPoseMode sog_modes;

      // ---------------------------------------------------------------
      // Now, we have to choose between the methods:
      //  - CGridMapAligner::amRobustMatch  ("normal" RANSAC)
      //  - CGridMapAligner::amModifiedRANSAC
      // ---------------------------------------------------------------
      if (options.methodSelection == CGridMapAligner::amRobustMatch)
      {
        // ====================================================
        //             METHOD: "Normal" RANSAC
        // ====================================================

        // RANSAC over the found correspondences:
        // -------------------------------------------------
        const unsigned int min_inliers = options.ransac_minSetSizeRatio * (nLM1 + nLM2) / 2;

        mrpt::tfest::TSE2RobustParams tfest_params;
        tfest_params.ransac_minSetSize = min_inliers;
        tfest_params.ransac_maxSetSize = nLM1 + nLM2;
        tfest_params.ransac_mahalanobisDistanceThreshold =
            options.ransac_mahalanobisDistanceThreshold;
        tfest_params.ransac_nSimulations = 0;  // 0=auto
        tfest_params.ransac_fuseByCorrsMatch = true;
        tfest_params.ransac_fuseMaxDiffXY = 0.01;
        tfest_params.ransac_fuseMaxDiffPhi = 0.1_deg;
        tfest_params.ransac_algorithmForLandmarks = true;
        tfest_params.probability_find_good_model = options.ransac_prob_good_inliers;
        tfest_params.verbose = false;

        mrpt::tfest::TSE2RobustResult tfest_result;
        mrpt::tfest::se2_l2_robust(
            correspondences, options.ransac_SOG_sigma_m, tfest_params, tfest_result);

        ASSERT_(pdf_SOG);
        *pdf_SOG = tfest_result.transformation;
        largestConsensusCorrs = tfest_result.largestSubSet;

        // Simplify the SOG by merging close modes:
        // -------------------------------------------------
        size_t nB = pdf_SOG->size();
        outInfo.sog1 = pdf_SOG;

        // Keep only the most-likely Gaussian mode:
        CPosePDFSOG::TGaussianMode best_mode;
        best_mode.log_w = -std::numeric_limits<double>::max();

        for (size_t n = 0; n < pdf_SOG->size(); n++)
        {
          const CPosePDFSOG::TGaussianMode& m = (*pdf_SOG)[n];

          if (m.log_w > best_mode.log_w) best_mode = m;
        }

        pdf_SOG->clear();
        pdf_SOG->push_back(best_mode);
        outInfo.sog2 = pdf_SOG;

        MRPT_LOG_INFO_STREAM(
            "[CGridMapAligner] amRobustMatch: " << nB << " SOG modes reduced to " << pdf_SOG->size()
                                                << " (most-likely) (min.inliers=" << min_inliers
                                                << ")\n");

      }  // end of amRobustMatch
      else
      {
        // ====================================================
        //             METHOD: "Modified" RANSAC
        // ====================================================
        mrpt::tfest::TMatchingPairList all_corrs = correspondences;

        const size_t nCorrs = all_corrs.size();
        ASSERT_(nCorrs >= 2);

        pdf_SOG->clear();  // Start with 0 Gaussian modes

        // Build a points-map for exploiting its KD-tree:
        // ----------------------------------------------------
        CSimplePointsMap lm1_pnts, lm2_pnts;
        lm1_pnts.reserve(nLM1);
        for (size_t i = 0; i < nLM1; i++) lm1_pnts.insertPoint(lm1->landmarks.get(i)->pose_mean);
        lm2_pnts.reserve(nLM2);
        for (size_t i = 0; i < nLM2; i++) lm2_pnts.insertPoint(lm2->landmarks.get(i)->pose_mean);

        // RANSAC loop
        // ---------------------
        const size_t minInliersTOaccept =
            round(options.ransac_minSetSizeRatio * 0.5 * (nLM1 + nLM2));
        // Set an initial # of iterations:
        const unsigned int ransac_min_nSimulations = 2 * (nLM1 + nLM2);  // 1000;
        unsigned int ransac_nSimulations =
            10;  // It doesn't matter actually, since will be changed in
        // the first loop
        const double probability_find_good_model = 0.9999;

        const double chi2_thres_dim1 = mrpt::math::chi2inv(options.ransac_chi2_quantile, 1);
        const double chi2_thres_dim2 = mrpt::math::chi2inv(options.ransac_chi2_quantile, 2);

        // Generic 2x2 covariance matrix for all features in their local
        // coords:
        CMatrixDouble22 COV_pnt;
        COV_pnt(0, 0) = COV_pnt(1, 1) = square(options.ransac_SOG_sigma_m);

        // The absolute number of trials.
        // in practice it's only important for a reduced number of
        // correspondences, to avoid a dead-lock:
        //  It's the binomial coefficient:
        //   / n \      n!          n * (n-1)
        //   |   | = ----------- = -----------
        //   \ 2 /    2! (n-2)!         2
        //
        const unsigned int max_trials =
            (nCorrs * (nCorrs - 1) / 2) * 5;  // "*5" is just for safety...

        unsigned int iter = 0;  // Valid iterations (those passing the
        // first mahalanobis test)
        unsigned int trials = 0;  // counter of all iterations,
        // including "iter" + failing ones.
        while (iter < ransac_nSimulations &&
               trials < max_trials)  // ransac_nSimulations can be dynamic
        {
          trials++;

          mrpt::tfest::TMatchingPairList tentativeSubSet;

          // Pick 2 random correspondences:
          uint32_t idx1, idx2;
          idx1 = getRandomGenerator().drawUniform32bit() % nCorrs;
          do
          {
            idx2 = getRandomGenerator().drawUniform32bit() % nCorrs;
          } while (idx1 == idx2);  // Avoid a degenerated case!

          // Uniqueness of features:
          if (all_corrs[idx1].globalIdx == all_corrs[idx2].globalIdx ||
              all_corrs[idx1].globalIdx == all_corrs[idx2].localIdx)
            continue;
          if (all_corrs[idx1].localIdx == all_corrs[idx2].globalIdx ||
              all_corrs[idx1].localIdx == all_corrs[idx2].localIdx)
            continue;

          // Check the feasibility of this pair "idx1"-"idx2":
          //  The distance between the pair of points in MAP1 must be
          //  very close
          //   to that of their correspondences in MAP2:
          const double corrs_dist1 = mrpt::math::distanceBetweenPoints(
              all_corrs[idx1].global.x, all_corrs[idx1].global.y, all_corrs[idx1].global.z,
              all_corrs[idx2].global.x, all_corrs[idx2].global.y, all_corrs[idx2].global.z);

          const double corrs_dist2 = mrpt::math::distanceBetweenPoints(
              all_corrs[idx1].local.x, all_corrs[idx1].local.y, all_corrs[idx1].local.z,
              all_corrs[idx2].local.x, all_corrs[idx2].local.y, all_corrs[idx2].local.z);

          // Is is a consistent possibility?
          //  We use a chi2 test (see paper for the derivation)
          const double corrs_dist_chi2 = square(square(corrs_dist1) - square(corrs_dist2)) /
                                         (8.0 * square(options.ransac_SOG_sigma_m) *
                                          (square(corrs_dist1) + square(corrs_dist2)));

          if (corrs_dist_chi2 > chi2_thres_dim1) continue;  // Nope

          iter++;  // Do not count iterations if they fail the test
          // above.

          // before proceeding with this hypothesis, is it an old one?
          bool is_new_hyp = true;
          for (auto& sog_mode : sog_modes)
          {
            if (sog_mode.first.contains(all_corrs[idx1]) &&
                sog_mode.first.contains(all_corrs[idx2]))
            {
              // Increment weight:
              sog_mode.second.log_w = std::log(std::exp(sog_mode.second.log_w) + 1.0);
              is_new_hyp = false;
              break;
            }
          }
          if (!is_new_hyp) continue;

          // Ok, it's a new hypothesis:
          tentativeSubSet.push_back(all_corrs[idx1]);
          tentativeSubSet.push_back(all_corrs[idx2]);

          // Maintain a list of already used landmarks IDs in both
          // maps to avoid repetitions:
          std::vector<bool> used_landmarks1(nLM1, false);
          std::vector<bool> used_landmarks2(nLM2, false);

          used_landmarks1[all_corrs[idx1].globalIdx] = true;
          used_landmarks1[all_corrs[idx2].globalIdx] = true;
          used_landmarks2[all_corrs[idx1].localIdx] = true;
          used_landmarks2[all_corrs[idx2].localIdx] = true;

          // Build the transformation for these temptative
          // correspondences:
          bool keep_incorporating = true;
          CPosePDFGaussian temptPose;
          do  // Incremently incorporate inliers:
          {
            if (!mrpt::tfest::se2_l2(tentativeSubSet, temptPose)) continue;  // Invalid matching...

            // The computed cov is "normalized", i.e. must be
            // multiplied by std^2_xy
            temptPose.cov *= square(options.ransac_SOG_sigma_m);

            // Find the landmark in MAP2 with the best (maximum)
            // product-integral:
            //   (i^* , j^*) = arg max_(i,j) \int p_i()p_j()
            //----------------------------------------------------------------------
            const double ccos = cos(temptPose.mean.phi());
            const double ssin = sin(temptPose.mean.phi());

            CMatrixDouble22 Hc;  // Jacobian wrt point_j
            Hc(1, 1) = ccos;
            Hc(0, 0) = ccos;
            Hc(1, 0) = ssin;
            Hc(0, 1) = -ssin;

            CMatrixFixed<double, 2, 3> Hq;  // Jacobian wrt transformation q
            Hq(0, 0) = 1;
            Hq(1, 1) = 1;

            TPoint2D p2_j_local;
            vector<float> matches_dist;
            std::vector<size_t> matches_idx;

            CPoint2DPDFGaussian pdf_M2_j;
            CPoint2DPDFGaussian pdf_M1_i;

// Use integral-of-product vs. mahalanobis distances to match:
#define GRIDMAP_USE_PROD_INTEGRAL

#ifdef GRIDMAP_USE_PROD_INTEGRAL
            double best_pair_value = 0;
#else
            double best_pair_value = std::numeric_limits<double>::max();
#endif
            double best_pair_d2 = std::numeric_limits<double>::max();
            pair<size_t, size_t> best_pair_ij;

            //#define SHOW_CORRS

#ifdef SHOW_CORRS
            CDisplayWindowPlots win("Matches");
#endif
            for (size_t j = 0; j < nLM2; j++)
            {
              if (used_landmarks2[j]) continue;

              lm2_pnts.getPoint(j, p2_j_local);  // In local coords.
              pdf_M2_j.mean = mrpt::poses::CPoint2D(
                  temptPose.mean + p2_j_local);  // In (temptative) global coords:
              pdf_M2_j.cov(0, 0) = pdf_M2_j.cov(1, 1) = square(options.ransac_SOG_sigma_m);

#ifdef SHOW_CORRS
              win.plotEllipse(
                  pdf_M2_j.mean.x(), pdf_M2_j.mean.y(), pdf_M2_j.cov, 2, "b-",
                  format("M2_%u", (unsigned)j), true);
#endif

              static const unsigned int N_KDTREE_SEARCHED = 3;

              // Look for a few close features which may be
              // potential matches:
              lm1_pnts.kdTreeNClosestPoint2DIdx(
                  pdf_M2_j.mean.x(), pdf_M2_j.mean.y(), N_KDTREE_SEARCHED, matches_idx,
                  matches_dist);

              // And for each one, compute the product-integral:
              for (unsigned long u : matches_idx)
              {
                if (used_landmarks1[u]) continue;

                // Jacobian wrt transformation q
                Hq(0, 2) = -p2_j_local.x * ssin - p2_j_local.y * ccos;
                Hq(1, 2) = p2_j_local.x * ccos - p2_j_local.y * ssin;

                // COV_j = Hq \Sigma_q Hq^t + Hc Cj Hc^t
                pdf_M1_i.cov = mrpt::math::multiply_HCHt(Hc, COV_pnt);
                pdf_M1_i.cov += mrpt::math::multiply_HCHt(Hq, temptPose.cov);

                float px, py;
                lm1_pnts.getPoint(u, px, py);
                pdf_M1_i.mean.x(px);
                pdf_M1_i.mean.y(py);

#ifdef SHOW_CORRS
                win.plotEllipse(
                    pdf_M1_i.mean.x(), pdf_M1_i.mean.y(), pdf_M1_i.cov, 2, "r-",
                    format("M1_%u", (unsigned)matches_idx[u]), true);
#endif

// And now compute the product integral:
#ifdef GRIDMAP_USE_PROD_INTEGRAL
                const double prod_ij = pdf_M1_i.productIntegralWith(pdf_M2_j);

                if (prod_ij > best_pair_value)
#else
                const double prod_ij = pdf_M1_i.mean.distanceTo(pdf_M2_j.mean);
                if (prod_ij < best_pair_value)
#endif
                {
                  // const double prodint_ij =
                  // pdf_M1_i.productIntegralWith2D(pdf_M2_j);

                  best_pair_value = prod_ij;
                  best_pair_ij.first = u;
                  best_pair_ij.second = j;

                  best_pair_d2 = square(pdf_M1_i.mahalanobisDistanceTo(pdf_M2_j));
                }
              }  // end for u (closest matches of LM2 in MAP 1)

#ifdef SHOW_CORRS
              win.axis_fit(true);
              win.waitForKey();
              win.clear();
#endif

            }  // end for each LM2

            // Stop when the best choice has a bad mahal. dist.
            keep_incorporating = false;

            // For the best (i,j), gate by the mahalanobis distance:
            if (best_pair_d2 < chi2_thres_dim2)
            {
              // AND, also, check if the descriptors have some
              // resemblance!!
              // ----------------------------------------------------------------
              // double feat_dist =
              // lm1->landmarks.get(best_pair_ij.first)->features[0]->descriptorDistanceTo(*lm1->landmarks.get(best_pair_ij.second)->features[0]);
              // if (feat_dist< options.threshold_max)
              {
                float p1_i_localx, p1_i_localy;
                float p2_j_localx, p2_j_localy;
                lm1_pnts.getPoint(best_pair_ij.first, p1_i_localx, p1_i_localy);
                lm2_pnts.getPoint(
                    best_pair_ij.second, p2_j_localx,
                    p2_j_localy);  // In local coords.

                used_landmarks1[best_pair_ij.first] = true;
                used_landmarks2[best_pair_ij.second] = true;

                tentativeSubSet.push_back(mrpt::tfest::TMatchingPair(
                    best_pair_ij.first, best_pair_ij.second, p1_i_localx, p1_i_localy, 0,  // MAP1
                    p2_j_localx, p2_j_localy, 0                                            // MAP2
                    ));

                keep_incorporating = true;
              }
            }

          } while (keep_incorporating);

          // Consider this pairing?
          const size_t ninliers = tentativeSubSet.size();
          if (ninliers > minInliersTOaccept)
          {
            CPosePDFSOG::TGaussianMode newGauss;
            newGauss.log_w = 0;  // log(1);  //
            // std::log(static_cast<double>(nCoincidences));
            newGauss.mean = temptPose.mean;
            newGauss.cov = temptPose.cov;

            sog_modes[tentativeSubSet] = newGauss;

            // cout << "ITER: " << iter << " #inliers: " << ninliers
            // << " q: " << temptPose.mean << endl;
          }

          // Keep the largest consensus & dynamic # of steps:
          if (largestConsensusCorrs.size() < ninliers)
          {
            largestConsensusCorrs = tentativeSubSet;

            // Update estimate of N, the number of trials to ensure
            // we pick,
            // with probability p, a data set with no outliers.
            const double fracinliers = ninliers / static_cast<double>(std::min(nLM1, nLM2));
            double pNoOutliers = 1 - pow(fracinliers,
                                         static_cast<double>(2.0));  // minimumSizeSamplesToFit

            pNoOutliers = std::max(
                std::numeric_limits<double>::epsilon(),
                pNoOutliers);  // Avoid division by -Inf
            pNoOutliers = std::min(
                1.0 - std::numeric_limits<double>::epsilon(),
                pNoOutliers);  // Avoid division by 0.
            // Number of
            ransac_nSimulations = log(1 - probability_find_good_model) / log(pNoOutliers);

            if (ransac_nSimulations < ransac_min_nSimulations)
              ransac_nSimulations = ransac_min_nSimulations;

            // if (verbose)
            //	cout << "[Align] Iter #" << iter << " Est. #iters: "
            //<< ransac_nSimulations << "  pNoOutliers=" <<
            // pNoOutliers << " #inliers: " << ninliers << endl;
          }

        }  // end of RANSAC loop

        // Move SOG modes into pdf_SOG:
        pdf_SOG->clear();
        for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
        {
          MRPT_LOG_INFO_STREAM("SOG mode: " << s->second.mean << " inliers: " << s->first.size());
          pdf_SOG->push_back(s->second);
        }

        // Normalize:
        if (pdf_SOG->size() > 0) pdf_SOG->normalizeWeights();

        // Simplify the SOG by merging close modes:
        // -------------------------------------------------
        size_t nB = pdf_SOG->size();
        outInfo.sog1 = pdf_SOG;

        CTicTac merge_clock;
        pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
        const double merge_clock_tim = merge_clock.Tac();

        outInfo.sog2 = pdf_SOG;
        size_t nA = pdf_SOG->size();
        MRPT_LOG_INFO(mrpt::format(
            "[CGridMapAligner] amModifiedRANSAC: %u SOG modes "
            "merged to %u in %.03fsec\n",
            (unsigned int)nB, (unsigned int)nA, merge_clock_tim));

      }  // end of amModifiedRANSAC

      // Save best corrs:
      if (options.debug_save_map_pairs)
      {
        static unsigned int NN = 0;
        static const COccupancyGridMap2D* lastM1 = nullptr;
        if (lastM1 != m1)
        {
          lastM1 = m1;
          NN = 0;
        }
        MRPT_LOG_INFO_FMT(
            "Largest consensus: %u", static_cast<unsigned>(largestConsensusCorrs.size()));
        CEnhancedMetaFile::LINUX_IMG_WIDTH(m1->getSizeX() + m2->getSizeX() + 50);
        CEnhancedMetaFile::LINUX_IMG_HEIGHT(max(m1->getSizeY(), m2->getSizeY()) + 50);

        for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
        {
          COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
              format("__debug_corrsGrid_%05u.png", NN), m1, m2, s->first);
          ++NN;
        }
      }

      // --------------------------------------------------------------------
      // At this point:
      //   - "pdf_SOG": has the resulting PDF with the SOG (from whatever
      //   method)
      //   - "largestConsensusCorrs": The 'best' set of correspondences
      //
      //  Now: If we had a multi-metric map, use the points map to improve
      //        the estimation with ICP.
      // --------------------------------------------------------------------
      if (multimap1 && multimap2 && multimap1->countMapsByClass<CSimplePointsMap>() != 0 &&
          multimap2->countMapsByClass<CSimplePointsMap>() != 0)
      {
        auto pnts1 = multimap1->mapByClass<CSimplePointsMap>();
        auto pnts2 = multimap2->mapByClass<CSimplePointsMap>();

        CICP icp;
        CICP::TReturnInfo icpInfo;

        icp.options.maxIterations = 20;
        icp.options.smallestThresholdDist = 0.05f;
        icp.options.thresholdDist = 0.75f;

        // Invoke ICP once for each mode in the SOG:
        size_t cnt = 0;
        outInfo.icp_goodness_all_sog_modes.clear();
        for (auto i = pdf_SOG->begin(); i != pdf_SOG->end(); ++cnt)
        {
          CPosePDF::Ptr icp_est = icp.Align(pnts1.get(), pnts2.get(), (i)->mean, icpInfo);

          //(i)->cov(0,0) += square( 0.05 );
          //(i)->cov(1,1) += square( 0.05 );
          //(i)->cov(2,2) += square( 0.05_deg );

          CPosePDFGaussian i_gauss(i->mean, i->cov);
          CPosePDFGaussian icp_gauss(icp_est->getMeanVal(), i->cov);

          const double icp_maha_dist = i_gauss.mahalanobisDistanceTo(icp_gauss);

          MRPT_LOG_INFO_STREAM(
              "ICP " << cnt << " log-w: " << i->log_w << " Goodness: " << 100 * icpInfo.goodness
                     << "  D_M= " << icp_maha_dist);
          MRPT_LOG_INFO_STREAM("final pos: " << icp_est->getMeanVal());
          MRPT_LOG_INFO_STREAM("  org pos: " << i->mean);

          outInfo.icp_goodness_all_sog_modes.push_back(icpInfo.goodness);

          // Discard?
          if (icpInfo.goodness > options.min_ICP_goodness &&
              icp_maha_dist < options.max_ICP_mahadist)
          {
            icp_est->getMean((i)->mean);
            ++i;
          }
          else
          {
            // Delete:
            i = pdf_SOG->erase(i);
          }

        }  // end for i

        // Merge:
        outInfo.sog3 = pdf_SOG;

        pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
        MRPT_LOG_DEBUG_STREAM(
            "[CGridMapAligner] " << pdf_SOG->size() << " SOG modes merged after ICP.");

      }  // end multimapX

    }  // end of, yes, we have enough correspondences

  }  // end of: yes, there are landmarks in the grid maps!

  outInfo.executionTime = tictac.Tac();

  return pdf_SOG;

  MRPT_END
}

/*---------------------------------------------------------------
          AlignPDF_correlation
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_correlation(
    const mrpt::maps::CMetricMap* mm1,
    const mrpt::maps::CMetricMap* mm2,
    [[maybe_unused]] const CPosePDFGaussian& initialEstimationPDF,
    TReturnInfo& outInfo)
{
  MRPT_START

  //#define	CORRELATION_SHOW_DEBUG

  mrpt::system::CTicTac tictac;
  tictac.Tic();

  // Asserts:
  // -----------------
  ASSERT_(mm1->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
  ASSERT_(mm2->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
  const auto* m1 = dynamic_cast<const COccupancyGridMap2D*>(mm1);
  const auto* m2 = dynamic_cast<const COccupancyGridMap2D*>(mm2);

  ASSERT_EQUAL_(m1->getResolution(), m2->getResolution());

  // The PDF to estimate:
  // ------------------------------------------------------
  CPosePDFGaussian::Ptr PDF = std::make_shared<CPosePDFGaussian>();

  // Determine the extension to compute the correlation into:
  // ----------------------------------------------------------
  float phiResolution = DEG2RAD(0.2f);
  float phiMin = -M_PIf + 0.5f * phiResolution;
  float phiMax = M_PIf;

  // Compute the difference between maps for each (u,v) pair!
  // --------------------------------------------------------------
  float phi;
  float pivotPt_x = 0.5f * (m1->getXMax() + m1->getXMin());
  float pivotPt_y = 0.5f * (m1->getYMax() + m1->getYMin());
  COccupancyGridMap2D map2_mod;
  CImage map1_img, map2_img;
  float currentMaxCorr = -1e6f;

  m1->getAsImage(map1_img);

  map2_mod.setSize(m1->getXMin(), m1->getXMax(), m1->getYMin(), m1->getYMax(), m1->getResolution());
  size_t map2_lx = map2_mod.getSizeX();
  size_t map2_ly = map2_mod.getSizeY();
  CMatrixF outCrossCorr, bestCrossCorr;
  float map2width_2 = 0.5f * (map2_mod.getXMax() - map2_mod.getXMin());

#ifdef CORRELATION_SHOW_DEBUG
  CDisplayWindow* win = new CDisplayWindow("corr");
  CDisplayWindow* win2 = new CDisplayWindow("map2_mod");
#endif

  // --------------------------------------------------------
  // Use FFT-based correlation for each orientation:
  // --------------------------------------------------------
  for (phi = phiMin; phi < phiMax; phi += phiResolution)
  {
    // Create the displaced map2 grid, for the size of map1:
    // --------------------------------------------------------
    CPose2D v2(
        pivotPt_x - cos(phi) * map2width_2, pivotPt_y - sin(phi) * map2width_2,
        phi);  // Rotation point: the centre of img1:
    CPoint2D v1, v3;
    v2 = CPose2D(0, 0, 0) - v2;  // Inverse

    for (size_t cy2 = 0; cy2 < map2_ly; cy2++)
    {
      COccupancyGridMap2D::cellType* row = map2_mod.getRow(cy2);
      for (size_t cx2 = 0; cx2 < map2_lx; cx2++)
      {
        v3 = v2 + CPoint2D(map2_mod.idx2x(cx2), map2_mod.idx2y(cy2));
        *row++ = m2->p2l(m2->getPos(v3.x(), v3.y()));
      }
    }

    map2_mod.getAsImage(map2_img);
    //		map2_img.saveToBMP("__debug_map2mod.bmp");

    // Compute the correlation:
    map1_img.cross_correlation_FFT(
        map2_img, outCrossCorr, -1, -1, -1, -1,
        127,  // Bias to be subtracted
        127   // Bias to be subtracted
    );

    float corrPeak = outCrossCorr.maxCoeff();
    MRPT_LOG_INFO_FMT("phi = %fdeg \tmax corr=%f", RAD2DEG(phi), corrPeak);

    // Keep the maximum:
    if (corrPeak > currentMaxCorr)
    {
      currentMaxCorr = corrPeak;
      bestCrossCorr = outCrossCorr;
      PDF->mean.phi(phi);
    }

#ifdef CORRELATION_SHOW_DEBUG
    outCrossCorr.adjustRange(0, 1);
    CImage aux(outCrossCorr, true);
    win->showImage(aux);
    win2->showImage(map2_img);
    std::this_thread::sleep_for(5ms);
#endif

  }  // end for phi

  outInfo.executionTime = tictac.Tac();

#ifdef CORRELATION_SHOW_DEBUG
  CImage aux;
  aux.setFromMatrix(bestCrossCorr, false /* do normalization [0,1]*/);
  aux.saveToFile("_debug_best_corr.png");
  delete win;
  delete win2;
#endif

  // Transform the best corr matrix peak into coordinates:
  std::size_t uMax, vMax;
  currentMaxCorr = bestCrossCorr.maxCoeff(uMax, vMax);

  PDF->mean.x(m1->idx2x(uMax));
  PDF->mean.y(m1->idx2y(vMax));

  return PDF;

  // Done!
  MRPT_END
}

/*---------------------------------------------------------------
          TConfigParams
  ---------------------------------------------------------------*/
CGridMapAligner::TConfigParams::TConfigParams() : feature_detector_options() {}
/*---------------------------------------------------------------
          dumpToTextStream
  ---------------------------------------------------------------*/
void CGridMapAligner::TConfigParams::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [CGridMapAligner::TConfigParams] ------------ \n\n";

  LOADABLEOPTS_DUMP_VAR(methodSelection, int)
  LOADABLEOPTS_DUMP_VAR(featsPerSquareMeter, float)
  LOADABLEOPTS_DUMP_VAR(threshold_max, float)
  LOADABLEOPTS_DUMP_VAR(threshold_delta, float)
  LOADABLEOPTS_DUMP_VAR(min_ICP_goodness, float)
  LOADABLEOPTS_DUMP_VAR(max_ICP_mahadist, double)
  LOADABLEOPTS_DUMP_VAR(maxKLd_for_merge, float)
  LOADABLEOPTS_DUMP_VAR(ransac_minSetSizeRatio, float)
  LOADABLEOPTS_DUMP_VAR(ransac_mahalanobisDistanceThreshold, float)
  LOADABLEOPTS_DUMP_VAR(ransac_chi2_quantile, double)
  LOADABLEOPTS_DUMP_VAR(ransac_prob_good_inliers, double)
  LOADABLEOPTS_DUMP_VAR(ransac_SOG_sigma_m, float)
  LOADABLEOPTS_DUMP_VAR(save_feat_coors, bool)
  LOADABLEOPTS_DUMP_VAR(debug_save_map_pairs, bool)

  LOADABLEOPTS_DUMP_VAR(feature_descriptor, int)

  feature_detector_options.dumpToTextStream(out);

  out << "\n";
}

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

  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(featsPerSquareMeter, float, iniFile, section)
  MRPT_LOAD_CONFIG_VAR(ransac_SOG_sigma_m, float, iniFile, section)

  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_max, float, iniFile, section)
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_delta, float, iniFile, section)

  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(min_ICP_goodness, float, iniFile, section)
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(max_ICP_mahadist, double, iniFile, section)

  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(maxKLd_for_merge, float, iniFile, section)
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(ransac_minSetSizeRatio, float, iniFile, section)
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(ransac_mahalanobisDistanceThreshold, float, iniFile, section)
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(ransac_chi2_quantile, double, iniFile, section)
  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(ransac_prob_good_inliers, double, iniFile, section)

  MRPT_LOAD_CONFIG_VAR(save_feat_coors, bool, iniFile, section)
  MRPT_LOAD_CONFIG_VAR(debug_save_map_pairs, bool, iniFile, section)

  feature_descriptor = iniFile.read_enum(section, "feature_descriptor", feature_descriptor, true);
  feature_detector_options.loadFromConfigFile(iniFile, section);
}

CPose3DPDF::Ptr CGridMapAligner::Align3DPDF(
    [[maybe_unused]] const mrpt::maps::CMetricMap* m1,
    [[maybe_unused]] const mrpt::maps::CMetricMap* m2,
    [[maybe_unused]] const CPose3DPDFGaussian& initialEstimationPDF,
    [[maybe_unused]] mrpt::optional_ref<TMetricMapAlignmentResult> outInfo)
{
  THROW_EXCEPTION("Align3D method not applicable to CGridMapAligner");
}
