MRPT  1.9.9
CHMTSLAM_AA.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 
12 #include <mrpt/system/CTicTac.h>
13 #include <mrpt/random.h>
14 #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::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:
45 
46  // Process msg:
47  THypothesisID LMH_ID = LMH->m_ID;
48  resMsg->hypothesisID = LMH_ID;
49 
50  for (TPoseIDList::const_iterator newID = newPoseIDs.begin();
51  newID != newPoseIDs.end(); ++newID)
52  {
53  // Add a new node to the graph:
54  obj->logFmt(
55  mrpt::system::LVL_DEBUG, "[thread_AA] Processing new pose ID: %u\n",
56  static_cast<unsigned>(*newID));
57 
58  // Get SF & pose pdf for the new pose.
59  const CSensoryFrame* sf;
60  CPose3DPDFParticles::Ptr posePDF =
61  mrpt::make_aligned_shared<CPose3DPDFParticles>();
62 
63  {
64  // std::lock_guard<std::mutex> lock( LMH->m_lock ); // We are
65  // already within the LMH's lock!
66 
67  // SF:
69  LMH->m_SFs.find(*newID);
70  ASSERT_(itSFs != LMH->m_SFs.end());
71  sf = &itSFs->second;
72 
73  // Pose PDF:
74  LMH->getPoseParticles(*newID, *posePDF);
75  } // end of LMH's critical section lock
76 
77  {
78  std::lock_guard<std::mutex> locker(LMH->m_robotPosesGraph.lock);
79 
80  // Add to the graph partitioner:
82  obj->m_options.AA_options;
83 
84  unsigned int newIdx =
86  *sf, *posePDF);
87  LMH->m_robotPosesGraph.idx2pose[newIdx] = *newID;
88  } // end of critical section lock on "m_robotPosesGraph.lock"
89  } // end for each new ID
90 
91  vector<std::vector<uint32_t>> partitions;
92  {
93  std::lock_guard<std::mutex> locker(LMH->m_robotPosesGraph.lock);
95  }
96 
97  // Send result to LSLAM
98  resMsg->partitions.resize(partitions.size());
100  vector<std::vector<uint32_t>>::const_iterator itSrc;
101  for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin();
102  itSrc != partitions.end(); itSrc++, itDest++)
103  {
104  itDest->resize(itSrc->size());
107  for (it1 = itSrc->begin(), it2 = itDest->begin(); it1 != itSrc->end();
108  it1++, it2++)
109  *it2 = LMH->m_robotPosesGraph.idx2pose[*it1];
110  }
111 
112  resMsg->dumpToConsole();
113 
114  return resMsg;
115 
116  MRPT_END
117 }
118 
119 void CHMTSLAM::TMessageLSLAMfromAA::dumpToConsole() const
120 {
121  cout << format(
122  "Hypo ID: %i has %i partitions:\n", (int)hypothesisID,
123  (int)partitions.size());
124 
125  for (std::vector<TPoseIDList>::const_iterator it = partitions.begin();
126  it != partitions.end(); ++it)
127  {
129  cout << endl;
130  }
131 }
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
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.
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:54
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:4070
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
std::vector< TPoseID > TPoseIDList
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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:52
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_END
Definition: exceptions.h:266
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020