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 CObservationPtr &obs)
{
        mrpt::synch::CCriticalSectionLocker 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.present())
 
        // 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 CObservationOdometryPtr odo = CObservationOdometryPtr(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 = NULL;
                if (ICP_options.matchAgainstTheGrid && !metricMap.m_gridMaps.empty() )
                {
                        matchWith = static_cast<CMetricMap*>(metricMap.m_gridMaps[0].pointer());
                        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].pointer());
                        MRPT_LOG_DEBUG("processObservation(): matching against point map.");
                }
                ASSERT_(matchWith!=NULL)
 
                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 ) )
                                {
                                        CObservation2DRangeScanPtr obsLaser = CObservation2DRangeScanPtr(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;
 
                                CPosePDFPtr 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);
                        CPose3DPDFPtr  pose3D = CPose3DPDFPtr( CPose3DPDF::createFrom2D( posePDF ) );
 
                        CSensoryFramePtr sf = CSensoryFrame::Create();
                        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:
        CActionRobotMovement2DPtr movEstimation = action.getBestMovementEstimation();
        if (movEstimation)
        {
                m_auxAccumOdometry.composeFrom( m_auxAccumOdometry, movEstimation->poseChange->getMeanVal() );
 
                CObservationOdometryPtr obs = CObservationOdometry::Create();
                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
  ---------------------------------------------------------------*/
CPose3DPDFPtr CMetricMapBuilderICP::getCurrentPoseEstimation() const
{
        CPosePDFGaussian  pdf2D;
        m_lastPoseEst.getLatestRobotPose(pdf2D.mean);
        pdf2D.cov = m_lastPoseEst_cov;
 
        CPose3DPDFGaussianPtr pdf3D = CPose3DPDFGaussian::Create();
        pdf3D->copyFrom(pdf2D);
        return pdf3D;
}
 
/*---------------------------------------------------------------
                                                initialize
  ---------------------------------------------------------------*/
void  CMetricMapBuilderICP::initialize(
        const CSimpleMap  &initialMap,
        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:
        mrpt::synch::CCriticalSectionLocker 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++)
        {
                CPose3DPDFPtr           posePDF;
                CSensoryFramePtr        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;
}