CMetricMapBuilderICP.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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/CICP.h>
#include <mrpt/slam/CMetricMapBuilderICP.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/utils/CEnhancedMetaFile.h>

using namespace std;
using namespace mrpt::slam;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace mrpt::math;

/*---------------------------------------------------------------
                 Constructor
  ---------------------------------------------------------------*/
CMetricMapBuilderICP::CMetricMapBuilderICP()
        : ICP_options(m_min_verbosity_level)
{
        this->setLoggerName("CMetricMapBuilderICP");
        this->initialize(CSimpleMap());
}

/*---------------------------------------------------------------
                                                        Destructor
  ---------------------------------------------------------------*/
CMetricMapBuilderICP::~CMetricMapBuilderICP()
{
        // Asure, we have exit all critical zones:
        enterCriticalSection();
        leaveCriticalSection();

        // Save current map to current file:
        setCurrentMapFile("");
}

/*---------------------------------------------------------------
                                                        Options
  ---------------------------------------------------------------*/
CMetricMapBuilderICP::TConfigParams::TConfigParams(
        mrpt::utils::VerbosityLevel& parent_verbosity_level)
        : matchAgainstTheGrid(false),
          insertionLinDistance(1.0),
          insertionAngDistance(DEG2RAD(30)),
          localizationLinDistance(0.20),
          localizationAngDistance(DEG2RAD(30)),
          minICPgoodnessToAccept(0.40),
          verbosity_level(parent_verbosity_level),
          mapInitializers()
{
}

CMetricMapBuilderICP::TConfigParams& CMetricMapBuilderICP::TConfigParams::
	operator=(const CMetricMapBuilderICP::TConfigParams& other)
{
        matchAgainstTheGrid = other.matchAgainstTheGrid;
        insertionLinDistance = other.insertionLinDistance;
        insertionAngDistance = other.insertionAngDistance;
        localizationLinDistance = other.localizationLinDistance;
        localizationAngDistance = other.localizationAngDistance;
        minICPgoodnessToAccept = other.minICPgoodnessToAccept;
        //      We can't copy a reference type
        //      verbosity_level         = other.verbosity_level;
        mapInitializers = other.mapInitializers;
        return *this;
}

void CMetricMapBuilderICP::TConfigParams::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& source, const std::string& section)
{
        MRPT_LOAD_CONFIG_VAR(matchAgainstTheGrid, bool, source, section)
        MRPT_LOAD_CONFIG_VAR(insertionLinDistance, double, source, section)
        MRPT_LOAD_CONFIG_VAR_DEGREES(insertionAngDistance, source, section)
        MRPT_LOAD_CONFIG_VAR(localizationLinDistance, double, source, section)
        MRPT_LOAD_CONFIG_VAR_DEGREES(localizationAngDistance, source, section)
        verbosity_level = source.read_enum<mrpt::utils::VerbosityLevel>(
                section, "verbosity_level", verbosity_level);

        MRPT_LOAD_CONFIG_VAR(minICPgoodnessToAccept, double, source, section)

        mapInitializers.loadFromConfigFile(source, section);
}

void CMetricMapBuilderICP::TConfigParams::dumpToTextStream(CStream& out) const
{
        out.printf(
                "\n----------- [CMetricMapBuilderICP::TConfigParams] ------------ "
                "\n\n");

        out.printf(
                "insertionLinDistance                    = %f m\n",
                insertionLinDistance);
        out.printf(
                "insertionAngDistance                    = %f deg\n",
                RAD2DEG(insertionAngDistance));
        out.printf(
                "localizationLinDistance                 = %f m\n",
                localizationLinDistance);
        out.printf(
                "localizationAngDistance                 = %f deg\n",
                RAD2DEG(localizationAngDistance));
        out.printf(
                "verbosity_level                         = %s\n",
                mrpt::utils::TEnumType<mrpt::utils::VerbosityLevel>::value2name(
                        verbosity_level)
                        .c_str());

        out.printf("  Now showing 'mapsInitializers':\n");
        mapInitializers.dumpToTextStream(out);
}

/*---------------------------------------------------------------
                                                processObservation
 This is the new entry point of the algorithm (the old one
  was processActionObservation, which now is a wrapper to
  this method).
  ---------------------------------------------------------------*/
void CMetricMapBuilderICP::processObservation(const CObservation::Ptr& obs)
{
        std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);

        MRPT_START

        if (metricMap.m_pointsMaps.empty() && metricMap.m_gridMaps.empty())
                throw std::runtime_error(
                        "Neither grid maps nor points map: Have you called initialize() "
                        "after setting ICP_options.mapInitializers?");

        ASSERT_(obs)

        // Is it an odometry observation??
        if (IS_CLASS(obs, CObservationOdometry))
        {
                MRPT_LOG_DEBUG("processObservation(): obs is CObservationOdometry");
                m_there_has_been_an_odometry = true;

                const CObservationOdometry::Ptr odo =
                        std::dynamic_pointer_cast<CObservationOdometry>(obs);
                ASSERT_(odo->timestamp != INVALID_TIMESTAMP)

                CPose2D pose_before;
                bool pose_before_valid = m_lastPoseEst.getLatestRobotPose(pose_before);

                // Move our estimation:
                m_lastPoseEst.processUpdateNewOdometry(
                        odo->odometry, odo->timestamp, odo->hasVelocities,
                        odo->velocityLocal);

                if (pose_before_valid)
                {
                        // Accumulate movement:
                        CPose2D pose_after;
                        if (m_lastPoseEst.getLatestRobotPose(pose_after))
                                this->accumulateRobotDisplacementCounters(pose_after);
                        MRPT_LOG_DEBUG_STREAM(
                                "processObservation(): obs is CObservationOdometry, new "
                                "post_after="
                                << pose_after);
                }
        }  // end it's odometry
        else
        {
                // Current robot pose given the timestamp of the observation (this can
                // include a small extrapolation
                //  using the latest known robot velocities):
                TPose2D initialEstimatedRobotPose(0, 0, 0);
                {
                        mrpt::math::TTwist2D robotVelLocal, robotVelGlobal;
                        if (obs->timestamp != INVALID_TIMESTAMP)
                        {
                                MRPT_LOG_DEBUG(
                                        "processObservation(): extrapolating pose from latest pose "
                                        "and new observation timestamp...");
                                if (!m_lastPoseEst.getCurrentEstimate(
                                                initialEstimatedRobotPose, robotVelLocal,
                                                robotVelGlobal, obs->timestamp))
                                {  // couldn't had a good extrapolation estimate... we'll have
                                        // to live with the latest pose:
                                        m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
                                        MRPT_LOG_WARN(
                                                "processObservation(): new pose extrapolation failed, "
                                                "using last pose as is.");
                                }
                        }
                        else
                        {
                                MRPT_LOG_WARN(
                                        "processObservation(): invalid observation timestamp.");
                                m_lastPoseEst.getLatestRobotPose(initialEstimatedRobotPose);
                        }
                }

                // To know the total path length:
                CPose2D previousKnownRobotPose;
                m_lastPoseEst.getLatestRobotPose(previousKnownRobotPose);

                // Increment (this may only include the effects of extrapolation with
                // velocity...):
                this->accumulateRobotDisplacementCounters(
                        previousKnownRobotPose);  // initialEstimatedRobotPose-previousKnownRobotPose);

                // We'll skip ICP-based localization for this observation only if:
                //  - We had some odometry since the last pose correction
                //  (m_there_has_been_an_odometry=true).
                //  - AND, the traversed distance is small enough:
                const bool we_skip_ICP_pose_correction =
                        m_there_has_been_an_odometry &&
                        m_distSinceLastICP.lin < std::min(
                                                                                 ICP_options.localizationLinDistance,
                                                                                 ICP_options.insertionLinDistance) &&
                        m_distSinceLastICP.ang < std::min(
                                                                                 ICP_options.localizationAngDistance,
                                                                                 ICP_options.insertionAngDistance);

                MRPT_LOG_DEBUG_STREAM(
                        "processObservation(): skipping ICP pose correction due to small "
                        "odometric displacement? : "
                        << (we_skip_ICP_pose_correction ? "YES" : "NO"));

                CICP::TReturnInfo icpReturn;
                bool can_do_icp = false;

                // Select the map to match with ....
                CMetricMap* matchWith = nullptr;
                if (ICP_options.matchAgainstTheGrid && !metricMap.m_gridMaps.empty())
                {
                        matchWith = static_cast<CMetricMap*>(metricMap.m_gridMaps[0].get());
                        MRPT_LOG_DEBUG("processObservation(): matching against gridmap.");
                }
                else
                {
                        ASSERTMSG_(
                                metricMap.m_pointsMaps.size(),
                                "No points map in multi-metric map.")
                        matchWith =
                                static_cast<CMetricMap*>(metricMap.m_pointsMaps[0].get());
                        MRPT_LOG_DEBUG("processObservation(): matching against point map.");
                }
                ASSERT_(matchWith != nullptr)

                if (!we_skip_ICP_pose_correction)
                {
                        m_there_has_been_an_odometry = false;

                        // --------------------------------------------------------------------------------------
                        // Any other observation:
                        //  1) If the observation generates points in a point map, do ICP
                        //  2) In any case, insert the observation if the minimum distance
                        //  has been satisfaced.
                        // --------------------------------------------------------------------------------------
                        CSimplePointsMap sensedPoints;
                        sensedPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
                        sensedPoints.insertionOptions.also_interpolate = false;

                        // Create points representation of the observation:
                        // Insert only those planar range scans in the altitude of the grid
                        // map:
                        if (ICP_options.matchAgainstTheGrid &&
                                !metricMap.m_gridMaps.empty() &&
                                metricMap.m_gridMaps[0]->insertionOptions.useMapAltitude)
                        {
                                // Use grid altitude:
                                if (IS_CLASS(obs, CObservation2DRangeScan))
                                {
                                        CObservation2DRangeScan::Ptr obsLaser =
                                                std::dynamic_pointer_cast<CObservation2DRangeScan>(obs);
                                        if (std::abs(
                                                        metricMap.m_gridMaps[0]
                                                                ->insertionOptions.mapAltitude -
                                                        obsLaser->sensorPose.z()) < 0.01)
                                                can_do_icp = sensedPoints.insertObservationPtr(obs);
                                }
                        }
                        else
                        {
                                // Do not use grid altitude:
                                can_do_icp = sensedPoints.insertObservationPtr(obs);
                        }

                        if (IS_DERIVED(matchWith, CPointsMap) &&
                                static_cast<CPointsMap*>(matchWith)->empty())
                                can_do_icp = false;  // The reference map is empty!

                        if (can_do_icp)
                        {
                                // We DO HAVE points with this observation:
                                // Execute ICP over the current points map and the sensed
                                // points:
                                // ----------------------------------------------------------------------
                                CICP ICP;
                                float runningTime;

                                ICP.options = ICP_params;

                                CPosePDF::Ptr pestPose = ICP.Align(
                                        matchWith,  // Map 1
                                        &sensedPoints,  // Map 2
                                        mrpt::poses::CPose2D(
                                                initialEstimatedRobotPose),  // a first gross estimation
                                        // of map 2 relative to map
                                        // 1.
                                        &runningTime,  // Running time
                                        &icpReturn  // Returned information
                                        );

                                if (icpReturn.goodness > ICP_options.minICPgoodnessToAccept)
                                {
                                        // save estimation:
                                        CPosePDFGaussian pEst2D;
                                        pEst2D.copyFrom(*pestPose);

                                        m_lastPoseEst.processUpdateNewPoseLocalization(
                                                TPose2D(pEst2D.mean), obs->timestamp);
                                        m_lastPoseEst_cov = pEst2D.cov;

                                        m_distSinceLastICP.updatePose(pEst2D.mean);

                                        // Debug output to console:
                                        MRPT_LOG_INFO_STREAM(
                                                "processObservation: previousPose="
                                                << previousKnownRobotPose << "-> currentPose="
                                                << pEst2D.getMeanVal() << std::endl);
                                        MRPT_LOG_INFO(
                                                format(
                                                        "[CMetricMapBuilderICP]   Fit:%.1f%% Itr:%i In "
                                                        "%.02fms \n",
                                                        icpReturn.goodness * 100, icpReturn.nIterations,
                                                        1000 * runningTime));
                                }
                                else
                                {
                                        MRPT_LOG_WARN_STREAM(
                                                "Ignoring ICP of low quality: "
                                                << icpReturn.goodness * 100 << std::endl);
                                }

                                // Compute the transversed length:
                                CPose2D currentKnownRobotPose;
                                m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);

                                this->accumulateRobotDisplacementCounters(
                                        currentKnownRobotPose);  // currentKnownRobotPose -
                                // previousKnownRobotPose);

                        }  // end we can do ICP.
                        else
                        {
                                MRPT_LOG_WARN_STREAM(
                                        "Cannot do ICP: empty pointmap or not suitable "
                                        "gridmap...\n");
                        }

                }  // else, we do ICP pose correction

                // ----------------------------------------------------------
                //                              CRITERION TO DECIDE MAP UPDATE:
                //   A distance large-enough from the last update for each sensor, AND
                //    either: (i) this was a good match or (ii) this is the first time
                //    for this sensor.
                // ----------------------------------------------------------
                const bool firstTimeForThisSensor =
                        m_distSinceLastInsertion.find(obs->sensorLabel) ==
                        m_distSinceLastInsertion.end();
                bool update =
                        firstTimeForThisSensor ||
                        ((!can_do_icp ||
                          icpReturn.goodness > ICP_options.minICPgoodnessToAccept) &&
                         (m_distSinceLastInsertion[obs->sensorLabel].lin >=
                                  ICP_options.insertionLinDistance ||
                          m_distSinceLastInsertion[obs->sensorLabel].ang >=
                                  ICP_options.insertionAngDistance));

                // Used any "options.alwaysInsertByClass" ??
                if (options.alwaysInsertByClass.contains(obs->GetRuntimeClass()))
                        update = true;

                // We need to always insert ALL the observations at the beginning until
                // the first one
                //  that actually insert some points into the map used as a reference,
                //  since otherwise
                //  we'll not be able to do ICP against an empty map!!
                if (matchWith && matchWith->isEmpty()) update = true;

                MRPT_LOG_DEBUG_STREAM(
                        "update map: " << (update ? "YES" : "NO")
                                                   << " options.enableMapUpdating: "
                                                   << (options.enableMapUpdating ? "YES" : "NO"));

                if (options.enableMapUpdating && update)
                {
                        CTicTac tictac;

                        tictac.Tic();

                        // Insert the observation:
                        CPose2D currentKnownRobotPose;
                        m_lastPoseEst.getLatestRobotPose(currentKnownRobotPose);

                        // Create new entry:
                        m_distSinceLastInsertion[obs->sensorLabel].last_update =
                                currentKnownRobotPose;

                        // Reset distance counters:
                        resetRobotDisplacementCounters(currentKnownRobotPose);
                        // m_distSinceLastInsertion[obs->sensorLabel].updatePose(currentKnownRobotPose);

                        MRPT_LOG_INFO(
                                mrpt::format(
                                        "Updating map from pose %s\n",
                                        currentKnownRobotPose.asString().c_str()));

                        CPose3D estimatedPose3D(currentKnownRobotPose);
                        const bool anymap_update =
                                metricMap.insertObservationPtr(obs, &estimatedPose3D);
                        if (!anymap_update)
                                MRPT_LOG_WARN_STREAM(
                                        "**No map was updated** after inserting an observation of "
                                        "type `"
                                        << obs->GetRuntimeClass()->className << "`");

                        // Add to the vector of "poses"-"SFs" pairs:
                        CPosePDFGaussian posePDF(currentKnownRobotPose);
                        CPose3DPDF::Ptr pose3D =
                                CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(posePDF));

                        CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
                        sf->insert(obs);

                        SF_Poses_seq.insert(pose3D, sf);

                        MRPT_LOG_INFO_STREAM(
                                "Map updated OK. Done in "
                                << mrpt::system::formatTimeInterval(tictac.Tac()) << std::endl);
                }

        }  // end other observation

        // Robot path history:
        {
                TPose2D p;
                if (m_lastPoseEst.getLatestRobotPose(p)) m_estRobotPath.push_back(p);
        }

        MRPT_END

}  // end processObservation

/*---------------------------------------------------------------

                                                processActionObservation

  ---------------------------------------------------------------*/
void CMetricMapBuilderICP::processActionObservation(
        CActionCollection& action, CSensoryFrame& in_SF)
{
        // 1) process action:
        CActionRobotMovement2D::Ptr movEstimation =
                action.getBestMovementEstimation();
        if (movEstimation)
        {
                m_auxAccumOdometry.composeFrom(
                        m_auxAccumOdometry, movEstimation->poseChange->getMeanVal());

                CObservationOdometry::Ptr obs =
                        mrpt::make_aligned_shared<CObservationOdometry>();
                obs->timestamp = movEstimation->timestamp;
                obs->odometry = m_auxAccumOdometry;
                this->processObservation(obs);
        }

        // 2) Process observations one by one:
        for (CSensoryFrame::iterator i = in_SF.begin(); i != in_SF.end(); ++i)
                this->processObservation(*i);
}

/*---------------------------------------------------------------
                                                setCurrentMapFile
  ---------------------------------------------------------------*/
void CMetricMapBuilderICP::setCurrentMapFile(const char* mapFile)
{
        // Save current map to current file:
        if (currentMapFile.size()) saveCurrentMapToFile(currentMapFile.c_str());

        // Sets new current map file:
        currentMapFile = mapFile;

        // Load map from file or create an empty one:
        if (currentMapFile.size()) loadCurrentMapFromFile(mapFile);
}

/*---------------------------------------------------------------
                                                getCurrentPoseEstimation
  ---------------------------------------------------------------*/
CPose3DPDF::Ptr CMetricMapBuilderICP::getCurrentPoseEstimation() const
{
        CPosePDFGaussian pdf2D;
        m_lastPoseEst.getLatestRobotPose(pdf2D.mean);
        pdf2D.cov = m_lastPoseEst_cov;

        CPose3DPDFGaussian::Ptr pdf3D =
                mrpt::make_aligned_shared<CPose3DPDFGaussian>();
        pdf3D->copyFrom(pdf2D);
        return pdf3D;
}

/*---------------------------------------------------------------
                                                initialize
  ---------------------------------------------------------------*/
void CMetricMapBuilderICP::initialize(
        const CSimpleMap& initialMap, const CPosePDF* x0)
{
        MRPT_START

        // Reset vars:
        m_estRobotPath.clear();
        m_auxAccumOdometry = CPose2D(0, 0, 0);

        m_distSinceLastICP.lin = m_distSinceLastICP.ang = 0;
        m_distSinceLastInsertion.clear();

        m_there_has_been_an_odometry = false;

        // Init path & map:
        std::lock_guard<std::mutex> lock_cs(critZoneChangingMap);

        // Create metric maps:
        metricMap.setListOfMaps(&ICP_options.mapInitializers);

        // copy map:
        SF_Poses_seq = initialMap;

        // Parse SFs to the hybrid map:
        // Set options:
        // ---------------------
        // if (metricMap.m_pointsMaps.size())
        //{
        //      metricMap.m_pointsMaps[0]->insertionOptions.fuseWithExisting =
        // false;
        //      metricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints =
        // 0.05f;
        //      metricMap.m_pointsMaps[0]->insertionOptions.disableDeletion                     =
        // true;
        //      metricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap                         =
        // true;
        //      metricMap.m_pointsMaps[0]->insertionOptions.matchStaticPointsOnly =
        // true;
        //}

        // Load estimated pose from given PDF:
        m_lastPoseEst.reset();

        if (x0)
                m_lastPoseEst.processUpdateNewPoseLocalization(
                        x0->getMeanVal(), mrpt::system::now());

        for (size_t i = 0; i < SF_Poses_seq.size(); i++)
        {
                CPose3DPDF::Ptr posePDF;
                CSensoryFrame::Ptr SF;

                // Get the SF and its pose:
                SF_Poses_seq.get(i, posePDF, SF);

                CPose3D estimatedPose3D;
                posePDF->getMean(estimatedPose3D);

                // Insert observations into the map:
                SF->insertObservationsInto(&metricMap, &estimatedPose3D);
        }

        MRPT_LOG_INFO("loadCurrentMapFromFile() OK.\n");

        MRPT_END
}

/*---------------------------------------------------------------
                                                getCurrentMapPoints
  ---------------------------------------------------------------*/
void CMetricMapBuilderICP::getCurrentMapPoints(
        std::vector<float>& x, std::vector<float>& y)
{
        // Critical section: We are using our global metric map
        enterCriticalSection();

        ASSERT_(metricMap.m_pointsMaps.size() > 0);
        metricMap.m_pointsMaps[0]->getAllPoints(x, y);

        // Exit critical zone.
        leaveCriticalSection();
}

/*---------------------------------------------------------------
                                getCurrentlyBuiltMap
  ---------------------------------------------------------------*/
void CMetricMapBuilderICP::getCurrentlyBuiltMap(CSimpleMap& out_map) const
{
        out_map = SF_Poses_seq;
}

const CMultiMetricMap* CMetricMapBuilderICP::getCurrentlyBuiltMetricMap() const
{
        return &metricMap;
}

/*---------------------------------------------------------------
                        getCurrentlyBuiltMapSize
  ---------------------------------------------------------------*/
unsigned int CMetricMapBuilderICP::getCurrentlyBuiltMapSize()
{
        return SF_Poses_seq.size();
}

/*---------------------------------------------------------------
                                saveCurrentEstimationToImage
  ---------------------------------------------------------------*/
void CMetricMapBuilderICP::saveCurrentEstimationToImage(
        const std::string& file, bool formatEMF_BMP)
{
        MRPT_START

        CImage img;
        const size_t nPoses = m_estRobotPath.size();

        ASSERT_(metricMap.m_gridMaps.size() > 0);

        if (!formatEMF_BMP) THROW_EXCEPTION("Not implemented yet for BMP!");

        // grid map as bitmap:
        // ----------------------------------
        metricMap.m_gridMaps[0]->getAsImage(img);

        // Draw paths (using vectorial plots!) over the EMF file:
        // -------------------------------------------------
        //      float                                   SCALE = 1000;
        CEnhancedMetaFile EMF(file, 1000);

        EMF.drawImage(0, 0, img);

        unsigned int imgHeight = img.getHeight();

        // Path hypothesis:
        // ----------------------------------
        int x1, x2, y1, y2;

        // First point: (0,0)
        x2 = metricMap.m_gridMaps[0]->x2idx(0.0f);
        y2 = metricMap.m_gridMaps[0]->y2idx(0.0f);

        // Draw path in the bitmap:
        for (size_t j = 0; j < nPoses; j++)
        {
                // For next segment
                x1 = x2;
                y1 = y2;

                // Coordinates -> pixels
                x2 = metricMap.m_gridMaps[0]->x2idx(m_estRobotPath[j].x);
                y2 = metricMap.m_gridMaps[0]->y2idx(m_estRobotPath[j].y);

                // Draw line:
                EMF.line(x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
        }

        MRPT_END
}

void CMetricMapBuilderICP::accumulateRobotDisplacementCounters(
        const CPose2D& new_pose)
{
        m_distSinceLastICP.updateDistances(new_pose);

        for (mrpt::aligned_containers<
                         std::string, CMetricMapBuilderICP::TDist>::map_t::iterator it =
                         m_distSinceLastInsertion.begin();
                 it != m_distSinceLastInsertion.end(); ++it)
                it->second.updateDistances(new_pose);
}

void CMetricMapBuilderICP::resetRobotDisplacementCounters(
        const CPose2D& new_pose)
{
        m_distSinceLastICP.updatePose(new_pose);

        for (mrpt::aligned_containers<
                         std::string, CMetricMapBuilderICP::TDist>::map_t::iterator it =
                         m_distSinceLastInsertion.begin();
                 it != m_distSinceLastInsertion.end(); ++it)
                it->second.updatePose(new_pose);
}

void CMetricMapBuilderICP::TDist::updateDistances(const mrpt::poses::CPose2D& p)
{
        mrpt::poses::CPose2D Ap = p - this->last_update;
        lin = Ap.norm();
        ang = std::abs(Ap.phi());
}

void CMetricMapBuilderICP::TDist::updatePose(const mrpt::poses::CPose2D& p)
{
        this->last_update = p;
        lin = 0;
        ang = 0;
}