MRPT  1.9.9
CHMTSLAM_perform_TLC.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hmtslam-precomp.h" // Precomp header
11 
13 #include <mrpt/io/CFileStream.h>
15 #include <mrpt/random.h>
16 #include <mrpt/system/CTicTac.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  // ----------------------------------------------------------------------------------------------
107  for (auto& m_nodeIDmembership : LMH.m_nodeIDmemberships)
108  if (m_nodeIDmembership.second == Ai) m_nodeIDmembership.second = Ae;
109 
110  // * Replace "Ai" by "Ae" in the list of neighbors areas:
111  // ----------------------------------------------------------------------------------------------
112  LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
113  LMH.m_neighbors.insert(Ae);
114 
115  // * Reflect the change of coordinates in all existing arcs from/to "Ai",
116  // which are not part of the LMH (list of neighbors)
117  // "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
118  // NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
119  // ----------------------------------------------------------------------------------------------
120  {
121  TArcList lstArcs;
122  m_map.getNodeByID(Ai)->getArcs(lstArcs, LMH.m_ID);
123  for (auto arc = lstArcs.begin(); arc != lstArcs.end(); ++arc)
124  {
125  // TODO ...
126  }
127  }
128 
129  // * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
130  // ----------------------------------------------------------------------------------------------
131  {
132  TArcList lstArcs;
133  m_map.getNodeByID(Ai)->getArcs(lstArcs);
134  for (auto& lstArc : lstArcs)
135  lstArc.reset(); // The "delete" will automatically remove the entry
136  // in "m_map". Other smrtpnts will be cleared too.
137 
138  m_map.getNodeByID(Ai).reset(); // The "delete" will automatically
139  // remove the entry in "m_map"
140  }
141 
142  // * Merge Ae into the LMH: This includes the change of coordinates to those
143  // used now in "Ai" and the rest of neighbors!
144  // ----------------------------------------------------------------------------------------------
145  CPose3DPDFParticles pdfAiRef;
146  LMH.getPoseParticles(poseID_Ai_origin, pdfAiRef);
147  CPose3D AiRef;
148  pdfAiRef.getMean(AiRef);
149  const CPose3D& Apose_ie = pose_i_wrt_e.mean;
150  // const CPose3D Apose_ei = -Apose_ie;
151  const CPose3D AeRefInLMH = AiRef + Apose_ie;
152 
153  CRobotPosesGraph::Ptr AePG =
154  m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(
156 
157  // For each pose in Ae:
158  for (auto it = AePG->begin(); it != AePG->end(); ++it)
159  {
160  const TPoseID poseId = it->first;
161  const TPoseInfo& pi = it->second;
162 
163  // Insert the observations into the metric maps:
164  ASSERT_(LMH.m_particles.size() == pi.pdf.size());
165 
166  for (size_t i = 0; i < pi.pdf.size(); i++)
167  {
168  // Transport coordinates:
169  const CPose3D p = CPose3D(pi.pdf.m_particles[i].d);
170  LMH.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
171  // pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps,
172  // pi.pdf.m_particles[i].d );
173  }
174 
175  // Add node membership:
176  LMH.m_nodeIDmemberships[poseId] = Ae;
177 
178  // Now, insert the SF
179  LMH.m_SFs[poseId] = pi.sf;
180 
181  // Also add to the list of poses pending to be evaluated by the AA:
182  LMH.m_posesPendingAddPartitioner.push_back(poseId);
183  }
184 
185  // * The current pose of the robot should be in the new coordinate system:
186  // ----------------------------------------------------------------------------------------------
187  // This is already done since the current pose is just another poseID and
188  // it's threaded like the others.
189 
190  // * Rebuild metric maps for consistency:
191  // ----------------------------------------------------------------------------------------------
192  LMH.rebuildMetricMaps();
193 
194  // * Update all the data in the node "Ae":
195  // ----------------------------------------------------------------------------------------------
196  LMH.updateAreaFromLMH(Ae);
197 
198  MRPT_END
199 }
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.
#define MRPT_START
Definition: exceptions.h:241
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:44
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:120
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:85
#define MRPT_END
Definition: exceptions.h:245
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:6398
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: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019