MRPT  1.9.9
CHMTSLAM_AA.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::poses;
22 using namespace mrpt::obs;
23 using namespace std;
24 
25 /*---------------------------------------------------------------
26 
27  areaAbstraction
28 
29  Area Abstraction (AA) process within HMT-SLAM
30 
31  ---------------------------------------------------------------*/
32 CHMTSLAM::TMessageLSLAMfromAA::Ptr CHMTSLAM::areaAbstraction(
33  CLocalMetricHypothesis* LMH, const TPoseIDList& newPoseIDs)
34 {
36 
37  ASSERT_(!newPoseIDs.empty());
38  ASSERT_(LMH);
39  CHMTSLAM* obj = LMH->m_parent.get();
40  ASSERT_(obj);
41 
42  // The output results:
43  TMessageLSLAMfromAA::Ptr resMsg = std::make_shared<TMessageLSLAMfromAA>();
44 
45  // Process msg:
46  THypothesisID LMH_ID = LMH->m_ID;
47  resMsg->hypothesisID = LMH_ID;
48 
49  for (unsigned long newPoseID : newPoseIDs)
50  {
51  // Add a new node to the graph:
52  obj->logFmt(
53  mrpt::system::LVL_DEBUG, "[thread_AA] Processing new pose ID: %u\n",
54  static_cast<unsigned>(newPoseID));
55 
56  // Get SF & pose pdf for the new pose.
57  const CSensoryFrame* sf;
58  CPose3DPDFParticles::Ptr posePDF =
59  std::make_shared<CPose3DPDFParticles>();
60 
61  {
62  // std::lock_guard<std::mutex> lock( LMH->m_lock ); // We are
63  // already within the LMH's lock!
64 
65  // SF:
66  auto itSFs = LMH->m_SFs.find(newPoseID);
67  ASSERT_(itSFs != LMH->m_SFs.end());
68  sf = &itSFs->second;
69 
70  // Pose PDF:
71  LMH->getPoseParticles(newPoseID, *posePDF);
72  } // end of LMH's critical section lock
73 
74  {
75  std::lock_guard<std::mutex> locker(LMH->m_robotPosesGraph.lock);
76 
77  // Add to the graph partitioner:
79  obj->m_options.AA_options;
80 
81  unsigned int newIdx =
82  LMH->m_robotPosesGraph.partitioner.addMapFrame(*sf, *posePDF);
83  LMH->m_robotPosesGraph.idx2pose[newIdx] = newPoseID;
84  } // end of critical section lock on "m_robotPosesGraph.lock"
85  } // end for each new ID
86 
87  vector<std::vector<uint32_t>> partitions;
88  {
89  std::lock_guard<std::mutex> locker(LMH->m_robotPosesGraph.lock);
91  }
92 
93  // Send result to LSLAM
94  resMsg->partitions.resize(partitions.size());
95  vector<TPoseIDList>::iterator itDest;
96  vector<std::vector<uint32_t>>::const_iterator itSrc;
97  for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin();
98  itSrc != partitions.end(); itSrc++, itDest++)
99  {
100  itDest->resize(itSrc->size());
101  std::vector<uint32_t>::const_iterator it1;
102  TPoseIDList::iterator it2;
103  for (it1 = itSrc->begin(), it2 = itDest->begin(); it1 != itSrc->end();
104  it1++, it2++)
105  *it2 = LMH->m_robotPosesGraph.idx2pose[*it1];
106  }
107 
108  resMsg->dumpToConsole();
109 
110  return resMsg;
111 
112  MRPT_END
113 }
114 
115 void CHMTSLAM::TMessageLSLAMfromAA::dumpToConsole() const
116 {
117  cout << format(
118  "Hypo ID: %i has %i partitions:\n", (int)hypothesisID,
119  (int)partitions.size());
120 
121  for (const auto& partition : partitions)
122  {
123  mrpt::containers::printf_vector("%i", partition);
124  cout << endl;
125  }
126 }
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
#define MRPT_START
Definition: exceptions.h:241
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
void printf_vector(const char *fmt, const std::vector< T > &V)
Prints a vector in the format [A,B,C,...] to std::cout, and the fmt string for each vector element...
Definition: printf_vector.h:40
STL namespace.
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
std::vector< TPoseID > TPoseIDList
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
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.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
#define MRPT_END
Definition: exceptions.h:245
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...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 6e6d28d09 Wed Nov 13 18:58:42 2019 +0100 at miƩ nov 13 19:00:09 CET 2019