CLocalMetricHypothesis.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 "hmtslam-precomp.h"  // Precomp header
 
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/os.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/opengl/CSimpleLine.h>
#include <mrpt/opengl/CText.h>
#include <mrpt/opengl/CSphere.h>
#include <mrpt/opengl/CArrow.h>
#include <mrpt/opengl/stock_objects.h>
 
#include <mrpt/hmtslam/CRobotPosesGraph.h>
 
using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::hmtslam;
using namespace mrpt::opengl;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;
 
IMPLEMENTS_SERIALIZABLE(CLocalMetricHypothesis, CSerializable, mrpt::hmtslam)
IMPLEMENTS_SERIALIZABLE(CLSLAMParticleData, CSerializable, mrpt::hmtslam)
 
/*---------------------------------------------------------------
                                Normal constructor
  ---------------------------------------------------------------*/
CLocalMetricHypothesis::CLocalMetricHypothesis(CHMTSLAM* parent)
        : m_parent(parent), m_log_w_metric_history()
// m_log_w_topol_history()
{
        m_log_w = 0;
        m_accumRobotMovementIsValid = false;
}
 
/*---------------------------------------------------------------
                                Destructor
  ---------------------------------------------------------------*/
CLocalMetricHypothesis::~CLocalMetricHypothesis() { clearParticles(); }
/*---------------------------------------------------------------
                                   getAs3DScene
 
        Returns a 3D representation of the current robot pose, all
        the poses in the auxiliary graph, and each of the areas
        they belong to.
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::getAs3DScene(
        opengl::CSetOfObjects::Ptr& objs) const
{
        objs->clear();
 
        // -------------------------------------------
        // Draw a grid on the ground:
        // -------------------------------------------
        {
                opengl::CGridPlaneXY::Ptr obj =
                        mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
                                -100, 100, -100, 100, 0, 5);
                obj->setColor(0.4, 0.4, 0.4);
 
                objs->insert(obj);  // it will free the memory
        }
 
        // ---------------------------------------------------------
        // Draw each of the robot poses as 2D/3D ellipsoids
        //  For the current pose, draw the robot itself as well.
        // ---------------------------------------------------------
        std::map<TPoseID, CPose3DPDFParticles> lstPoses;
        std::map<TPoseID, CPose3DPDFParticles>::iterator it;
        getPathParticles(lstPoses);
 
        // -----------------------------------------------
        // Draw a 3D coordinates corner for each cluster
        // -----------------------------------------------
        {
                std::lock_guard<std::mutex> lock(m_parent->m_map_cs);
 
                for (TNodeIDSet::const_iterator n = m_neighbors.begin();
                         n != m_neighbors.end(); ++n)
                {
                        const CHMHMapNode::Ptr node = m_parent->m_map.getNodeByID(*n);
                        ASSERT_(node);
                        TPoseID poseID_origin;
                        CPose3D originPose;
                        if (node->m_annotations.getElemental(
                                        NODE_ANNOTATION_REF_POSEID, poseID_origin, m_ID))
                        {
                                lstPoses[poseID_origin].getMean(originPose);
 
                                opengl::CSetOfObjects::Ptr corner = stock_objects::CornerXYZ();
                                corner->setPose(originPose);
                                objs->insert(corner);
                        }
                }
        }  // end of lock on map_cs
 
        // Add a camera following the robot:
        // -----------------------------------------
        const CPose3D meanCurPose = lstPoses[m_currentRobotPose].getMeanVal();
        {
                opengl::CCamera::Ptr cam = mrpt::make_aligned_shared<opengl::CCamera>();
                cam->setZoomDistance(85);
                cam->setAzimuthDegrees(45 + RAD2DEG(meanCurPose.yaw()));
                cam->setElevationDegrees(45);
                cam->setPointingAt(meanCurPose);
                objs->insert(cam);
        }
 
        map<CHMHMapNode::TNodeID, CPoint3D>
                areas_mean;  // Store the mean pose of each area
        map<CHMHMapNode::TNodeID, int> areas_howmany;
 
        for (it = lstPoses.begin(); it != lstPoses.end(); it++)
        {
                opengl::CEllipsoid::Ptr ellip =
                        mrpt::make_aligned_shared<opengl::CEllipsoid>();
                // Color depending on being into the current area:
                if (m_nodeIDmemberships.find(it->first)->second ==
                        m_nodeIDmemberships.find(m_currentRobotPose)->second)
                        ellip->setColor(0, 0, 1);
                else
                        ellip->setColor(1, 0, 0);
 
                const CPose3DPDFParticles* pdfParts = &it->second;
                CPose3DPDFGaussian pdf;
                pdf.copyFrom(*pdfParts);
 
                // Mean:
                ellip->setLocation(
                        pdf.mean.x(), pdf.mean.y(),
                        pdf.mean.z() + 0.005);  // To be above the ground gridmap...
 
                // Cov:
                CMatrixDouble C = CMatrixDouble(pdf.cov);  // 6x6 cov.
                C.setSize(3, 3);
 
                // Is a 2D pose??
                if (pdf.mean.pitch() == 0 && pdf.mean.roll() == 0 &&
                        pdf.cov(2, 2) < 1e-4f)
                        C.setSize(2, 2);
 
                ellip->setCovMatrix(C);
 
                ellip->enableDrawSolid3D(false);
 
                // Name:
                ellip->setName(format("P%u", (unsigned int)it->first));
                ellip->enableShowName();
 
                // Add it:
                objs->insert(ellip);
 
                // Add an arrow for the mean direction also:
                {
                        mrpt::opengl::CArrow::Ptr obj = mrpt::opengl::CArrow::Create(
                                0, 0, 0, 0.20f, 0, 0, 0.25f, 0.005f, 0.02f);
                        obj->setColor(1, 0, 0);
                        obj->setPose(pdf.mean);
                        objs->insert(obj);
                }
 
                // Is this the current robot pose?
                if (it->first == m_currentRobotPose)
                {
                        // Draw robot:
                        opengl::CSetOfObjects::Ptr robot = stock_objects::RobotPioneer();
                        robot->setPose(pdf.mean);
 
                        // Add it:
                        objs->insert(robot);
                }
                else  // The current robot pose does not count
                {
                        // Compute the area means:
                        std::map<TPoseID, CHMHMapNode::TNodeID>::const_iterator itAreaId =
                                m_nodeIDmemberships.find(it->first);
                        ASSERT_(itAreaId != m_nodeIDmemberships.end());
                        CHMHMapNode::TNodeID areaId = itAreaId->second;
                        areas_howmany[areaId]++;
                        areas_mean[areaId].x_incr(pdf.mean.x());
                        areas_mean[areaId].y_incr(pdf.mean.y());
                        areas_mean[areaId].z_incr(pdf.mean.z());
                }
        }  // end for it
 
        // Average area means:
        map<CHMHMapNode::TNodeID, CPoint3D>::iterator itMeans;
        map<CHMHMapNode::TNodeID, int>::iterator itHowMany;
        ASSERT_(areas_howmany.size() == areas_mean.size());
        for (itMeans = areas_mean.begin(), itHowMany = areas_howmany.begin();
                 itMeans != areas_mean.end(); itMeans++, itHowMany++)
        {
                ASSERT_(itHowMany->second > 0);
 
                float f = 1.0f / itHowMany->second;
                itMeans->second *= f;
        }
 
        // -------------------------------------------------------------------
        // Draw lines between robot poses & their corresponding area sphere
        // -------------------------------------------------------------------
        for (it = lstPoses.begin(); it != lstPoses.end(); it++)
        {
                if (it->first != m_currentRobotPose)
                {
                        CPoint3D areaPnt(
                                areas_mean[m_nodeIDmemberships.find(it->first)->second]);
                        areaPnt.z_incr(m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
 
                        const CPose3DPDFParticles* pdfParts = &it->second;
                        CPose3DPDFGaussian pdf;
                        pdf.copyFrom(*pdfParts);
 
                        opengl::CSimpleLine::Ptr line =
                                mrpt::make_aligned_shared<opengl::CSimpleLine>();
                        line->setColor(0.8, 0.8, 0.8, 0.3);
                        line->setLineWidth(2);
 
                        line->setLineCoords(
                                pdf.mean.x(), pdf.mean.y(), pdf.mean.z(), areaPnt.x(),
                                areaPnt.y(), areaPnt.z());
                        objs->insert(line);
                }
        }  // end for it
 
        // -------------------------------------------------------------------
        // Draw lines for links between robot poses
        // -------------------------------------------------------------------
        //      for (it=m_robotPoses.begin(); it!=m_robotPoses.end();it++)
        /*      for (it=lstPoses.begin(); it!=lstPoses.end();it++)
                {
                        const CPose3DPDFParticles  *myPdfParts = &it->second;
                        CPose3DPDFGaussian   myPdf;
                        myPdf.copyFrom( *myPdfParts );
 
                        std::map<TPoseID,TInterRobotPosesInfo>::const_iterator itLink;
                        for
           (itLink=it->second.m_links.begin();itLink!=it->second.m_links.end();itLink++)
                        {
                                if (itLink->second.SSO>0.7)
                                {
                                        CRobotPosesAuxiliaryGraph::const_iterator hisIt =
           m_robotPoses.find( itLink->first );
                                        ASSERT_( hisIt !=m_robotPoses.end() );
 
                                        const CPose3DPDFGaussian  *hisPdf = & hisIt->second.m_pose;
 
                                        opengl::CSimpleLine::Ptr line =
           mrpt::make_aligned_shared<opengl::CSimpleLine>();
                                        line->m_color_R = 0.2f;
                                        line->m_color_G = 0.8f;
                                        line->m_color_B = 0.2f;
                                        line->m_color_A = 0.3f;
                                        line->m_lineWidth = 3.0f;
 
                                        line->m_x0 = myPdf->mean.x;
                                        line->m_y0 = myPdf->mean.y;
                                        line->m_z0 = myPdf->mean.z;
 
                                        line->m_x1 = hisPdf->mean.x;
                                        line->m_y1 = hisPdf->mean.y;
                                        line->m_z1 = hisPdf->mean.z;
 
                                        objs->insert( line );
                                }
                        }
                } // end for it
        */
 
        // ---------------------------------------------------------
        // Draw each of the areas in the neighborhood:
        // ---------------------------------------------------------
        {
                std::lock_guard<std::mutex> lock(
                        m_parent->m_map_cs);  // To access nodes' labels.
 
                for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
                         itMeans++)
                {
                        opengl::CSphere::Ptr sphere =
                                mrpt::make_aligned_shared<opengl::CSphere>();
 
                        if (itMeans->first ==
                                m_nodeIDmemberships.find(m_currentRobotPose)->second)
                        {  // Color of current area
                                sphere->setColor(0.1, 0.1, 0.7);
                        }
                        else
                        {  // Color of other areas
                                sphere->setColor(0.7, 0, 0);
                        }
 
                        sphere->setLocation(
                                itMeans->second.x(), itMeans->second.y(),
                                itMeans->second.z() +
                                        m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
 
                        sphere->setRadius(m_parent->m_options.VIEW3D_AREA_SPHERES_RADIUS);
 
                        // Add it:
                        objs->insert(sphere);
 
                        // And text label:
                        opengl::CText::Ptr txt = mrpt::make_aligned_shared<opengl::CText>();
                        txt->setColor(1, 1, 1);
 
                        const CHMHMapNode::Ptr node =
                                m_parent->m_map.getNodeByID(itMeans->first);
                        ASSERT_(node);
 
                        txt->setLocation(
                                itMeans->second.x(), itMeans->second.y(),
                                itMeans->second.z() +
                                        m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
 
                        //                      txt->m_str = node->m_label;
                        txt->setString(format("%li", (long int)node->getID()));
 
                        objs->insert(txt);
                }
        }  // end of lock on map_cs
 
        // ---------------------------------------------------------
        // Draw links between areas:
        // ---------------------------------------------------------
        {
                std::lock_guard<std::mutex> lock(m_parent->m_map_cs);
 
                for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
                         itMeans++)
                {
                        CHMHMapNode::TNodeID srcAreaID = itMeans->first;
                        const CHMHMapNode::Ptr srcArea =
                                m_parent->m_map.getNodeByID(srcAreaID);
                        ASSERT_(srcArea);
 
                        TArcList lstArcs;
                        srcArea->getArcs(lstArcs);
                        for (TArcList::const_iterator a = lstArcs.begin();
                                 a != lstArcs.end(); ++a)
                        {
                                // target is in the neighborhood of LMH:
                                if ((*a)->getNodeFrom() == srcAreaID)
                                {
                                        map<CHMHMapNode::TNodeID, CPoint3D>::const_iterator
                                                trgAreaPoseIt = areas_mean.find((*a)->getNodeTo());
                                        if (trgAreaPoseIt != areas_mean.end())
                                        {
                                                // Yes, target node of the arc is in the LMH: Draw it:
                                                opengl::CSimpleLine::Ptr line =
                                                        mrpt::make_aligned_shared<opengl::CSimpleLine>();
                                                line->setColor(0.8, 0.8, 0);
                                                line->setLineWidth(3);
 
                                                line->setLineCoords(
                                                        areas_mean[srcAreaID].x(),
                                                        areas_mean[srcAreaID].y(),
                                                        areas_mean[srcAreaID].z() +
                                                                m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT,
                                                        trgAreaPoseIt->second.x(),
                                                        trgAreaPoseIt->second.y(),
                                                        trgAreaPoseIt->second.z() +
                                                                m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
 
                                                objs->insert(line);
                                        }
                                }
                        }  // end for each arc
                }  // end for each area
 
        }  // end of lock on map_cs
}
 
/** The PF algorithm implementation.  */
void CLocalMetricHypothesis::prediction_and_update_pfAuxiliaryPFOptimal(
        const mrpt::obs::CActionCollection* action,
        const mrpt::obs::CSensoryFrame* observation,
        const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
        ASSERT_(m_parent.get());
        ASSERT_(m_parent->m_LSLAM_method);
        m_parent->m_LSLAM_method->prediction_and_update_pfAuxiliaryPFOptimal(
                this, action, observation, PF_options);
}
 
void CLocalMetricHypothesis::prediction_and_update_pfOptimalProposal(
        const mrpt::obs::CActionCollection* action,
        const mrpt::obs::CSensoryFrame* observation,
        const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
        ASSERT_(m_parent.get());
        ASSERT_(m_parent->m_LSLAM_method);
        m_parent->m_LSLAM_method->prediction_and_update_pfOptimalProposal(
                this, action, observation, PF_options);
}
 
/*---------------------------------------------------------------
                                        getMeans
   Returns the mean of each robot pose in this LMH, as
         computed from the set of particles.
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::getMeans(TMapPoseID2Pose3D& outList) const
{
        MRPT_START
 
        outList.clear();
 
        // Build list of particles pdfs:
        std::map<TPoseID, CPose3DPDFParticles> parts;
        getPathParticles(parts);
 
        std::map<TPoseID, CPose3DPDFParticles>::iterator it;
 
        for (it = parts.begin(); it != parts.end(); it++)
                it->second.getMean(outList[it->first]);
 
        MRPT_END
}
 
/*---------------------------------------------------------------
                                        getPathParticles
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::getPathParticles(
        std::map<TPoseID, CPose3DPDFParticles>& outList) const
{
        MRPT_START
 
        outList.clear();
 
        if (m_particles.empty()) return;
 
        // For each poseID:
        for (TMapPoseID2Pose3D::const_iterator itPoseID =
                         m_particles.begin()->d->robotPoses.begin();
                 itPoseID != m_particles.begin()->d->robotPoses.end(); ++itPoseID)
        {
                CPose3DPDFParticles auxPDF(m_particles.size());
                CParticleList::const_iterator it;
                CPose3DPDFParticles::CParticleList::iterator itP;
                for (it = m_particles.begin(), itP = auxPDF.m_particles.begin();
                         it != m_particles.end(); it++, itP++)
                {
                        itP->log_w = it->log_w;
                        itP->d = it->d->robotPoses.find(itPoseID->first)->second.asTPose();
                }
 
                // Save PDF:
                outList[itPoseID->first] = auxPDF;
        }  // end for itPoseID
 
        MRPT_END
}
 
/*---------------------------------------------------------------
                                        getPoseParticles
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::getPoseParticles(
        const TPoseID& poseID, CPose3DPDFParticles& outPDF) const
{
        MRPT_START
 
        ASSERT_(!m_particles.empty());
 
        CParticleList::const_iterator it;
        outPDF.resetDeterministic(TPose3D(0, 0, 0, 0, 0, 0), m_particles.size());
        CPose3DPDFParticles::CParticleList::iterator itP;
        for (it = m_particles.begin(), itP = outPDF.m_particles.begin();
                 it != m_particles.end(); it++, itP++)
        {
                itP->log_w = it->log_w;
                TMapPoseID2Pose3D::const_iterator itPose =
                        it->d->robotPoses.find(poseID);
                ASSERT_(itPose != it->d->robotPoses.end());
                itP->d = itPose->second.asTPose();
        }
 
        MRPT_END
}
 
/*---------------------------------------------------------------
                                        clearRobotPoses
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::clearRobotPoses()
{
        clearParticles();
        m_particles.resize(m_parent->m_options.pf_options.sampleSize);
        for (CParticleList::iterator it = m_particles.begin();
                 it != m_particles.end(); ++it)
        {
                // Create particle:
                it->log_w = 0;
                it->d.reset(new CLSLAMParticleData(
                        &m_parent->m_options.defaultMapsInitializers));
 
                // Fill in:
                it->d->robotPoses.clear();
        }
}
 
/*---------------------------------------------------------------
                                                getCurrentPose
 ---------------------------------------------------------------*/
const CPose3D* CLocalMetricHypothesis::getCurrentPose(
        const size_t& particleIdx) const
{
        if (particleIdx >= m_particles.size())
                THROW_EXCEPTION("Particle index out of bounds!");
 
        TMapPoseID2Pose3D::const_iterator it =
                m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
        ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
        return &it->second;
}
 
/*---------------------------------------------------------------
                                                getCurrentPose
 ---------------------------------------------------------------*/
CPose3D* CLocalMetricHypothesis::getCurrentPose(const size_t& particleIdx)
{
        if (particleIdx >= m_particles.size())
                THROW_EXCEPTION("Particle index out of bounds!");
 
        TMapPoseID2Pose3D::iterator it =
                m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
        ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
        return &it->second;
}
 
/*---------------------------------------------------------------
                                                getRelativePose
 ---------------------------------------------------------------*/
void CLocalMetricHypothesis::getRelativePose(
        const TPoseID& reference, const TPoseID& pose,
        CPose3DPDFParticles& outPDF) const
{
        MRPT_START
 
        // Resize output:
        outPDF.resetDeterministic(TPose3D(0, 0, 0, 0, 0, 0), m_particles.size());
 
        CParticleList::const_iterator it;
        CPose3DPDFParticles::CParticleList::iterator itP;
        for (it = m_particles.begin(), itP = outPDF.m_particles.begin();
                 it != m_particles.end(); it++, itP++)
        {
                itP->log_w = it->log_w;
 
                auto srcPose = it->d->robotPoses.find(reference);
                auto trgPose = it->d->robotPoses.find(pose);
 
                ASSERT_(srcPose != it->d->robotPoses.end());
                ASSERT_(trgPose != it->d->robotPoses.end());
                itP->d =
                        (CPose3D(trgPose->second) - CPose3D(srcPose->second)).asTPose();
        }
 
        MRPT_END
}
 
/*---------------------------------------------------------------
                                                changeCoordinateOrigin
 ---------------------------------------------------------------*/
void CLocalMetricHypothesis::changeCoordinateOrigin(const TPoseID& newOrigin)
{
        CPose3DPDFParticles originPDF(m_particles.size());
 
        CParticleList::iterator it;
        CPose3DPDFParticles::CParticleList::iterator itOrgPDF;
 
        for (it = m_particles.begin(), itOrgPDF = originPDF.m_particles.begin();
                 it != m_particles.end(); it++, itOrgPDF++)
        {
                auto refPoseIt = it->d->robotPoses.find(newOrigin);
                ASSERT_(refPoseIt != it->d->robotPoses.end());
                const CPose3D& refPose = refPoseIt->second;
 
                // Save in pdf to compute mean:
                itOrgPDF->d = refPose.asTPose();
                itOrgPDF->log_w = it->log_w;
 
                TMapPoseID2Pose3D::iterator End = it->d->robotPoses.end();
                // Change all other poses first:
                for (TMapPoseID2Pose3D::iterator itP = it->d->robotPoses.begin();
                         itP != End; ++itP)
                        if (itP != refPoseIt) itP->second = itP->second - refPose;
 
                // Now set new origin to 0:
                refPoseIt->second.setFromValues(0, 0, 0);
        }
 
        // Rebuild metric maps for consistency:
        rebuildMetricMaps();
 
        // Change coords in incr. partitioning as well:
        {
                std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
 
                CSimpleMap* SFseq = m_robotPosesGraph.partitioner.getSequenceOfFrames();
                for (std::map<uint32_t, TPoseID>::const_iterator it =
                                 m_robotPosesGraph.idx2pose.begin();
                         it != m_robotPosesGraph.idx2pose.end(); ++it)
                {
                        CPose3DPDF::Ptr pdf;
                        CSensoryFrame::Ptr sf;
                        SFseq->get(it->first, pdf, sf);
 
                        // Copy from particles:
                        ASSERT_(pdf->GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles));
                        CPose3DPDFParticles::Ptr pdfParts =
                                std::dynamic_pointer_cast<CPose3DPDFParticles>(pdf);
                        getPoseParticles(it->second, *pdfParts);
                }
        }
}
 
/*---------------------------------------------------------------
                                                rebuildMetricMaps
 ---------------------------------------------------------------*/
void CLocalMetricHypothesis::rebuildMetricMaps()
{
        for (CParticleList::iterator it = m_particles.begin();
                 it != m_particles.end(); ++it)
        {
                it->d->metricMaps.clear();
 
                // Follow all robot poses:
                TMapPoseID2Pose3D::iterator End = it->d->robotPoses.end();
                for (TMapPoseID2Pose3D::iterator itP = it->d->robotPoses.begin();
                         itP != End; ++itP)
                {
                        if (itP->first !=
                                m_currentRobotPose)  // Current robot pose has no SF stored.
                        {
                                std::map<TPoseID, CSensoryFrame>::const_iterator SFit =
                                        m_SFs.find(itP->first);
                                ASSERT_(SFit != m_SFs.end());
                                SFit->second.insertObservationsInto(
                                        &it->d->metricMaps, &itP->second);
                        }
                }
        }
}
 
/** Removes a given area from the LMH:
 *      - The corresponding node in the HMT map is updated with the robot poses &
 *SFs in the LMH.
 *      - Robot poses belonging to that area are removed from:
 *              - the particles.
 *              - the graph partitioner.
 *              - the list of SFs.
 *              - the list m_nodeIDmemberships.
 *              - The weights of all particles are changed to remove the effects of the
 *removed metric observations.
 *      - After calling this the metric maps should be updated.
 */
void CLocalMetricHypothesis::removeAreaFromLMH(
        const CHMHMapNode::TNodeID areaID)
{
        MRPT_START
 
        // Remove from m_neighbors:
        // -----------------------------------
        TNodeIDSet::iterator itNeig = m_neighbors.find(areaID);
        if (itNeig != m_neighbors.end()) m_neighbors.erase(itNeig);
 
        // Build the list with the poses in the area to be removed from LMH:
        // ----------------------------------------------------------------------
        TNodeIDList lstPoseIDs;
        for (map<TPoseID, CHMHMapNode::TNodeID>::iterator it =
                         m_nodeIDmemberships.begin();
                 it != m_nodeIDmemberships.end(); ++it)
                if (it->second == areaID) lstPoseIDs.insert(it->first);
 
        ASSERT_(!lstPoseIDs.empty());
 
        // ----------------------------------------------------------------------
        // The corresponding node in the HMT map is updated with the
        //   robot poses & SFs in the LMH.
        // ----------------------------------------------------------------------
        updateAreaFromLMH(areaID, true);
 
        // - Robot poses belonging to that area are removed from:
        //      - the particles.
        // ----------------------------------------------------------------------
        for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
                 it != lstPoseIDs.end(); ++it)
                for (CParticleList::iterator p = m_particles.begin();
                         p != m_particles.end(); ++p)
                        p->d->robotPoses.erase(p->d->robotPoses.find(*it));
 
        // - The weights of all particles are changed to remove the effects of the
        // removed metric observations.
        // ----------------------------------------------------------------------
        {
                CParticleList::iterator p;
                vector<map<TPoseID, double>>::iterator ws_it;
                ASSERT_(m_log_w_metric_history.size() == m_particles.size());
 
                for (ws_it = m_log_w_metric_history.begin(), p = m_particles.begin();
                         p != m_particles.end(); ++p, ++ws_it)
                {
                        for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
                                 it != lstPoseIDs.end(); ++it)
                        {
                                map<TPoseID, double>::iterator itW = ws_it->find(*it);
                                if (itW != ws_it->end())
                                {
                                        MRPT_CHECK_NORMAL_NUMBER(itW->second);
 
                                        p->log_w -= itW->second;
                                        // No longer required:
                                        ws_it->erase(itW);
                                }
                        }
                }
        }
 
        // - Robot poses belonging to that area are removed from:
        //      - the graph partitioner.
        // ----------------------------------------------------------------------
        {
                std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
 
                std::vector<uint32_t> indexesToRemove;
                indexesToRemove.reserve(lstPoseIDs.size());
 
                for (std::map<uint32_t, TPoseID>::iterator it =
                                 m_robotPosesGraph.idx2pose.begin();
                         it != m_robotPosesGraph.idx2pose.end();)
                {
                        if (lstPoseIDs.find(it->second) != lstPoseIDs.end())
                        {
                                indexesToRemove.push_back(it->first);
 
                                // Remove from the mapping indexes->nodeIDs as well:
                                std::map<uint32_t, TPoseID>::iterator it2 = it;
                                it2++;
                                // it = m_robotPosesGraph.idx2pose.erase( it );
                                m_robotPosesGraph.idx2pose.erase(it);
                                it = it2;
                        }
                        else
                                it++;
                }
 
                m_robotPosesGraph.partitioner.removeSetOfNodes(indexesToRemove);
 
                // Renumbering of indexes<->posesIDs to be the same than in
                // "m_robotPosesGraph.partitioner":
                unsigned idx = 0;
                map<uint32_t, TPoseID> newList;
                for (map<uint32_t, TPoseID>::iterator i =
                                 m_robotPosesGraph.idx2pose.begin();
                         i != m_robotPosesGraph.idx2pose.end(); ++i, idx++)
                        newList[idx] = i->second;
                m_robotPosesGraph.idx2pose = newList;
        }
 
        // - Robot poses belonging to that area are removed from:
        // - the list of SFs.
        // ----------------------------------------------------------------------
        // Already done above.
 
        // - Robot poses belonging to that area are removed from:
        // - the list m_nodeIDmemberships.
        // ----------------------------------------------------------------------
        for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
                 it != lstPoseIDs.end(); ++it)
                m_nodeIDmemberships.erase(m_nodeIDmemberships.find(*it));
 
        double out_max_log_w;
        normalizeWeights(&out_max_log_w);  // Normalize weights:
 
        MRPT_END
}
 
/*---------------------------------------------------------------
                                        TRobotPosesPartitioning::pose2idx
 ---------------------------------------------------------------*/
unsigned int CLocalMetricHypothesis::TRobotPosesPartitioning::pose2idx(
        const TPoseID& id) const
{
        for (std::map<uint32_t, TPoseID>::const_iterator it = idx2pose.begin();
                 it != idx2pose.end(); ++it)
                if (it->second == id) return it->first;
        THROW_EXCEPTION_FMT("PoseID=%i not found.", static_cast<int>(id));
}
 
/*---------------------------------------------------------------
                                        updateAreaFromLMH
 
// The corresponding node in the HMT map is updated with the
//   robot poses & SFs in the LMH.
 ---------------------------------------------------------------*/
void CLocalMetricHypothesis::updateAreaFromLMH(
        const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH)
{
        // Build the list with the poses belonging to that area from LMH:
        // ----------------------------------------------------------------------
        TNodeIDList lstPoseIDs;
        for (map<TPoseID, CHMHMapNode::TNodeID>::const_iterator it =
                         m_nodeIDmemberships.begin();
                 it != m_nodeIDmemberships.end(); ++it)
                if (it->second == areaID) lstPoseIDs.insert(it->first);
 
        ASSERT_(!lstPoseIDs.empty());
 
        CHMHMapNode::Ptr node;
        {
                std::lock_guard<std::mutex>(m_parent->m_map_cs);
                node = m_parent->m_map.getNodeByID(areaID);
                ASSERT_(node);
                ASSERT_(node->m_hypotheses.has(m_ID));
        }  // end of HMT map cs
 
        // The pose to become the origin:
        TPoseID poseID_origin;
        node->m_annotations.getElemental(
                NODE_ANNOTATION_REF_POSEID, poseID_origin, m_ID, true);
 
        // 1) The set of robot poses and SFs
        //    In annotation:                                    NODE_ANNOTATION_POSES_GRAPH
        // ---------------------------------------------------------------------
        CRobotPosesGraph::Ptr posesGraph;
        {
                CSerializable::Ptr annot =
                        node->m_annotations.get(NODE_ANNOTATION_POSES_GRAPH, m_ID);
                if (!annot)
                {
                        // Add it now:
                        posesGraph = mrpt::make_aligned_shared<CRobotPosesGraph>();
                        node->m_annotations.setMemoryReference(
                                NODE_ANNOTATION_POSES_GRAPH, posesGraph, m_ID);
                }
                else
                {
                        posesGraph = std::dynamic_pointer_cast<CRobotPosesGraph>(annot);
                        posesGraph->clear();
                }
        }
 
        // For each pose in the area:
        CPose3DPDFParticles pdfOrigin;
        bool pdfOrigin_ok = false;
        for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
                 it != lstPoseIDs.end(); ++it)
        {
                TPoseInfo& poseInfo = (*posesGraph)[*it];
                getPoseParticles(*it, poseInfo.pdf);  // Save pose particles
 
                // Save the PDF of the origin:
                if (*it == poseID_origin)
                {
                        pdfOrigin.copyFrom(poseInfo.pdf);
                        pdfOrigin_ok = true;
                }
 
                if (*it != m_currentRobotPose)  // The current robot pose has no SF
                {
                        std::map<TPoseID, CSensoryFrame>::iterator itSF = m_SFs.find(*it);
                        ASSERT_(itSF != m_SFs.end());
 
                        if (eraseSFsFromLMH)
                        {
                                poseInfo.sf.moveFrom(itSF->second);  // This leaves m_SFs[*it]
                                // without observations,
                                // but it is being erased
                                // just now:
                                m_SFs.erase(itSF);
                        }
                        else
                        {
                                poseInfo.sf = itSF->second;  // Copy observations
                        }
                }
        }
 
        // Readjust to set the origin pose ID:
        ASSERT_(pdfOrigin_ok);
        CPose3DPDFParticles pdfOriginInv;
        pdfOrigin.inverse(pdfOriginInv);
        for (CRobotPosesGraph::iterator it = posesGraph->begin();
                 it != posesGraph->end(); ++it)
        {
                CPose3DPDFParticles::CParticleList::iterator orgIt, pdfIt;
                ASSERT_(it->second.pdf.size() == pdfOriginInv.size());
                for (pdfIt = it->second.pdf.m_particles.begin(),
                        orgIt = pdfOriginInv.m_particles.begin();
                         orgIt != pdfOriginInv.m_particles.end(); orgIt++, pdfIt++)
                        pdfIt->d = (CPose3D(orgIt->d) + CPose3D(pdfIt->d)).asTPose();
        }
 
        // 2) One single metric map built from the most likelily robot poses
        //    In annotation:                                    NODE_ANNOTATION_METRIC_MAPS
        // ---------------------------------------------------------------------
        CMultiMetricMap::Ptr metricMap = node->m_annotations.getAs<CMultiMetricMap>(
                NODE_ANNOTATION_METRIC_MAPS, m_ID, false);
        metricMap->clear();
        posesGraph->insertIntoMetricMap(*metricMap);
}
 
/*---------------------------------------------------------------
                                dumpAsText
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::dumpAsText(std::vector<std::string>& st) const
{
        st.clear();
        st.push_back("LIST OF POSES IN LMH");
        st.push_back("====================");
 
        string s;
        s = "Neighbors: ";
        for (TNodeIDSet::const_iterator it = m_neighbors.begin();
                 it != m_neighbors.end(); ++it)
                s += format("%i ", (int)*it);
        st.push_back(s);
 
        TMapPoseID2Pose3D lst;
        getMeans(lst);
 
        for (TMapPoseID2Pose3D::const_iterator it = lst.begin(); it != lst.end();
                 ++it)
        {
                map<TPoseID, CHMHMapNode::TNodeID>::const_iterator area =
                        m_nodeIDmemberships.find(it->first);
 
                string s = format(
                        "  ID: %i \t AREA: %i \t %.03f,%.03f,%.03fdeg", (int)it->first,
                        (int)area->second, it->second.x(), it->second.y(),
                        RAD2DEG(it->second.yaw()));
                st.push_back(s);
        }
}
 
/*---------------------------------------------------------------
                                        readFromStream
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::serializeFrom(
        mrpt::serialization::CArchive& in, uint8_t version)
{
        switch (version)
        {
                case 0:
                {
                        in >> m_ID >> m_currentRobotPose >> m_neighbors >>
                                m_nodeIDmemberships >> m_SFs >> m_posesPendingAddPartitioner >>
                                m_areasPendingTBI >> m_log_w >> m_log_w_metric_history >>
                                m_robotPosesGraph.partitioner >> m_robotPosesGraph.idx2pose;
 
                        // particles:
                        readParticlesFromStream(in);
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}
 
uint8_t CLocalMetricHypothesis::serializeGetVersion() const { return 0; }
void CLocalMetricHypothesis::serializeTo(
        mrpt::serialization::CArchive& out) const
{
        out << m_ID << m_currentRobotPose << m_neighbors << m_nodeIDmemberships
                << m_SFs << m_posesPendingAddPartitioner << m_areasPendingTBI << m_log_w
                << m_log_w_metric_history << m_robotPosesGraph.partitioner
                << m_robotPosesGraph.idx2pose;
 
        // particles:
        writeParticlesToStream(out);
}
 
void CLSLAMParticleData::serializeFrom(
        mrpt::serialization::CArchive& in, uint8_t version)
{
        switch (version)
        {
                case 0:
                {
                        in >> metricMaps >> robotPoses;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}
 
uint8_t CLSLAMParticleData::serializeGetVersion() const { return 0; }
void CLSLAMParticleData::serializeTo(mrpt::serialization::CArchive& out) const
{
        out << metricMaps << robotPoses;
}