Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #include "hmtslam-precomp.h" // Precomp header
11 
13 #include <mrpt/system/CTicTac.h>
14 #include <mrpt/random.h>
15 #include <mrpt/io/CFileStream.h>
17 #include <mrpt/system/os.h>
18 
19 using namespace mrpt::slam;
20 using namespace mrpt::hmtslam;
21 using namespace mrpt::random;
22 using namespace mrpt::poses;
23 using namespace std;
24 
25 /*---------------------------------------------------------------
26 
27  perform_TLC
28 
29  Topological Loop Closure: Performs all the required operations
30  to close a loop between two areas which have been determined
31  to be the same.
32 
33  ---------------------------------------------------------------*/
34 void CHMTSLAM::perform_TLC(
36  const CHMHMapNode::TNodeID Ai, // areaInLMH,
37  const CHMHMapNode::TNodeID Ae, // areaLoopClosure,
38  const mrpt::poses::CPose3DPDFGaussian& pose_i_wrt_e)
39 {
41  ASSERT_(Ai != Ae);
42 
43  std::lock_guard<std::mutex> locker(LMH.m_robotPosesGraph.lock);
44 
45  logFmt(
46  mrpt::system::LVL_DEBUG, "[perform_TLC] TLC of areas: %u <-> %u \n",
47  (unsigned)Ai, (unsigned)Ae);
48 
49  // * Verify a1 \in LMH & a2 \notin LMH
50  // ----------------------------------------------------------------------
51  ASSERT_(LMH.m_neighbors.count(Ai) == 1);
52  ASSERT_(LMH.m_neighbors.count(Ae) == 0);
53 
54  // * Get the old pose reference of the area which will desapear:
55  // ----------------------------------------------------------------------
56  TPoseID poseID_Ai_origin;
57  m_map.getNodeByID(Ai)->m_annotations.getElemental(
58  NODE_ANNOTATION_REF_POSEID, poseID_Ai_origin, LMH.m_ID, true);
59  // TPoseID poseID_new_origin;
60  // m_map.getNodeByID(Ae)->m_annotations.getElemental(
61  // NODE_ANNOTATION_REF_POSEID,poseID_new_origin, LMH.m_ID, true);
62 
63  // * Make a list Li of pose IDs in Ai, which need a change in coordinates:
64  // ----------------------------------------------------------------------------------------------
65  // TPoseIDSet Li;
66  // for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator
67  // it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
68  // if(it->second==Ai)
69  // Li.insert(it->first);
70 
71  // * Change all poses in "Ai" from "p" to " 0 (-) pose1wrt2 (+) (p (-)
72  // oldRef )"
73  // ----------------------------------------------------------------------------------------------
74  // For all the particles
75  // for ( CLocalMetricHypothesis::CParticleList::const_iterator it =
76  // LMH.m_particles.begin(); it!=LMH.m_particles.end(); ++it)
77  //{
78  // // Touch only the affected poseIDs:
79  // for (TPoseIDSet::const_iterator n=Li.begin();n!=Li.end();++n)
80  // {
81  // map<TPoseID,CPose3D>::iterator itPos = it->d->robotPoses.find(*n);
82  // ASSERT_(itPos!=it->d->robotPoses.end());
83  // itPos->second = Apose_ei + (itPos->second - oldRef);
84  // }
85  //}
86 
87  // Change coords in incr. partitioning as well:
88  // CSimpleMap *SFseq =
89  // LMH.m_robotPosesGraph.partitioner.getSequenceOfFrames();
90  // for (std::map<uint32_t,TPoseID>::const_iterator
91  // it=LMH.m_robotPosesGraph.idx2pose.begin();it!=LMH.m_robotPosesGraph.idx2pose.end();++it)
92  //{
93  // // Only if the pose has been affected:
94  // if (Li.count(it->second)==0) continue;
95 
96  // CPose3DPDF::Ptr pdf;
97  // CSensoryFrame::Ptr sf;
98  // SFseq->get( it->first, pdf, sf);
99 
100  // // Copy from particles:
101  // CPose3DPDFParticles::Ptr pdfParts = CPose3DPDFParticles::Ptr(pdf);
102  // LMH.getPoseParticles( it->second, *pdfParts);
103  //}
104 
105  // * Mark all poses in LMH \in Ai as being of "Ae":
106  // ----------------------------------------------------------------------------------------------
108  LMH.m_nodeIDmemberships.begin();
109  it != LMH.m_nodeIDmemberships.end(); ++it)
110  if (it->second == Ai) it->second = Ae;
111 
112  // * Replace "Ai" by "Ae" in the list of neighbors areas:
113  // ----------------------------------------------------------------------------------------------
114  LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
115  LMH.m_neighbors.insert(Ae);
116 
117  // * Reflect the change of coordinates in all existing arcs from/to "Ai",
118  // which are not part of the LMH (list of neighbors)
119  // "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
120  // NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
121  // ----------------------------------------------------------------------------------------------
122  {
123  TArcList lstArcs;
124  m_map.getNodeByID(Ai)->getArcs(lstArcs, LMH.m_ID);
125  for (TArcList::iterator arc = lstArcs.begin(); arc != lstArcs.end();
126  ++arc)
127  {
128  // TODO ...
129  }
130  }
131 
132  // * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
133  // ----------------------------------------------------------------------------------------------
134  {
135  TArcList lstArcs;
136  m_map.getNodeByID(Ai)->getArcs(lstArcs);
137  for (TArcList::iterator arc = lstArcs.begin(); arc != lstArcs.end();
138  ++arc)
139  arc->reset(); // The "delete" will automatically remove the entry
140  // in "m_map". Other smrtpnts will be cleared too.
141 
142  m_map.getNodeByID(Ai).reset(); // The "delete" will automatically
143  // remove the entry in "m_map"
144  }
145 
146  // * Merge Ae into the LMH: This includes the change of coordinates to those
147  // used now in "Ai" and the rest of neighbors!
148  // ----------------------------------------------------------------------------------------------
149  CPose3DPDFParticles pdfAiRef;
150  LMH.getPoseParticles(poseID_Ai_origin, pdfAiRef);
151  CPose3D AiRef;
152  pdfAiRef.getMean(AiRef);
153  const CPose3D& Apose_ie = pose_i_wrt_e.mean;
154  // const CPose3D Apose_ei = -Apose_ie;
155  const CPose3D AeRefInLMH = AiRef + Apose_ie;
156 
157  CRobotPosesGraph::Ptr AePG =
158  m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(
160 
161  // For each pose in Ae:
162  for (CRobotPosesGraph::const_iterator it = AePG->begin(); it != AePG->end();
163  ++it)
164  {
165  const TPoseID poseId = it->first;
166  const TPoseInfo& pi = it->second;
167 
168  // Insert the observations into the metric maps:
169  ASSERT_(LMH.m_particles.size() == pi.pdf.size());
170 
171  for (size_t i = 0; i < pi.pdf.size(); i++)
172  {
173  // Transport coordinates:
174  const CPose3D p = CPose3D(pi.pdf.m_particles[i].d);
175  LMH.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
176  // pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps,
177  // pi.pdf.m_particles[i].d );
178  }
179 
180  // Add node membership:
181  LMH.m_nodeIDmemberships[poseId] = Ae;
182 
183  // Now, insert the SF
184  LMH.m_SFs[poseId] = pi.sf;
185 
186  // Also add to the list of poses pending to be evaluated by the AA:
187  LMH.m_posesPendingAddPartitioner.push_back(poseId);
188  }
189 
190  // * The current pose of the robot should be in the new coordinate system:
191  // ----------------------------------------------------------------------------------------------
192  // This is already done since the current pose is just another poseID and
193  // it's threaded like the others.
194 
195  // * Rebuild metric maps for consistency:
196  // ----------------------------------------------------------------------------------------------
197  LMH.rebuildMetricMaps();
198 
199  // * Update all the data in the node "Ae":
200  // ----------------------------------------------------------------------------------------------
201  LMH.updateAreaFromLMH(Ae);
202 
203  MRPT_END
204 }
A namespace of pseudo-random numbers generators of diferent distributions.
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
CPose3D mean
The mean value.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:47
CParticleList m_particles
The array of particles.
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
STL namespace.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
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...
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
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:88
#define MRPT_END
Definition: exceptions.h:266
mrpt::obs::CSensoryFrame sf
The observations.
#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...
#define NODE_ANNOTATION_POSES_GRAPH
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019