CMultiMetricMapPDF_RBPF.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/random.h>
#include <mrpt/math/utils.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/io/CFileStream.h>

#include <mrpt/maps/CMultiMetricMapPDF.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFGrid.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/maps/CLandmarksMap.h>

#include <mrpt/slam/PF_aux_structs.h>

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace std;

namespace mrpt::slam
{
/** Fills out a "TPoseBin2D" variable, given a path hypotesis and (if not set to
 * nullptr) a new pose appended at the end, using the KLD params in "options".
    */
template <>
void KLF_loadBinFromParticle(
    detail::TPoseBin2D& outBin, const TKLDParams& opts,
    const mrpt::maps::CRBPFParticleData* currentParticleValue,
    const TPose3D* newPoseToBeInserted)
{
    // 2D pose approx: Use the latest pose only:
    if (newPoseToBeInserted)
    {
        outBin.x = round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
        outBin.y = round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
        outBin.phi = round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
    }
    else
    {
        ASSERT_(
            currentParticleValue && !currentParticleValue->robotPath.empty());
        const TPose3D& p = *currentParticleValue->robotPath.rbegin();
        outBin.x = round(p.x / opts.KLD_binSize_XY);
        outBin.y = round(p.y / opts.KLD_binSize_XY);
        outBin.phi = round(p.yaw / opts.KLD_binSize_PHI);
    }
}

/** Fills out a "TPathBin2D" variable, given a path hypotesis and (if not set to
 * nullptr) a new pose appended at the end, using the KLD params in "options".
    */
template <>
void KLF_loadBinFromParticle(
    detail::TPathBin2D& outBin, const TKLDParams& opts,
    const mrpt::maps::CRBPFParticleData* currentParticleValue,
    const TPose3D* newPoseToBeInserted)
{
    const size_t lenBinPath = (currentParticleValue != nullptr)
                                  ? currentParticleValue->robotPath.size()
                                  : 0;

    // Set the output bin dimensionality:
    outBin.bins.resize(lenBinPath + (newPoseToBeInserted != nullptr ? 1 : 0));

    // Is a path provided??
    if (currentParticleValue != nullptr)
        for (size_t i = 0; i < lenBinPath; ++i)  // Fill the bin data:
        {
            outBin.bins[i].x = round(
                currentParticleValue->robotPath[i].x / opts.KLD_binSize_XY);
            outBin.bins[i].y = round(
                currentParticleValue->robotPath[i].y / opts.KLD_binSize_XY);
            outBin.bins[i].phi = round(
                currentParticleValue->robotPath[i].yaw / opts.KLD_binSize_PHI);
        }

    // Is a newPose provided??
    if (newPoseToBeInserted != nullptr)
    {
        // And append the last pose: the new one:
        outBin.bins[lenBinPath].x =
            round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
        outBin.bins[lenBinPath].y =
            round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
        outBin.bins[lenBinPath].phi =
            round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
    }
}
}

// Include this AFTER specializations:
#include <mrpt/slam/PF_implementations.h>

/** Auxiliary for optimal sampling in RO-SLAM */
struct TAuxRangeMeasInfo
{
    TAuxRangeMeasInfo()
        : sensorLocationOnRobot(),
          sensedDistance(0),
          beaconID(INVALID_BEACON_ID),
          nGaussiansInMap(0)
    {
    }

    CPoint3D sensorLocationOnRobot;
    float sensedDistance;
    int64_t beaconID;
    size_t
        nGaussiansInMap;  // Number of Gaussian modes in the map representation

    /** Auxiliary for optimal sampling in RO-SLAM */
    static bool cmp_Asc(const TAuxRangeMeasInfo& a, const TAuxRangeMeasInfo& b)
    {
        return a.nGaussiansInMap < b.nGaussiansInMap;
    }
};

/*----------------------------------------------------------------------------------
            prediction_and_update_pfAuxiliaryPFOptimal

  See paper reference in "PF_SLAM_implementation_pfAuxiliaryPFOptimal"
 ----------------------------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFOptimal(
    const mrpt::obs::CActionCollection* actions,
    const mrpt::obs::CSensoryFrame* sf,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
    MRPT_START

    PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin2D>(
        actions, sf, PF_options, options.KLD_params);

    MRPT_END
}

/*----------------------------------------------------------------------------------
            PF_SLAM_implementation_pfAuxiliaryPFStandard
 ----------------------------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFStandard(
    const mrpt::obs::CActionCollection* actions,
    const mrpt::obs::CSensoryFrame* sf,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
    MRPT_START

    PF_SLAM_implementation_pfAuxiliaryPFStandard<
        mrpt::slam::detail::TPoseBin2D>(
        actions, sf, PF_options, options.KLD_params);

    MRPT_END
}

/*----------------------------------------------------------------------------------
            prediction_and_update_pfOptimalProposal

For grid-maps:
==============
 Approximation by Grissetti et al:  Use scan matching to approximate
   the observation model by a Gaussian:
  See: "Improved Grid-based SLAM with Rao-Blackwellized PF by Adaptive Proposals
           and Selective Resampling" (G. Grisetti, C. Stachniss, W. Burgard)

For beacon maps:
===============
  (JLBC: Method under development)

 ----------------------------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal(
    const mrpt::obs::CActionCollection* actions,
    const mrpt::obs::CSensoryFrame* sf,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
    MRPT_START

    // ----------------------------------------------------------------------
    //                      PREDICTION STAGE
    // ----------------------------------------------------------------------
    CVectorDouble rndSamples;
    size_t M = m_particles.size();
    bool updateStageAlreadyDone = false;
    CPose3D initialPose, incrPose, finalPose;

    // ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
    CICP icp(
        options.icp_params);  // Set our ICP params instead of default ones.
    CICP::TReturnInfo icpInfo;

    CParticleList::iterator partIt;

    ASSERT_(sf != nullptr);
    // Find a robot movement estimation:
    CPose3D motionModelMeanIncr;  // The mean motion increment:
    CPoseRandomSampler robotActionSampler;
    {
        CActionRobotMovement2D::Ptr robotMovement2D =
            actions->getBestMovementEstimation();

        // If there is no 2D action, look for a 3D action:
        if (robotMovement2D)
        {
            robotActionSampler.setPosePDF(
                robotMovement2D->poseChange.get_ptr());
            motionModelMeanIncr =
                mrpt::poses::CPose3D(robotMovement2D->poseChange->getMeanVal());
        }
        else
        {
            CActionRobotMovement3D::Ptr robotMovement3D =
                actions->getActionByClass<CActionRobotMovement3D>();
            if (robotMovement3D)
            {
                robotActionSampler.setPosePDF(robotMovement3D->poseChange);
                robotMovement3D->poseChange.getMean(motionModelMeanIncr);
            }
            else
            {
                motionModelMeanIncr.setFromValues(0, 0, 0);
            }
        }
    }

    // Average map will need to be updated after this:
    averageMapIsUpdated = false;

    // --------------------------------------------------------------------------------------
    //  Prediction:
    //
    //  Compute a new mean and covariance by sampling around the mean of the
    //  input "action"
    // --------------------------------------------------------------------------------------
    printf(" 1) Prediction...");
    M = m_particles.size();

    // To be computed as an average from all m_particles:
    size_t particleWithHighestW = 0;
    for (size_t i = 0; i < M; i++)
        if (getW(i) > getW(particleWithHighestW)) particleWithHighestW = i;

    //   The paths MUST already contain the starting location for each particle:
    ASSERT_(!m_particles[0].d->robotPath.empty());
    // Build the local map of points for ICP:
    CSimplePointsMap localMapPoints;
    CLandmarksMap localMapLandmarks;
    bool built_map_points = false;
    bool built_map_lms = false;

    // Update particle poses:
    size_t i;
    for (i = 0, partIt = m_particles.begin(); partIt != m_particles.end();
         partIt++, i++)
    {
        double extra_log_lik = 0;  // Used for the optimal_PF with ICP

        // Set initial robot pose estimation for this particle:
        const CPose3D ith_last_pose = CPose3D(
            *partIt->d->robotPath.rbegin());  // The last robot pose in the path

        CPose3D initialPoseEstimation = ith_last_pose + motionModelMeanIncr;

        // Use ICP with the map associated to particle?
        if (options.pfOptimalProposal_mapSelection == 0 ||
            options.pfOptimalProposal_mapSelection == 1 ||
            options.pfOptimalProposal_mapSelection == 3)
        {
            CPosePDFGaussian icpEstimation;

            // Configure the matchings that will take place in the ICP process:
            if (partIt->d->mapTillNow.m_pointsMaps.size())
            {
                ASSERT_(partIt->d->mapTillNow.m_pointsMaps.size() == 1);
                // partIt->d->mapTillNow.m_pointsMaps[0]->insertionOptions.matchStaticPointsOnly
                // = false;
            }

            CMetricMap* map_to_align_to = nullptr;

            if (options.pfOptimalProposal_mapSelection == 0)  // Grid map
            {
                ASSERT_(!partIt->d->mapTillNow.m_gridMaps.empty());

                // Build local map of points.
                if (!built_map_points)
                {
                    built_map_points = true;

                    localMapPoints.insertionOptions.minDistBetweenLaserPoints =
                        0.02f;  // 3.0f *
                    // m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution();;
                    localMapPoints.insertionOptions.isPlanarMap = true;
                    sf->insertObservationsInto(&localMapPoints);
                }

                map_to_align_to = partIt->d->mapTillNow.m_gridMaps[0].get();
            }
            else if (options.pfOptimalProposal_mapSelection == 3)  // Map of
            // points
            {
                ASSERT_(!partIt->d->mapTillNow.m_pointsMaps.empty());

                // Build local map of points.
                if (!built_map_points)
                {
                    built_map_points = true;

                    localMapPoints.insertionOptions.minDistBetweenLaserPoints =
                        0.02f;  // 3.0f *
                    // m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution();;
                    localMapPoints.insertionOptions.isPlanarMap = true;
                    sf->insertObservationsInto(&localMapPoints);
                }

                map_to_align_to = partIt->d->mapTillNow.m_pointsMaps[0].get();
            }
            else
            {
                ASSERT_(partIt->d->mapTillNow.m_landmarksMap);

                // Build local map of LMs.
                if (!built_map_lms)
                {
                    built_map_lms = true;
                    sf->insertObservationsInto(&localMapLandmarks);
                }

                map_to_align_to = partIt->d->mapTillNow.m_landmarksMap.get();
            }

            ASSERT_(map_to_align_to != nullptr);

            // Use ICP to align to each particle's map:
            {
                CPosePDF::Ptr alignEst = icp.Align(
                    map_to_align_to, &localMapPoints,
                    CPose2D(initialPoseEstimation), nullptr, &icpInfo);
                icpEstimation.copyFrom(*alignEst);
            }

            if (i == particleWithHighestW)
            {
                newInfoIndex = 1 - icpInfo.goodness;  // newStaticPointsRatio;
                // //* icpInfo.goodness;
            }

            // Set the gaussian pose:
            CPose3DPDFGaussian finalEstimatedPoseGauss(icpEstimation);

            // printf("[rbpf-slam] gridICP[%u]: %.02f%%\n", i,
            // 100*icpInfo.goodness);
            if (icpInfo.goodness < options.ICPGlobalAlign_MinQuality &&
                SFs.size())
            {
                printf(
                    "[rbpf-slam] Warning: gridICP[%u]: %.02f%% -> Using "
                    "odometry instead!\n",
                    (unsigned int)i, 100 * icpInfo.goodness);
                icpEstimation.mean = CPose2D(initialPoseEstimation);
            }

// As a way to take into account the odometry / "prior", use
//  a correcting factor in the likelihood from the mismatch
//  prior<->icp_estimate:
//          const double prior_dist_lin =
// initialPoseEstimation.distanceTo(icpEstimation.mean);
//          const double prior_dist_ang = std::abs( mrpt::math::wrapToPi(
// initialPoseEstimation.yaw()-icpEstimation.mean.phi() ) );
////            if (prior_dist_lin>0.10 || prior_dist_ang>DEG2RAD(3))
////                printf(" >>>>>>>>>> %f
///%f\n",prior_dist_lin,RAD2DEG(prior_dist_ang));
//          extra_log_lik = -(prior_dist_lin/0.20) -
//(prior_dist_ang/DEG2RAD(20));

//              printf("gICP: %.02f%%,
// Iters=%u\n",icpInfo.goodness,icpInfo.nIterations);

#if 0  // Use hacked ICP covariance:
            CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose;
            const double  Ap_dist = Ap.norm();

            finalEstimatedPoseGauss.cov.zeros();
            finalEstimatedPoseGauss.cov(0,0) = square( fabs(Ap_dist)*0.01 );
            finalEstimatedPoseGauss.cov(1,1) = square( fabs(Ap_dist)*0.01 );
            finalEstimatedPoseGauss.cov(2,2) = square( fabs(Ap.yaw())*0.02 );
#else
            // Use real ICP covariance (with a minimum level):
            keep_max(finalEstimatedPoseGauss.cov(0, 0), square(0.002));
            keep_max(finalEstimatedPoseGauss.cov(1, 1), square(0.002));
            keep_max(finalEstimatedPoseGauss.cov(2, 2), square(DEG2RAD(0.1)));

#endif

            // Generate gaussian-distributed 2D-pose increments according to
            // "finalEstimatedPoseGauss":
            // -------------------------------------------------------------------------------------------
            finalPose =
                finalEstimatedPoseGauss.mean;  // Add to the new robot pose:
            getRandomGenerator().drawGaussianMultivariate(
                rndSamples, finalEstimatedPoseGauss.cov);
            // Add noise:
            finalPose.setFromValues(
                finalPose.x() + rndSamples[0], finalPose.y() + rndSamples[1],
                finalPose.z(), finalPose.yaw() + rndSamples[2],
                finalPose.pitch(), finalPose.roll());
        }
        else if (options.pfOptimalProposal_mapSelection == 2)
        {
            // --------------------------------------------------------
            //     Perform optimal sampling with the beacon map:
            //  Described in paper...
            // --------------------------------------------------------
            /** \todo Add paper ref!
              */
            ASSERT_(partIt->d->mapTillNow.m_beaconMap);
            CBeaconMap::Ptr beacMap = partIt->d->mapTillNow.m_beaconMap;

            updateStageAlreadyDone =
                true;  // We'll also update the weight of the particle here

            // =====================================================================
            // SUMMARY:
            // For each beacon measurement in the SF, stored in
            // "lstObservedRanges",
            //  compute the SOG of the observation model, and multiply all of
            //  them
            //  (fuse) in "fusedObsModels". The result will hopefully be a very
            //  small
            //  PDF where to draw a sample from for the new robot pose.
            // =====================================================================
            bool methodSOGorGrid = false;  // TRUE=SOG
            CPoint3D newDrawnPosition;
            float firstEstimateRobotHeading = std::numeric_limits<float>::max();

            // The parameters to discard too far gaussians:
            CPoint3D centerPositionPrior(ith_last_pose);
            float centerPositionPriorRadius = 2.0f;

            if (!robotActionSampler.isPrepared())
            {
                firstEstimateRobotHeading = ith_last_pose.yaw();
                // If the map is empty: There is no solution!:
                // THROW_EXCEPTION("There is no odometry & the initial beacon
                // map is empty: RO-SLAM has no solution -> ABORTED!!!");

                if (!beacMap->size())
                {
                    // First iteration only...
                    cerr << "[RO-SLAM] Optimal filtering without map & "
                            "odometry...this message should appear only the "
                            "first iteration!!"
                         << endl;
                }
                else
                {
                    // To make RO-SLAM to have a solution, arbitrarily fix one
                    // of the beacons so
                    //  unbiguity dissapears.
                    if (beacMap->get(0).m_typePDF == CBeacon::pdfSOG)
                    {
                        cerr << "[RO-SLAM] Optimal filtering without map & "
                                "odometry->FIXING ONE BEACON!"
                             << endl;
                        ASSERT_(beacMap->get(0).m_locationSOG.size() > 0);
                        CPoint3D fixedBeacon(
                            beacMap->get(0).m_locationSOG[0].val.mean);

                        // Pass to gaussian without uncertainty:
                        beacMap->get(0).m_typePDF = CBeacon::pdfGauss;
                        beacMap->get(0).m_locationSOG.clear();
                        beacMap->get(0).m_locationGauss.mean = fixedBeacon;
                        beacMap->get(0).m_locationGauss.cov.unit(3, 1e-6);
                    }
                }
            }  // end if there is no odometry

            // 1. Make the list of beacon IDs-ranges:
            // -------------------------------------------
            deque<TAuxRangeMeasInfo> lstObservedRanges;

            for (CSensoryFrame::const_iterator itObs = sf->begin();
                 itObs != sf->end(); ++itObs)
            {
                if (IS_CLASS((*itObs), CObservationBeaconRanges))
                {
                    const CObservationBeaconRanges* obs =
                        static_cast<const CObservationBeaconRanges*>(
                            itObs->get());
                    deque<CObservationBeaconRanges::TMeasurement>::
                        const_iterator itRanges;
                    for (itRanges = obs->sensedData.begin();
                         itRanges != obs->sensedData.end(); itRanges++)
                    {
                        ASSERT_(itRanges->beaconID != INVALID_BEACON_ID);
                        // only add those in the map:
                        for (CBeaconMap::iterator itBeacs = beacMap->begin();
                             itBeacs != beacMap->end(); ++itBeacs)
                        {
                            if ((itBeacs)->m_ID == itRanges->beaconID)
                            {
                                TAuxRangeMeasInfo newMeas;
                                newMeas.beaconID = itRanges->beaconID;
                                newMeas.sensedDistance =
                                    itRanges->sensedDistance;
                                newMeas.sensorLocationOnRobot =
                                    itRanges->sensorLocationOnRobot;

                                ASSERT_(
                                    (itBeacs)->m_typePDF == CBeacon::pdfGauss ||
                                    (itBeacs)->m_typePDF == CBeacon::pdfSOG);
                                newMeas.nGaussiansInMap =
                                    (itBeacs)->m_typePDF == CBeacon::pdfSOG
                                        ? (itBeacs)->m_locationSOG.size()
                                        : 1 /*pdfGauss*/;

                                lstObservedRanges.push_back(newMeas);
                                break;  // Next observation
                            }
                        }
                    }
                }
            }

            // ASSERT_( lstObservedRanges.size()>=2 );

            // Sort ascending ranges: Smallest ranges first -> the problem is
            // easiest!
            sort(
                lstObservedRanges.begin(), lstObservedRanges.end(),
                &TAuxRangeMeasInfo::cmp_Asc);

            if (methodSOGorGrid)
            {
                CPointPDFSOG fusedObsModels;  //<- p(z_t|x_t) for all the range
                // measurements (fused)
                fusedObsModels.clear();

                // 0. OPTIONAL: Create a "prior" as a first mode in
                // "fusedObsModels"
                //       using odometry. If there is no odometry, we
                //       *absolutely* need
                //       at least one well-localized beacon at the beginning, or
                //       the symmetry cannot be broken!
                // -----------------------------------------------------------------------------------------------
                if (robotActionSampler.isPrepared())
                {
                    CPointPDFSOG::TGaussianMode newMode;
                    newMode.log_w = 0;

                    CPose3D auxPose =
                        ith_last_pose +
                        motionModelMeanIncr;  // CPose3D(robotMovement->poseChange->getEstimatedPose()));
                    firstEstimateRobotHeading = auxPose.yaw();

                    newMode.val.mean = CPoint3D(auxPose);

                    // Uncertainty in z is null:
                    // CMatrix poseCOV =
                    // robotMovement->poseChange->getEstimatedCovariance();
                    CMatrixD poseCOV;
                    robotActionSampler.getOriginalPDFCov2D(poseCOV);

                    poseCOV.setSize(2, 2);
                    poseCOV.setSize(3, 3);
                    newMode.val.cov = poseCOV;
                    fusedObsModels.push_back(newMode);  // Add it:
                }

                // 2. Generate the optimal proposal by fusing obs models
                // -------------------------------------------------------------
                for (CBeaconMap::iterator itBeacs = beacMap->begin();
                     itBeacs != beacMap->end(); ++itBeacs)
                {
                    // for each observed beacon (by its ID), generate
                    // observation model:
                    for (deque<TAuxRangeMeasInfo>::iterator itObs =
                             lstObservedRanges.begin();
                         itObs != lstObservedRanges.end(); ++itObs)
                    {
                        if ((itBeacs)->m_ID == itObs->beaconID)
                        {
                            // Match:
                            float sensedRange = itObs->sensedDistance;

                            CPointPDFSOG newObsModel;
                            (itBeacs)->generateObservationModelDistribution(
                                sensedRange, newObsModel,
                                beacMap.get(),  // The beacon map, for options
                                itObs->sensorLocationOnRobot,  // Sensor
                                // location on
                                // robot
                                centerPositionPrior, centerPositionPriorRadius);

                            if (!fusedObsModels.size())
                            {
                                // This is the first one: Just copy the obs.
                                // model here:
                                fusedObsModels = newObsModel;
                            }
                            else
                            {
                                // Fuse with existing:
                                CPointPDFSOG tempFused(0);
                                tempFused.bayesianFusion(
                                    fusedObsModels, newObsModel,
                                    3  // minMahalanobisDistToDrop
                                    );
                                fusedObsModels = tempFused;
                            }

                            // Remove modes with negligible weights:
                            // -----------------------------------------------------------

                            {
                                cout << "#modes bef: " << fusedObsModels.size()
                                     << " ESS: " << fusedObsModels.ESS()
                                     << endl;
                                double max_w = -1e100;
                                // int idx;

                                CPointPDFSOG::iterator it;

                                for (it = fusedObsModels.begin();
                                     it != fusedObsModels.end(); it++)
                                    max_w =
                                        max(max_w, (it)->log_w);  // keep the
                                // maximum
                                // mode weight

                                for (it = fusedObsModels.begin();
                                     it != fusedObsModels.end();)
                                {
                                    if (max_w - (it)->log_w >
                                        20)  // Remove the mode:
                                        it = fusedObsModels.erase(it);
                                    else
                                        it++;
                                }

                                cout
                                    << "#modes after: " << fusedObsModels.size()
                                    << endl;
                            }

                            // Shall we simplify the PDF?
                            // -----------------------------------------------------------
                            CMatrixDouble currentCov;
                            fusedObsModels.getCovariance(currentCov);
                            ASSERT_(
                                currentCov(0, 0) > 0 && currentCov(1, 1) > 0);
                            if (sqrt(currentCov(0, 0)) < 0.10f &&
                                sqrt(currentCov(1, 1)) < 0.10f &&
                                sqrt(currentCov(2, 2)) < 0.10f)
                            {
                                // Approximate by a single gaussian!
                                CPoint3D currentMean;
                                fusedObsModels.getMean(currentMean);
                                fusedObsModels[0].log_w = 0;
                                fusedObsModels[0].val.mean = currentMean;
                                fusedObsModels[0].val.cov = currentCov;

                                fusedObsModels.resize(1);
                            }

                            { /*
                                 CMatrixD  evalGrid;
                                 fusedObsModels.evaluatePDFInArea(-3,3,-3,3,0.1,0,evalGrid,
                                 true);
                                 evalGrid *= 1.0/evalGrid.maxCoeff();
                                 CImage imgF(evalGrid, true);
                                 static int autoCount=0;
                                 imgF.saveToFile(format("debug_%04i.png",autoCount++));*/
                            }
                        }
                    }  // end for itObs (in lstObservedRanges)

                }  // end for itBeacs

                /** /
                COpenGLScene    scene;
                opengl::CSetOfObjects *obj = new opengl::CSetOfObjects();
                fusedObsModels.getAs3DObject( *obj );
                scene.insert( obj );
                CFileStream("debug.3Dscene",fomWrite) << scene;
                cout << "fusedObsModels # of modes: " <<
                fusedObsModels.m_modes.size() << endl;
                printf("ESS: %f\n",fusedObsModels.ESS() );
                cout << fusedObsModels.getEstimatedCovariance() << endl;
                mrpt::system::pause(); / **/

                if (beacMap->size())
                    fusedObsModels.drawSingleSample(newDrawnPosition);
            }
            else
            {
                // =============================
                //         GRID METHOD
                // =============================

                float grid_min_x = ith_last_pose.x() - 0.5f;
                float grid_max_x = ith_last_pose.x() + 0.5f;
                float grid_min_y = ith_last_pose.y() - 0.5f;
                float grid_max_y = ith_last_pose.y() + 0.5f;
                float grid_resXY = 0.02f;

                bool repeatGridCalculation = false;

                do
                {
                    CPosePDFGrid* pdfGrid = new CPosePDFGrid(
                        grid_min_x, grid_max_x, grid_min_y, grid_max_y,
                        grid_resXY, DEG2RAD(180), 0, 0);

                    pdfGrid->uniformDistribution();

                    // Fuse all the observation models in the grid:
                    // -----------------------------------------------------
                    for (CBeaconMap::iterator itBeacs = beacMap->begin();
                         itBeacs != beacMap->end(); ++itBeacs)
                    {
                        // for each observed beacon (by its ID), generate
                        // observation model:
                        for (deque<TAuxRangeMeasInfo>::iterator itObs =
                                 lstObservedRanges.begin();
                             itObs != lstObservedRanges.end(); ++itObs)
                        {
                            if ((itBeacs)->m_ID == itObs->beaconID)
                            {
                                // Match:
                                float sensedRange = itObs->sensedDistance;

                                /** /
                                                                CPointPDFSOG
                                newObsModel;
                                                                (itBeacs)->generateObservationModelDistribution(
                                                                    sensedRange,
                                                                    newObsModel,
                                                                    beacMap,
                                // The beacon map, for options
                                                                    itObs->sensorLocationOnRobot,//
                                Sensor location on robot
                                                                    centerPositionPrior,
                                                                    centerPositionPriorRadius
                                );
                                / **/
                                for (size_t idxX = 0;
                                     idxX < pdfGrid->getSizeX(); idxX++)
                                {
                                    float grid_x = pdfGrid->idx2x(idxX);
                                    for (size_t idxY = 0;
                                         idxY < pdfGrid->getSizeY(); idxY++)
                                    {
                                        double grid_y = pdfGrid->idx2y(idxY);

                                        // Evaluate obs model:
                                        double* cell =
                                            pdfGrid->getByIndex(idxX, idxY, 0);

                                        //
                                        double lik =
                                            1;  // newObsModel.evaluatePDF(CPoint3D(grid_x,grid_y,0),true);
                                        switch ((itBeacs)->m_typePDF)
                                        {
                                            case CBeacon::pdfSOG:
                                            {
                                                CPointPDFSOG* sog =
                                                    &(itBeacs)->m_locationSOG;
                                                double sensorSTD2 = square(
                                                    beacMap->likelihoodOptions
                                                        .rangeStd);

                                                CPointPDFSOG::iterator it;
                                                for (it = sog->begin();
                                                     it != sog->end(); it++)
                                                {
                                                    lik *= exp(
                                                        -0.5 *
                                                        square(
                                                            sensedRange -
                                                            (it)->val.mean.distance2DTo(
                                                                grid_x +
                                                                    itObs
                                                                        ->sensorLocationOnRobot
                                                                        .x(),
                                                                grid_y +
                                                                    itObs
                                                                        ->sensorLocationOnRobot
                                                                        .y())) /
                                                        sensorSTD2);
                                                }
                                            }
                                            break;
                                            default:
                                                break;  // THROW_EXCEPTION("NO");
                                        }

                                        (*cell) *= lik;

                                    }  // for idxY
                                }  // for idxX
                                pdfGrid->normalize();
                            }  // end if match
                        }  // end for itObs
                    }  // end for beacons in map

                    // Draw the single pose from the grid:
                    if (beacMap->size())
                    {
                        // Take the most likely cell:
                        float maxW = -1e10f;
                        float maxX = 0, maxY = 0;
                        for (size_t idxX = 0; idxX < pdfGrid->getSizeX();
                             idxX++)
                        {
                            // float grid_x = pdfGrid->idx2x(idxX);
                            for (size_t idxY = 0; idxY < pdfGrid->getSizeY();
                                 idxY++)
                            {
                                // float grid_y = pdfGrid->idx2y(idxY);

                                // Evaluate obs model:
                                float c = *pdfGrid->getByIndex(idxX, idxY, 0);
                                if (c > maxW)
                                {
                                    maxW = c;
                                    maxX = pdfGrid->idx2x(idxX);
                                    maxY = pdfGrid->idx2y(idxY);
                                }
                            }
                        }
                        newDrawnPosition.x(maxX);
                        newDrawnPosition.y(maxY);

#if 0
                        {
                            //cout << "Grid: " << pdfGaussApprox << endl;
                            //pdfGrid->saveToTextFile("debug.txt");
                            CMatrixDouble outMat;
                            pdfGrid->getAsMatrix(0, outMat );
                            outMat *= 1.0f/outMat.maxCoeff();
                            CImage imgF(outMat, true);
                            static int autocount=0;
                            imgF.saveToFile(format("debug_grid_%f_%05i.png",grid_resXY,autocount++));
                            printf("grid res: %f   MAX: %f,%f\n",grid_resXY,maxX,maxY);
                            //mrpt::system::pause();
                        }
#endif

                        if (grid_resXY > 0.01f)
                        {
                            grid_min_x = maxX - 0.03f;
                            grid_max_x = maxX + 0.03f;
                            grid_min_y = maxY - 0.03f;
                            grid_max_y = maxY + 0.03f;
                            grid_resXY = 0.002f;
                            repeatGridCalculation = true;
                        }
                        else
                            repeatGridCalculation = false;

                        /*
                        // Approximate by a Gaussian:
                        CPosePDFGaussian pdfGaussApprox(
                            pdfGrid->getEstimatedPose(),
                            pdfGrid->getEstimatedCovariance() );
                        CPose2D newDrawnPose;
                        //pdfGrid->drawSingleSample( newDrawnPose );
                        pdfGaussApprox.drawSingleSample( newDrawnPose );
                        newDrawnPosition = newDrawnPose;
                        */
                    }
                    delete pdfGrid;
                    pdfGrid = nullptr;

                } while (repeatGridCalculation);

                // newDrawnPosition has the pose:
            }

            // 3. Get the new "finalPose" of this particle as a random sample
            // from the
            //  optimal proposal:
            // ---------------------------------------------------------------------------
            if (!beacMap->size())
            {
                // If we are in the first iteration (no map yet!) just stay
                // still:
                finalPose = ith_last_pose;
            }
            else
            {
                cout << "Drawn: " << newDrawnPosition << endl;
                // cout << "Final cov was:\n" <<
                // fusedObsModels.getEstimatedCovariance() << endl << endl;

                // Make sure it was initialized
                ASSERT_(
                    firstEstimateRobotHeading !=
                    std::numeric_limits<float>::max());

                finalPose.setFromValues(
                    newDrawnPosition.x(), newDrawnPosition.y(),
                    newDrawnPosition.z(), firstEstimateRobotHeading, 0, 0);
            }
            /** \todo If there are 2+ sensors on the robot, compute phi?
              */

            // 4. Update the weight:
            //     In optimal sampling this is the expectation of the
            //     observation likelihood: This is the observation likelihood
            //     averaged
            //     over the whole optimal proposal.
            // ---------------------------------------------------------------------------
            double weightUpdate = 0;
            partIt->log_w += PF_options.powFactor * weightUpdate;
        }
        else
        {
            // By default:
            // Generate gaussian-distributed 2D-pose increments according to
            // mean-cov:
            if (!robotActionSampler.isPrepared())
                THROW_EXCEPTION(
                    "Action list does not contain any CActionRobotMovement2D "
                    "or CActionRobotMovement3D object!");

            robotActionSampler.drawSample(incrPose);

            finalPose = ith_last_pose + incrPose;
        }

        // Insert as the new pose in the path:
        partIt->d->robotPath.push_back(finalPose.asTPose());

        // ----------------------------------------------------------------------
        //                      UPDATE STAGE
        // ----------------------------------------------------------------------
        if (!updateStageAlreadyDone)
        {
            partIt->log_w += PF_options.powFactor *
                             (PF_SLAM_computeObservationLikelihoodForParticle(
                                  PF_options, i, *sf, finalPose) +
                              extra_log_lik);
        }  // if update not already done...

    }  // end of for each particle "i" & "partIt"

    printf("Ok\n");

    MRPT_END
}

/*---------------------------------------------------------------
            prediction_and_update_pfStandardProposal
 ---------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfStandardProposal(
    const mrpt::obs::CActionCollection* actions,
    const mrpt::obs::CSensoryFrame* sf,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
    MRPT_START

    PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
        actions, sf, PF_options, options.KLD_params);

    // Average map will need to be updated after this:
    averageMapIsUpdated = false;

    MRPT_END
}

// Specialization for my kind of particles:
void CMultiMetricMapPDF::
    PF_SLAM_implementation_custom_update_particle_with_new_pose(
        CRBPFParticleData* particleData, const TPose3D& newPose) const
{
    particleData->robotPath.push_back(newPose);
}

// Specialization for RBPF maps:
bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
    const CMultiMetricMapPDF::CParticleList& particles,
    const CSensoryFrame* sf) const
{
    if (sf == nullptr) return false;
    ASSERT_(!particles.empty());
    return particles.begin()
        ->d.get()
        ->mapTillNow.canComputeObservationsLikelihood(*sf);
}

/** Do not move the particles until the map is populated.  */
bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement() const
{
    return 0 == getNumberOfObservationsInSimplemap();
}

/*---------------------------------------------------------------
 Evaluate the observation likelihood for one
   particle at a given location
 ---------------------------------------------------------------*/
double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
    const CParticleFilter::TParticleFilterOptions& PF_options,
    const size_t particleIndexForMap, const CSensoryFrame& observation,
    const CPose3D& x) const
{
    MRPT_UNUSED_PARAM(PF_options);
    CMultiMetricMap* map = const_cast<CMultiMetricMap*>(
        &m_particles[particleIndexForMap].d->mapTillNow);
    double ret = 0;
    for (CSensoryFrame::const_iterator it = observation.begin();
         it != observation.end(); ++it)
        ret += map->computeObservationLikelihood((CObservation*)it->get(), x);
    return ret;
}