CGridMapAligner.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include "slam-precomp.h"  // Precompiled headers
 
#include <mrpt/slam/CGridMapAligner.h>
#include <mrpt/random.h>
#include <mrpt/poses/CPoint2DPDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/geometry.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/img/CEnhancedMetaFile.h>
 
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/tfest/se2.h>
 
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, float* runningTime,
        void* info)
{
        MRPT_START
 
        switch (options.methodSelection)
        {
                case CGridMapAligner::amCorrelation:
                        return AlignPDF_correlation(
                                mm1, mm2, initialEstimationPDF, runningTime, info);
 
                case CGridMapAligner::amModifiedRANSAC:
                case CGridMapAligner::amRobustMatch:
                        // The same function has an internal switch for the specific method:
                        return AlignPDF_robustMatch(
                                mm1, mm2, initialEstimationPDF, runningTime, info);
 
                default:
                        THROW_EXCEPTION("Wrong value found in 'options.methodSelection'!!");
        }
 
        MRPT_END
}
 
bool myVectorOrder(const pair<size_t, float>& o1, const pair<size_t, float>& o2)
{
        return o1.second < o2.second;
}
 
/*---------------------------------------------------------------
                                        AlignPDF_robustMatch
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch(
        const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
        const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
        void* info)
{
        MRPT_START
 
        ASSERT_(
                options.methodSelection == CGridMapAligner::amRobustMatch ||
                options.methodSelection == CGridMapAligner::amModifiedRANSAC);
 
        TReturnInfo outInfo;
        mrpt::tfest::TMatchingPairList& correspondences =
                outInfo.correspondences;  // Use directly this placeholder to save 1
        // variable & 1 copy.
        mrpt::tfest::TMatchingPairList largestConsensusCorrs;
 
        CTicTac* tictac = nullptr;
 
        CPose2D grossEst = initialEstimationPDF.mean;
 
        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 = static_cast<const CMultiMetricMap*>(mm1);
                multimap2 = static_cast<const CMultiMetricMap*>(mm2);
 
                ASSERT_(multimap1->m_gridMaps.size() && multimap1->m_gridMaps[0]);
                ASSERT_(multimap2->m_gridMaps.size() && multimap2->m_gridMaps[0]);
 
                m1 = multimap1->m_gridMaps[0].get();
                m2 = multimap2->m_gridMaps[0].get();
        }
        else if (
                IS_CLASS(mm1, COccupancyGridMap2D) &&
                IS_CLASS(mm2, COccupancyGridMap2D))
        {
                m1 = static_cast<const COccupancyGridMap2D*>(mm1);
                m2 = static_cast<const COccupancyGridMap2D*>(mm2);
        }
        else
                THROW_EXCEPTION(
                        "Metric maps must be of classes COccupancyGridMap2D or "
                        "CMultiMetricMap")
 
        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 = mrpt::make_aligned_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);
 
        if (runningTime)
        {
                tictac = new CTicTac();
                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
        {
                //#define METHOD_FFT
                //#define DEBUG_SHOW_CORRELATIONS
 
                // Compute correlation between landmarks:
                // ---------------------------------------------
                CMatrixFloat CORR(lm1->size(), lm2->size()), auxCorr;
                CImage im1(1, 1, 1), im2(1, 1, 1);  // Grayscale
                CVectorFloat corr;
                unsigned int corrsCount = 0;
                std::vector<bool> hasCorr(nLM1, false);
 
                const double thres_max = options.threshold_max;
                const double thres_delta = options.threshold_delta;
 
                // CDisplayWindowPlots::Ptr     auxWin;
                if (options.debug_show_corrs)
                {
                        // auxWin = CDisplayWindowPlots::Ptr( new
                        // CDisplayWindowPlots("Individual corr.") );
                        std::cerr << "Warning: options.debug_show_corrs has no effect "
                                                 "since MRPT 0.9.1\n";
                }
 
                for (size_t idx1 = 0; idx1 < nLM1; idx1++)
                {
                        // CVectorFloat         corrs_indiv;
                        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++)
                        {
                                float minDist;
                                minDist =
                                        lm1->landmarks.get(idx1)->features[0]->descriptorDistanceTo(
                                                *lm2->landmarks.get(idx2)->features[0]);
 
                                corrs_indiv.push_back(std::make_pair(idx2, minDist));
                                corrs_indiv_only.push_back(minDist);
                        }  // end for idx2
 
                        // double corr_mean,corr_std;
                        // mrpt::math::meanAndStd(corrs_indiv_only,corr_mean,corr_std);
                        const double corr_best = mrpt::math::minimum(corrs_indiv_only);
                        // cout << "M: " << corr_mean << " std: " << corr_std << " best: "
                        // << corr_best << endl;
 
                        // Sort the list and keep the N best features:
                        std::sort(corrs_indiv.begin(), corrs_indiv.end(), myVectorOrder);
 
                        // const size_t nBestToKeep = std::min( (size_t)30,
                        // corrs_indiv.size() );
                        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.push_back(
                                                TReturnInfo::TPairPlusDistance(
                                                        idx1, corrs_indiv[w].first, corrs_indiv[w].second));
                                }
                        }
 
#if 0
                        if (options.debug_show_corrs)
                        {
                                auxWin->setWindowTitle(format("Corr: %i - rest", int(idx1)));
 
                                auxWin->plot(corrs_indiv_only,".3","the_corr");
 
                                CVectorFloat xs(2), ys(2);
                                xs[0]=0; xs[1]=corrs_indiv_only.size()+1;
 
                                ys[0]=ys[1] = corr_best+thres_delta;
                                auxWin->plot(xs,ys,"g","the_thres");
 
                                if (idx1==0) auxWin->axis_fit();
                                auxWin->waitForKey();
                        }
#endif
 
                }  // 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 (std::map<size_t, std::set<size_t>>::iterator it =
                                         corrs_by_idx.begin();
                                 it != corrs_by_idx.end(); ++it)
                        {
                                CMatrixFloat descriptor1;
                                lm1->landmarks.get(it->first)
                                        ->features[0]
                                        ->getFirstDescriptorAsMatrix(descriptor1);
 
                                im1 = CImage(descriptor1, true);
 
                                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 = CImage(descriptor2, true);
                                        img_compose.drawImage(
                                                10 + FEAT_W, 5 + j * (FEAT_H + 5), im2);
                                }
                                fil += ".png";
                                img_compose.saveToFile(fil);
                        }  // 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.this_idx = *it1;
                        mp.this_x = lm1->landmarks.get(*it1)->pose_mean.x;
                        mp.this_y = lm1->landmarks.get(*it1)->pose_mean.y;
                        mp.this_z = lm1->landmarks.get(*it1)->pose_mean.z;
 
                        mp.other_idx = *it2;
                        mp.other_x = lm2->landmarks.get(*it2)->pose_mean.x;
                        mp.other_y = lm2->landmarks.get(*it2)->pose_mean.y;
                        mp.other_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 =
                                mrpt::aligned_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 = DEG2RAD(0.1);
                                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.get_unsafe(0, 0) = COV_pnt.get_unsafe(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].this_idx == all_corrs[idx2].this_idx ||
                                                all_corrs[idx1].this_idx == all_corrs[idx2].other_idx)
                                                continue;
                                        if (all_corrs[idx1].other_idx == all_corrs[idx2].this_idx ||
                                                all_corrs[idx1].other_idx == all_corrs[idx2].other_idx)
                                                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].this_x, all_corrs[idx1].this_y,
                                                        all_corrs[idx1].this_z, all_corrs[idx2].this_x,
                                                        all_corrs[idx2].this_y, all_corrs[idx2].this_z);
 
                                        const double corrs_dist2 =
                                                mrpt::math::distanceBetweenPoints(
                                                        all_corrs[idx1].other_x, all_corrs[idx1].other_y,
                                                        all_corrs[idx1].other_z, all_corrs[idx2].other_x,
                                                        all_corrs[idx2].other_y, all_corrs[idx2].other_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 (TMapMatchingsToPoseMode::iterator itOldHyps =
                                                         sog_modes.begin();
                                                 itOldHyps != sog_modes.end(); ++itOldHyps)
                                        {
                                                if (itOldHyps->first.contains(all_corrs[idx1]) &&
                                                        itOldHyps->first.contains(all_corrs[idx2]))
                                                {
                                                        // Increment weight:
                                                        itOldHyps->second.log_w = std::log(
                                                                std::exp(itOldHyps->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].this_idx] = true;
                                        used_landmarks1[all_corrs[idx2].this_idx] = true;
                                        used_landmarks2[all_corrs[idx1].other_idx] = true;
                                        used_landmarks2[all_corrs[idx2].other_idx] = 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);
 
                                                // cout << "q: " << temptPose << endl;
 
                                                // 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.get_unsafe(1, 1) = ccos;
                                                Hc.get_unsafe(0, 0) = ccos;
                                                Hc.get_unsafe(1, 0) = ssin;
                                                Hc.get_unsafe(0, 1) = -ssin;
 
                                                CMatrixFixedNumeric<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.get_unsafe(0, 0) =
                                                                pdf_M2_j.cov.get_unsafe(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 (size_t u = 0; u < matches_idx.size(); u++)
                                                        {
                                                                if (used_landmarks1[matches_idx[u]]) continue;
 
                                                                // Jacobian wrt transformation q
                                                                Hq.get_unsafe(0, 2) =
                                                                        -p2_j_local.x * ssin - p2_j_local.y * ccos;
                                                                Hq.get_unsafe(1, 2) =
                                                                        p2_j_local.x * ccos - p2_j_local.y * ssin;
 
                                                                // COV_j = Hq \Sigma_q Hq^t + Hc Cj Hc^t
                                                                Hc.multiply_HCHt(COV_pnt, pdf_M1_i.cov);
                                                                Hq.multiply_HCHt(
                                                                        temptPose.cov, pdf_M1_i.cov, true);
 
                                                                float px, py;
                                                                lm1_pnts.getPoint(matches_idx[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);
                                                                // const double prod_ij_d2 = square(
                                                                // pdf_M1_i.mahalanobisDistanceTo(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 = matches_idx[u];
                                                                        best_pair_ij.second = j;
 
                                                                        best_pair_d2 = square(
                                                                                pdf_M1_i.mahalanobisDistanceTo(
                                                                                        pdf_M2_j));
 
                                                                        // cout << "P1: " << pdf_M1_i.mean << " C= "
                                                                        // << pdf_M1_i.cov.inMatlabFormat() << endl;
                                                                        // cout << "P2: " << pdf_M2_j.mean << " C= "
                                                                        // << pdf_M2_j.cov.inMatlabFormat() << endl;
                                                                        // cout << "  -> " << format("%e",prod_ij)
                                                                        // << " d2: " << best_pair_d2 << endl <<
                                                                        // endl;
                                                                }
                                                        }  // 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 (TMapMatchingsToPoseMode::const_iterator s =
                                                 sog_modes.begin();
                                         s != sog_modes.end(); ++s)
                                {
                                        cout << "SOG mode: " << s->second.mean
                                                 << " inliers: " << s->first.size() << endl;
                                        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;
                                }
                                printf(
                                        "   Largest consensus: %u\n",
                                        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 (TMapMatchingsToPoseMode::const_iterator s =
                                                 sog_modes.begin();
                                         s != sog_modes.end(); ++s)
                                {
                                        COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
                                                format("__debug_corrsGrid_%05u.emf", NN), m1, m2,
                                                s->first);
                                        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->m_pointsMaps.empty() &&
                                !multimap2->m_pointsMaps.empty() &&
                                multimap1->m_pointsMaps[0] && multimap2->m_pointsMaps[0])
                        {
                                CSimplePointsMap::Ptr pnts1 = multimap1->m_pointsMaps[0];
                                CSimplePointsMap::Ptr pnts2 = multimap2->m_pointsMaps[0];
 
                                CICP icp;
                                CICP::TReturnInfo icpInfo;
 
                                icp.options.maxIterations = 20;
                                icp.options.smallestThresholdDist = 0.05f;
                                icp.options.thresholdDist = 0.75f;
 
                                // cout << "Points: " << pnts1->size() << " " << pnts2->size()
                                // << endl;
 
                                // Invoke ICP once for each mode in the SOG:
                                size_t cnt = 0;
                                outInfo.icp_goodness_all_sog_modes.clear();
                                for (CPosePDFSOG::iterator i = pdf_SOG->begin();
                                         i != pdf_SOG->end(); ++cnt)
                                {
                                        CPosePDF::Ptr icp_est = icp.Align(
                                                pnts1.get(), pnts2.get(), (i)->mean, nullptr, &icpInfo);
 
                                        //(i)->cov(0,0) += square( 0.05 );
                                        //(i)->cov(1,1) += square( 0.05 );
                                        //(i)->cov(2,2) += square( DEG2RAD(0.05) );
 
                                        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);
 
                                        cout << "ICP " << cnt << " log-w: " << i->log_w
                                                 << " Goodness: " << 100 * icpInfo.goodness
                                                 << "  D_M= " << icp_maha_dist << endl;
                                        cout << "  final pos: " << icp_est->getMeanVal() << endl;
                                        cout << "    org pos: " << i->mean << endl;
 
                                        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!
 
        // Copy the output info if requested:
        // -------------------------------------------------
        MRPT_TODO(
                "Refactor `info` so it is polymorphic and can use dynamic_cast<> here");
        if (info)
        {
                TReturnInfo* info_ = static_cast<TReturnInfo*>(info);
                *info_ = outInfo;
        }
 
        if (runningTime)
        {
                *runningTime = tictac->Tac();
                delete tictac;
        }
 
        return pdf_SOG;
 
        MRPT_END
}
 
/*---------------------------------------------------------------
                                        AlignPDF_correlation
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_correlation(
        const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
        const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
        void* info)
{
        MRPT_UNUSED_PARAM(initialEstimationPDF);
        MRPT_UNUSED_PARAM(info);
 
        MRPT_START
 
        //#define       CORRELATION_SHOW_DEBUG
 
        CTicTac* tictac = nullptr;
 
        // Asserts:
        // -----------------
        ASSERT_(mm1->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
        ASSERT_(mm2->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
        const COccupancyGridMap2D* m1 =
                static_cast<const COccupancyGridMap2D*>(mm1);
        const COccupancyGridMap2D* m2 =
                static_cast<const COccupancyGridMap2D*>(mm2);
 
        ASSERT_(m1->getResolution() == m2->getResolution());
 
        if (runningTime)
        {
                tictac = new CTicTac();
                tictac->Tic();
        }
 
        // The PDF to estimate:
        // ------------------------------------------------------
        CPosePDFGaussian::Ptr PDF = mrpt::make_aligned_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();
        CMatrix 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 (unsigned int cy2 = 0; cy2 < map2_ly; cy2++)
                {
                        COccupancyGridMap2D::cellType* row = map2_mod.getRow(cy2);
                        for (unsigned int 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 substracted
                        127  // Bias to be substracted
                        );
 
                float corrPeak = outCrossCorr.maxCoeff();
                printf("phi = %fdeg \tmax corr=%f\n", 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
 
        if (runningTime)
        {
                *runningTime = tictac->Tac();
                delete tictac;
        }
 
        bestCrossCorr.normalize(0, 1);
        CImage aux(bestCrossCorr, true);
        aux.saveToFile("_debug_best_corr.png");
 
#ifdef CORRELATION_SHOW_DEBUG
        delete win;
        delete win2;
#endif
 
        // Transform the best corr matrix peak into coordinates:
        CMatrix::Index 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()
        : methodSelection(CGridMapAligner::amModifiedRANSAC),
 
          feature_descriptor(descPolarImages),
          feature_detector_options(),
 
          ransac_minSetSizeRatio(0.20f),
          ransac_SOG_sigma_m(0.10f),
          ransac_mahalanobisDistanceThreshold(6.0f),
          ransac_chi2_quantile(0.99),
          ransac_prob_good_inliers(0.9999),
          featsPerSquareMeter(0.015f),
          threshold_max(0.15f),
          threshold_delta(0.10f),
          min_ICP_goodness(0.30f),
          max_ICP_mahadist(10.0),
          maxKLd_for_merge(0.9),
 
          save_feat_coors(false),
          debug_show_corrs(false),
          debug_save_map_pairs(false)
{
}
 
/*---------------------------------------------------------------
                                        dumpToTextStream
  ---------------------------------------------------------------*/
void CGridMapAligner::TConfigParams::dumpToTextStream(std::ostream& out) const
{
        out << mrpt::format(
                "\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_show_corrs, bool)
        LOADABLEOPTS_DUMP_VAR(debug_save_map_pairs, bool)
 
        LOADABLEOPTS_DUMP_VAR(feature_descriptor, int)
 
        feature_detector_options.dumpToTextStream(out);
 
        out << mrpt::format("\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_show_corrs, 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(
        const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
        const CPose3DPDFGaussian& initialEstimationPDF, float* runningTime,
        void* info)
{
        MRPT_UNUSED_PARAM(m1);
        MRPT_UNUSED_PARAM(m2);
        MRPT_UNUSED_PARAM(initialEstimationPDF);
        MRPT_UNUSED_PARAM(runningTime);
        MRPT_UNUSED_PARAM(info);
        THROW_EXCEPTION("Align3D method not applicable to gridmap-aligner");
}