Main MRPT website > C++ reference for MRPT 1.5.6
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-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 #include "hmtslam-precomp.h" // Precomp header
11 
12 #include <mrpt/utils/CTicTac.h>
13 #include <mrpt/random.h>
14 #include <mrpt/utils/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 mrpt::utils;
24 using namespace mrpt::synch;
25 using namespace std;
26 
27 /*---------------------------------------------------------------
28 
29  areaAbstraction
30 
31  Area Abstraction (AA) process within HMT-SLAM
32 
33  ---------------------------------------------------------------*/
34 CHMTSLAM::TMessageLSLAMfromAAPtr CHMTSLAM::areaAbstraction(
36  const TPoseIDList &newPoseIDs )
37 {
39 
40  ASSERT_( !newPoseIDs.empty() );
41  ASSERT_(LMH);
42  CHMTSLAM *obj = LMH->m_parent.get();
43  ASSERT_(obj);
44 
45  // The output results:
47 
48 
49  // Process msg:
50  THypothesisID LMH_ID = LMH->m_ID;
51  resMsg->hypothesisID = LMH_ID;
52 
53  for (TPoseIDList::const_iterator newID=newPoseIDs.begin();newID!=newPoseIDs.end();++newID)
54  {
55  // Add a new node to the graph:
56  obj->logFmt(mrpt::utils::LVL_DEBUG, "[thread_AA] Processing new pose ID: %u\n", static_cast<unsigned>( *newID ) );
57 
58  // Get SF & pose pdf for the new pose.
59  const CSensoryFrame *sf;
60  CPose3DPDFParticlesPtr posePDF = CPose3DPDFParticles::Create();
61 
62  {
63  // CCriticalSectionLocker lock( & LMH->m_lock ); // We are already within the LMH's lock!
64 
65  // SF:
66  std::map<TPoseID,CSensoryFrame>::const_iterator itSFs = LMH->m_SFs.find( *newID );
67  ASSERT_(itSFs != LMH->m_SFs.end() );
68  sf = &itSFs->second;
69 
70  // Pose PDF:
71  LMH->getPoseParticles( *newID, *posePDF );
72  } // end of LMH's critical section lock
73 
74  {
76 
77  // Add to the graph partitioner:
79 
80  unsigned int newIdx = LMH->m_robotPosesGraph.partitioner.addMapFrame(
81  CSensoryFramePtr(new CSensoryFrame(*sf)),
82  posePDF );
83  LMH->m_robotPosesGraph.idx2pose[ newIdx ] = *newID;
84  } // end of critical section lock on "m_robotPosesGraph.lock"
85  } // end for each new ID
86 
87  vector< vector_uint > partitions;
88  {
90  // Recompute partitions:
91  LMH->m_robotPosesGraph.partitioner.markAllNodesForReconsideration(); // We will have always small local maps.
93  }
94 
95  // Send result to LSLAM
96  resMsg->partitions.resize( partitions.size() );
99  for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin(); itSrc!=partitions.end(); itSrc++, itDest++)
100  {
101  itDest->resize( itSrc->size() );
104  for (it1=itSrc->begin(), it2=itDest->begin(); it1!=itSrc->end(); 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 
116 
117 void CHMTSLAM::TMessageLSLAMfromAA::dumpToConsole( ) const
118 {
119  cout << format("Hypo ID: %i has %i partitions:\n",(int)hypothesisID,(int)partitions.size());
120 
121  for (std::vector< TPoseIDList >::const_iterator it=partitions.begin();it!=partitions.end();++it)
122  {
123  printf_vector( "%i",*it);
124  cout << endl;
125  }
126 }
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
void markAllNodesForReconsideration()
Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering jus...
void updatePartitions(std::vector< vector_uint > &partitions)
This method executed only the neccesary part of the partition to take into account the lastest added ...
Scalar * iterator
Definition: eigen_plugins.h:23
const Scalar * const_iterator
Definition: eigen_plugins.h:24
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
#define MRPT_END
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
mrpt::slam::CIncrementalMapPartitioner::TOptions options
GLhandleARB obj
Definition: glew.h:3276
mrpt::hmtslam::CHMTSLAM::TOptions m_options
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
unsigned int addMapFrame(const mrpt::obs::CSensoryFramePtr &frame, const mrpt::poses::CPosePDFPtr &robotPose2D)
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
std::vector< TPoseID > TPoseIDList
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...
#define MRPT_START
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
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
mrpt::slam::CIncrementalMapPartitioner::TOptions AA_options
[AA] The options for the partitioning algorithm
Definition: CHMTSLAM.h:436
#define ASSERT_(f)
synch::CCriticalSection lock
CS to access the entire struct.
stlplus::smart_ptr< TMessageLSLAMfromAA > TMessageLSLAMfromAAPtr
Definition: CHMTSLAM.h:97



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018