CHMTSLAM_perform_TLC.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 "hmtslam-precomp.h"  // Precomp header

#include <mrpt/hmtslam/CRobotPosesGraph.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/random.h>
#include <mrpt/utils/CFileStream.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/system/os.h>

using namespace mrpt::slam;
using namespace mrpt::hmtslam;
using namespace mrpt::utils;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace std;

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

                                                perform_TLC

        Topological Loop Closure: Performs all the required operations
        to close a loop between two areas which have been determined
        to be the same.

  ---------------------------------------------------------------*/
void CHMTSLAM::perform_TLC(
        CLocalMetricHypothesis& LMH,
        const CHMHMapNode::TNodeID Ai,  // areaInLMH,
        const CHMHMapNode::TNodeID Ae,  // areaLoopClosure,
        const mrpt::poses::CPose3DPDFGaussian& pose_i_wrt_e)
{
        MRPT_START
        ASSERT_(Ai != Ae)

        std::lock_guard<std::mutex> locker(LMH.m_robotPosesGraph.lock);

        logFmt(
                mrpt::utils::LVL_DEBUG, "[perform_TLC] TLC of areas: %u <-> %u \n",
                (unsigned)Ai, (unsigned)Ae);

        // * Verify a1 \in LMH & a2 \notin LMH
        // ----------------------------------------------------------------------
        ASSERT_(LMH.m_neighbors.count(Ai) == 1);
        ASSERT_(LMH.m_neighbors.count(Ae) == 0);

        // * Get the old pose reference of the area which will desapear:
        // ----------------------------------------------------------------------
        TPoseID poseID_Ai_origin;
        m_map.getNodeByID(Ai)->m_annotations.getElemental(
                NODE_ANNOTATION_REF_POSEID, poseID_Ai_origin, LMH.m_ID, true);
        //      TPoseID         poseID_new_origin;
        //      m_map.getNodeByID(Ae)->m_annotations.getElemental(
        // NODE_ANNOTATION_REF_POSEID,poseID_new_origin, LMH.m_ID, true);

        // * Make a list Li of pose IDs in Ai, which need a change in coordinates:
        // ----------------------------------------------------------------------------------------------
        // TPoseIDSet  Li;
        // for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator
        // it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
        //      if(it->second==Ai)
        //              Li.insert(it->first);

        // * Change all poses in "Ai" from "p" to " 0 (-) pose1wrt2 (+) (p (-)
        // oldRef )"
        // ----------------------------------------------------------------------------------------------
        // For all the particles
        // for ( CLocalMetricHypothesis::CParticleList::const_iterator it =
        // LMH.m_particles.begin(); it!=LMH.m_particles.end(); ++it)
        //{
        //      // Touch only the affected poseIDs:
        //      for (TPoseIDSet::const_iterator n=Li.begin();n!=Li.end();++n)
        //      {
        //              map<TPoseID,CPose3D>::iterator itPos = it->d->robotPoses.find(*n);
        //              ASSERT_(itPos!=it->d->robotPoses.end());
        //              itPos->second = Apose_ei + (itPos->second - oldRef);
        //      }
        //}

        // Change coords in incr. partitioning as well:
        // CSimpleMap *SFseq =
        // LMH.m_robotPosesGraph.partitioner.getSequenceOfFrames();
        // for (std::map<uint32_t,TPoseID>::const_iterator
        // it=LMH.m_robotPosesGraph.idx2pose.begin();it!=LMH.m_robotPosesGraph.idx2pose.end();++it)
        //{
        //      // Only if the pose has been affected:
        //      if (Li.count(it->second)==0) continue;

        //      CPose3DPDF::Ptr         pdf;
        //      CSensoryFrame::Ptr      sf;
        //      SFseq->get( it->first, pdf, sf);

        //      // Copy from particles:
        //      CPose3DPDFParticles::Ptr pdfParts = CPose3DPDFParticles::Ptr(pdf);
        //      LMH.getPoseParticles( it->second, *pdfParts);
        //}

        // * Mark all poses in LMH \in Ai as being of "Ae":
        // ----------------------------------------------------------------------------------------------
        for (map<TPoseID, CHMHMapNode::TNodeID>::iterator it =
                         LMH.m_nodeIDmemberships.begin();
                 it != LMH.m_nodeIDmemberships.end(); ++it)
                if (it->second == Ai) it->second = Ae;

        // * Replace "Ai" by "Ae" in the list of neighbors areas:
        // ----------------------------------------------------------------------------------------------
        LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
        LMH.m_neighbors.insert(Ae);

        // * Reflect the change of coordinates in all existing arcs from/to "Ai",
        // which are not part of the LMH (list of neighbors)
        //     "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
        //  NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
        // ----------------------------------------------------------------------------------------------
        {
                TArcList lstArcs;
                m_map.getNodeByID(Ai)->getArcs(lstArcs, LMH.m_ID);
                for (TArcList::iterator arc = lstArcs.begin(); arc != lstArcs.end();
                         ++arc)
                {
                        // TODO ...
                }
        }

        // * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
        // ----------------------------------------------------------------------------------------------
        {
                TArcList lstArcs;
                m_map.getNodeByID(Ai)->getArcs(lstArcs);
                for (TArcList::iterator arc = lstArcs.begin(); arc != lstArcs.end();
                         ++arc)
                        arc->reset();  // The "delete" will automatically remove the entry
                // in "m_map". Other smrtpnts will be cleared too.

                m_map.getNodeByID(Ai).reset();  // The "delete" will automatically
                // remove the entry in "m_map"
        }

        // * Merge Ae into the LMH: This includes the change of coordinates to those
        // used now in "Ai" and the rest of neighbors!
        // ----------------------------------------------------------------------------------------------
        CPose3DPDFParticles pdfAiRef;
        LMH.getPoseParticles(poseID_Ai_origin, pdfAiRef);
        CPose3D AiRef;
        pdfAiRef.getMean(AiRef);
        const CPose3D& Apose_ie = pose_i_wrt_e.mean;
        // const CPose3D  Apose_ei = -Apose_ie;
        const CPose3D AeRefInLMH = AiRef + Apose_ie;

        CRobotPosesGraph::Ptr AePG =
                m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(
                        NODE_ANNOTATION_POSES_GRAPH, LMH.m_ID);

        // For each pose in Ae:
        for (CRobotPosesGraph::const_iterator it = AePG->begin(); it != AePG->end();
                 ++it)
        {
                const TPoseID poseId = it->first;
                const TPoseInfo& pi = it->second;

                //  Insert the observations into the metric maps:
                ASSERT_(LMH.m_poseParticles.m_particles.size() == pi.pdf.size());

                for (size_t i = 0; i < pi.pdf.size(); i++)
                {
                        // Transport coordinates:
                        const CPose3D& p = *pi.pdf.m_particles[i].d;
                        LMH.m_poseParticles.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
                        // pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps,
                        // pi.pdf.m_particles[i].d );
                }

                // Add node membership:
                LMH.m_nodeIDmemberships[poseId] = Ae;

                // Now, insert the SF
                LMH.m_SFs[poseId] = pi.sf;

                // Also add to the list of poses pending to be evaluated by the AA:
                LMH.m_posesPendingAddPartitioner.push_back(poseId);
        }

        // * The current pose of the robot should be in the new coordinate system:
        // ----------------------------------------------------------------------------------------------
        //  This is already done since the current pose is just another poseID and
        //  it's threaded like the others.

        // * Rebuild metric maps for consistency:
        // ----------------------------------------------------------------------------------------------
        LMH.rebuildMetricMaps();

        // * Update all the data in the node "Ae":
        // ----------------------------------------------------------------------------------------------
        LMH.updateAreaFromLMH(Ae);

        MRPT_END
}