Main MRPT website > C++ reference for MRPT 1.5.7
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 
void getAdjacencyMatrix(MATRIX &outMatrix) const
Return a copy of the internal adjacency matrix.
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.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
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
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
GLenum GLsizei n
Definition: glext.h:4618
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
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.
STL namespace.
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 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.
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
Helper types for STL containers with Eigen memory allocators.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
#define MRPT_END
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
void getAs3DScene(mrpt::opengl::CSetOfObjectsPtr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
This namespace provides multitask, synchronization utilities.
Definition: atomic_incr.h:18
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...
Definition: CPoint.h:17
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.
The namespace for 3D scene representation and rendering.
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)
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 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...
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()



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019