Main MRPT website > C++ reference for MRPT 1.5.7
CHMTSLAM_perform_TLC.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 
11 #include "hmtslam-precomp.h" // Precomp header
12 
14 #include <mrpt/utils/CTicTac.h>
15 #include <mrpt/random.h>
16 #include <mrpt/utils/CFileStream.h>
18 #include <mrpt/system/os.h>
19 
20 using namespace mrpt::slam;
21 using namespace mrpt::hmtslam;
22 using namespace mrpt::utils;
23 using namespace mrpt::random;
24 using namespace mrpt::poses;
25 using namespace std;
26 
27 /*---------------------------------------------------------------
28 
29  perform_TLC
30 
31  Topological Loop Closure: Performs all the required operations
32  to close a loop between two areas which have been determined
33  to be the same.
34 
35  ---------------------------------------------------------------*/
36 void CHMTSLAM::perform_TLC(
38  const CHMHMapNode::TNodeID Ai, // areaInLMH,
39  const CHMHMapNode::TNodeID Ae, // areaLoopClosure,
40  const mrpt::poses::CPose3DPDFGaussian &pose_i_wrt_e
41  )
42 {
44  ASSERT_(Ai!=Ae)
45 
47 
48  logFmt(mrpt::utils::LVL_DEBUG, "[perform_TLC] TLC of areas: %u <-> %u \n",(unsigned)Ai, (unsigned)Ae );
49 
50  // * Verify a1 \in LMH & a2 \notin LMH
51  // ----------------------------------------------------------------------
52  ASSERT_( LMH.m_neighbors.count(Ai) == 1);
53  ASSERT_( LMH.m_neighbors.count(Ae) == 0);
54 
55  // * Get the old pose reference of the area which will desapear:
56  // ----------------------------------------------------------------------
57  TPoseID poseID_Ai_origin;
58  m_map.getNodeByID(Ai)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_Ai_origin, LMH.m_ID, true);
59 // TPoseID poseID_new_origin;
60 // m_map.getNodeByID(Ae)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_new_origin, LMH.m_ID, true);
61 
62  // * Make a list Li of pose IDs in Ai, which need a change in coordinates:
63  // ----------------------------------------------------------------------------------------------
64  //TPoseIDSet Li;
65  //for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
66  // if(it->second==Ai)
67  // Li.insert(it->first);
68 
69  // * Change all poses in "Ai" from "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"
70  // ----------------------------------------------------------------------------------------------
71  // For all the particles
72  //for ( CLocalMetricHypothesis::CParticleList::const_iterator it = LMH.m_particles.begin(); it!=LMH.m_particles.end(); ++it)
73  //{
74  // // Touch only the affected poseIDs:
75  // for (TPoseIDSet::const_iterator n=Li.begin();n!=Li.end();++n)
76  // {
77  // map<TPoseID,CPose3D>::iterator itPos = it->d->robotPoses.find(*n);
78  // ASSERT_(itPos!=it->d->robotPoses.end());
79  // itPos->second = Apose_ei + (itPos->second - oldRef);
80  // }
81  //}
82 
83  // Change coords in incr. partitioning as well:
84  //CSimpleMap *SFseq = LMH.m_robotPosesGraph.partitioner.getSequenceOfFrames();
85  //for (std::map<uint32_t,TPoseID>::const_iterator it=LMH.m_robotPosesGraph.idx2pose.begin();it!=LMH.m_robotPosesGraph.idx2pose.end();++it)
86  //{
87  // // Only if the pose has been affected:
88  // if (Li.count(it->second)==0) continue;
89 
90  // CPose3DPDFPtr pdf;
91  // CSensoryFramePtr sf;
92  // SFseq->get( it->first, pdf, sf);
93 
94  // // Copy from particles:
95  // CPose3DPDFParticlesPtr pdfParts = CPose3DPDFParticlesPtr(pdf);
96  // LMH.getPoseParticles( it->second, *pdfParts);
97  //}
98 
99  // * Mark all poses in LMH \in Ai as being of "Ae":
100  // ----------------------------------------------------------------------------------------------
102  if(it->second==Ai)
103  it->second=Ae;
104 
105  // * Replace "Ai" by "Ae" in the list of neighbors areas:
106  // ----------------------------------------------------------------------------------------------
107  LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
108  LMH.m_neighbors.insert(Ae);
109 
110 
111  // * Reflect the change of coordinates in all existing arcs from/to "Ai", which are not part of the LMH (list of neighbors)
112  // "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
113  // NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
114  // ----------------------------------------------------------------------------------------------
115  {
116  TArcList lstArcs;
117  m_map.getNodeByID(Ai)->getArcs(lstArcs,LMH.m_ID);
118  for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
119  {
120  // TODO ...
121  }
122  }
123 
124  // * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
125  // ----------------------------------------------------------------------------------------------
126  {
127  TArcList lstArcs;
128  m_map.getNodeByID(Ai)->getArcs(lstArcs);
129  for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
130  arc->clear(); // The "delete" will automatically remove the entry in "m_map". Other smrtpnts will be cleared too.
131 
132  m_map.getNodeByID(Ai).clear(); // The "delete" will automatically remove the entry in "m_map"
133  }
134 
135 
136  // * Merge Ae into the LMH: This includes the change of coordinates to those used now in "Ai" and the rest of neighbors!
137  // ----------------------------------------------------------------------------------------------
138  CPose3DPDFParticles pdfAiRef;
139  LMH.getPoseParticles(poseID_Ai_origin,pdfAiRef);
140  CPose3D AiRef;
141  pdfAiRef.getMean(AiRef);
142  const CPose3D &Apose_ie = pose_i_wrt_e.mean;
143  //const CPose3D Apose_ei = -Apose_ie;
144  const CPose3D AeRefInLMH = AiRef + Apose_ie;
145 
146  CRobotPosesGraphPtr AePG = m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH, LMH.m_ID);
147 
148  // For each pose in Ae:
149  for (CRobotPosesGraph::const_iterator it=AePG->begin();it!=AePG->end();++it)
150  {
151  const TPoseID poseId = it->first;
152  const TPoseInfo &pi = it->second;
153 
154  // Insert the observations into the metric maps:
155  ASSERT_(LMH.m_particles.size() == pi.pdf.size() );
156 
157  for (size_t i=0;i<pi.pdf.size();i++)
158  {
159  // Transport coordinates:
160  const CPose3D &p = *pi.pdf.m_particles[i].d;
161  LMH.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
162  //pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps, pi.pdf.m_particles[i].d );
163  }
164 
165  // Add node membership:
166  LMH.m_nodeIDmemberships[poseId] = Ae;
167 
168  // Now, insert the SF
169  LMH.m_SFs[poseId] = pi.sf;
170 
171  // Also add to the list of poses pending to be evaluated by the AA:
172  LMH.m_posesPendingAddPartitioner.push_back(poseId);
173  }
174 
175  // * The current pose of the robot should be in the new coordinate system:
176  // ----------------------------------------------------------------------------------------------
177  // This is already done since the current pose is just another poseID and it's threaded like the others.
178 
179 
180  // * Rebuild metric maps for consistency:
181  // ----------------------------------------------------------------------------------------------
182  LMH.rebuildMetricMaps();
183 
184 
185  // * Update all the data in the node "Ae":
186  // ----------------------------------------------------------------------------------------------
187  LMH.updateAreaFromLMH(Ae);
188 
189 
190 
191  MRPT_END
192 }
A namespace of pseudo-random numbers genrators of diferent distributions.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose3D mean
The mean value.
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
Scalar * iterator
Definition: eigen_plugins.h:23
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
CParticleList m_particles
The array of particles.
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
#define MRPT_END
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
#define MRPT_START
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
void rebuildMetricMaps()
Rebuild the metric maps of all particles from the observations and their estimated poses...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::obs::CSensoryFrame sf
The observations.
#define ASSERT_(f)
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Information kept for each robot pose used in CRobotPosesGraph.
void updateAreaFromLMH(const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH=false)
The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are ref...
synch::CCriticalSection lock
CS to access the entire struct.
#define NODE_ANNOTATION_POSES_GRAPH
GLfloat GLfloat p
Definition: glext.h:5587
A class for storing a sequence of arcs (a path).
void getPoseParticles(const TPoseID &poseID, mrpt::poses::CPose3DPDFParticles &outPDF) const
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles...
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019