Main MRPT website > C++ reference for MRPT 1.5.6
CHMTSLAM_LOG.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/system/os.h>
15 #include <mrpt/system/os.h>
16 #include <mrpt/utils/CTicTac.h>
17 
18 using namespace mrpt::slam;
19 using namespace mrpt::hmtslam;
20 using namespace mrpt::opengl;
21 using namespace mrpt::poses;
22 using namespace mrpt::utils;
23 using namespace mrpt::synch;
24 using namespace mrpt::system;
25 using namespace std;
26 
27 /*---------------------------------------------------------------
28 
29  CHMTSLAM_LOG
30 
31  Implements a 2D local SLAM method based on scan matching
32  between near observations and an EKF. A part of HMT-SLAM
33 
34 \param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
35 \param act The action to process (or NULL).
36 \param sf The observations to process (or NULL).
37 
38 --------------------------------------------------------------- */
39 void CHMTSLAM::generateLogFiles(unsigned int nIteration)
40 {
42 
43  // Speed up the storage of images (in opengl::CTexturedPlane's):
44  //CImage::DISABLE_ZIP_COMPRESSION = true;
45 
46  static CTicTac tictac;
47 
48  tictac.Tic();
49 
50  THypothesisID bestHypoID;
51  CLocalMetricHypothesis *bestLMH = NULL;
52  {
53  CCriticalSectionLocker locker( &m_LMHs_cs );
54 
55  MRPT_LOG_INFO_STREAM( "[LOG] Number of LMHs: " << m_LMHs.size());
56 
57  // Generate 3D view of local areas:
58  {
59  string filLocalAreas = format("%s/LSLAM_3D/mostLikelyLMH_LSLAM_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
60  COpenGLScenePtr sceneLSLAM = COpenGLScene::Create();
61 
62  // Look for the most likely LMH:
64  for ( it = m_LMHs.begin();it!=m_LMHs.end();it++)
65  {
66  if (!bestLMH)
67  {
68  bestLMH = & it->second;
69  }
70  else if ( it->second.m_log_w > bestLMH->m_log_w)
71  {
72  bestLMH = & it->second;
73  }
74  }
75  ASSERT_(bestLMH!=NULL)
76 
77  bestHypoID = bestLMH->m_ID;
78 
79  {
80  CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
81 
82  {
83  // Generate the metric maps 3D view...
84  opengl::CSetOfObjectsPtr maps3D = opengl::CSetOfObjects::Create();
85  maps3D->setName("metric-maps");
86  bestLMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject( maps3D );
87  sceneLSLAM->insert( maps3D );
88 
89  // ...and the robot poses, areas, etc:
90  opengl::CSetOfObjectsPtr LSLAM_3D = opengl::CSetOfObjects::Create();
91  LSLAM_3D->setName("LSLAM_3D");
92  bestLMH->getAs3DScene( LSLAM_3D );
93  sceneLSLAM->insert( LSLAM_3D );
94 
95  sceneLSLAM->enableFollowCamera(true);
96 
97  MRPT_LOG_INFO_STREAM( "[LOG] Saving " << filLocalAreas);
98  CFileGZOutputStream(filLocalAreas) << *sceneLSLAM;
99  }
100 
101 
102  // Save the SSO matrix:
103 #if 0
104  {
105  CCriticalSectionLocker locker( &bestLMH->m_robotPosesGraph.lock );
106  string filSSO = format("%s/ASSO/mostLikelyLMH_ASSO_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
107  COpenGLScene sceneSSO;
108  opengl::CSetOfObjectsPtr sso3D = opengl::CSetOfObjects::Create();
110  sceneSSO.insert(sso3D);
111  CFileGZOutputStream(filSSO) << sceneSSO;
112 
113  if (1)
114  {
115  CMatrix A;
117  if (A.getColCount()>0)
118  {
119  A.adjustRange();
120  A.saveToTextFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.txt", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
121  CImage(A,true).saveToFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.png", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
122  }
123  }
124  } // end lock partitioner's CS
125 #endif
126 
127  } // end LMH's lock
128 
129  }
130 
131  } // end of lock on LMHs_cs
132 
133 #if 1
134  {
135  // Save the whole HMT-SLAM state to a dump file
136  static int CNT = 0;
137  if ((CNT++ % 20) == 0)
138  {
139  string hmtmap_file( format("%s/HMTSLAM_state/state_%05u.hmtslam", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
140  MRPT_LOG_INFO_STREAM( "[LOG] Saving: " << hmtmap_file.c_str());
141  CFileGZOutputStream(hmtmap_file) << *this;
142  }
143  }
144 #endif
145 
146 
147 #if 1
148  {
149  // Update the poses-graph in the HMT-map from the LMH to draw it:
150  static int CNT = 0;
151  if ((CNT++ % 5) == 0)
152  {
153  CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
154 
155  for (TNodeIDSet::const_iterator n = bestLMH->m_neighbors.begin();n!=bestLMH->m_neighbors.end();++n)
156  bestLMH->updateAreaFromLMH( *n );
157 
158  // Save global map for most likely hypothesis:
159  COpenGLScene sceneGlobalHMTMAP;
160  {
161  CCriticalSectionLocker locker( &m_map_cs );
162  MRPT_LOG_INFO_STREAM( "[LOG] HMT-map: "<< m_map.nodeCount() << " nodes/ "<< m_map.arcCount() << " arcs");
163 
164  m_map.getAs3DScene(
165  sceneGlobalHMTMAP, // Scene
166  m_map.getFirstNode()->getID(), // Reference node
167  bestHypoID, // Hypothesis to get
168  3 // iterations
169  );
170  }
171 
172  string hmtmap_file( format("%s/HMAP_3D/mostLikelyHMT_MAP_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
173  MRPT_LOG_INFO_STREAM( "[LOG] Saving "<< hmtmap_file);
174  CFileGZOutputStream(hmtmap_file) << sceneGlobalHMTMAP;
175  }
176  }
177 #endif
178 
179 
180  // Save the memory usage:
181  unsigned long memUsage = mrpt::system::getMemoryUsage();
182 
183  FILE *f=os::fopen( format("%s/log_MemoryUsage.txt",m_options.LOG_OUTPUT_DIR.c_str()).c_str() ,"at");
184  if (f)
185  {
186  os::fprintf(f,"%u\t%f\n",nIteration,memUsage/(1024.0*1024.0));
187  os::fclose(f);
188  }
189 
190  double t_log = tictac.Tac();
191  MRPT_LOG_INFO_STREAM( "[LOG] Time for logging: " << mrpt::system::formatTimeInterval(t_log));
192 
193  MRPT_END
194 }
195 
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
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).
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
void getAdjacencyMatrix(MATRIX &outMatrix) const
Return a copy of the internal adjacency matrix.
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
double m_log_w
Log-weight of this hypothesis.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
std::string BASE_IMPEXP formatTimeInterval(const double timeSeconds)
Returns a formated string with the given time difference (passed as the number of seconds)...
Definition: datetime.cpp:227
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
Helper types for STL containers with Eigen memory allocators.
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
GLsizei n
Definition: glew.h:5051
#define MRPT_END
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:299
void getAs3DScene(mrpt::opengl::CSetOfObjectsPtr &objs, const std::map< uint32_t, int64_t > *renameIndexes=NULL) const
Return a 3D representation of the current state: poses & links between them.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
void getAs3DScene(mrpt::opengl::CSetOfObjectsPtr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph...
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
#define MRPT_START
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
unsigned long BASE_IMPEXP getMemoryUsage()
Returns the memory occupied by this process, in bytes.
Definition: memory.cpp:182
synch::CCriticalSection m_lock
Critical section for threads signaling they are working with the LMH.
mrpt::utils::copy_ptr< T > d
The data associated with this particle. The use of copy_ptr<> allows relying on compiler-generated co...
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
#define ASSERT_(f)
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...
synch::CCriticalSection lock
CS to access the entire struct.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
static CSetOfObjectsPtr Create()
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



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