Main MRPT website > C++ reference for MRPT 1.9.9
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-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/os.h>
15 #include <mrpt/system/os.h>
16 #include <mrpt/system/memory.h>
17 #include <mrpt/system/CTicTac.h>
19 
20 using namespace mrpt::slam;
21 using namespace mrpt::hmtslam;
22 using namespace mrpt::io;
23 using namespace mrpt::serialization;
24 using namespace mrpt::opengl;
25 using namespace mrpt::poses;
26 using namespace mrpt::system;
27 using namespace std;
28 
29 /*---------------------------------------------------------------
30 
31  CHMTSLAM_LOG
32 
33  Implements a 2D local SLAM method based on scan matching
34  between near observations and an EKF. A part of HMT-SLAM
35 
36 \param LMH The local metric hypothesis which must be updated by this SLAM
37 algorithm.
38 \param act The action to process (or nullptr).
39 \param sf The observations to process (or nullptr).
40 
41 --------------------------------------------------------------- */
42 void CHMTSLAM::generateLogFiles(unsigned int nIteration)
43 {
45 
46  // Speed up the storage of images (in opengl::CTexturedPlane's):
47  // CImage::DISABLE_ZIP_COMPRESSION = true;
48 
49  static CTicTac tictac;
50 
51  tictac.Tic();
52 
53  THypothesisID bestHypoID;
54  CLocalMetricHypothesis* bestLMH = nullptr;
55  {
56  std::lock_guard<std::mutex> lock(m_LMHs_cs);
57 
58  MRPT_LOG_INFO_STREAM("[LOG] Number of LMHs: " << m_LMHs.size());
59 
60  // Generate 3D view of local areas:
61  {
62  string filLocalAreas = format(
63  "%s/LSLAM_3D/mostLikelyLMH_LSLAM_%05u.3Dscene",
64  m_options.LOG_OUTPUT_DIR.c_str(), nIteration);
65  COpenGLScene::Ptr sceneLSLAM =
66  mrpt::make_aligned_shared<COpenGLScene>();
67 
68  // Look for the most likely LMH:
69  for (auto& m : m_LMHs)
70  {
71  if (!bestLMH)
72  {
73  bestLMH = &m.second;
74  }
75  else if (m.second.m_log_w > bestLMH->m_log_w)
76  {
77  bestLMH = &m.second;
78  }
79  }
80  ASSERT_(bestLMH != nullptr);
81 
82  bestHypoID = bestLMH->m_ID;
83 
84  {
85  std::lock_guard<std::mutex> lockerLMH(
86  bestLMH->threadLocks.m_lock);
87 
88  {
89  // Generate the metric maps 3D view...
91  mrpt::make_aligned_shared<opengl::CSetOfObjects>();
92  maps3D->setName("metric-maps");
93  bestLMH->getMostLikelyParticle()
94  ->d->metricMaps.getAs3DObject(maps3D);
95  sceneLSLAM->insert(maps3D);
96 
97  // ...and the robot poses, areas, etc:
99  mrpt::make_aligned_shared<opengl::CSetOfObjects>();
100  LSLAM_3D->setName("LSLAM_3D");
101  bestLMH->getAs3DScene(LSLAM_3D);
102  sceneLSLAM->insert(LSLAM_3D);
103 
104  sceneLSLAM->enableFollowCamera(true);
105 
106  MRPT_LOG_INFO_STREAM("[LOG] Saving " << filLocalAreas);
107  CFileGZOutputStream f(filLocalAreas);
108  archiveFrom(f) << *sceneLSLAM;
109  }
110 
111 // Save the SSO matrix:
112 #if 0
113  {
114  std::lock_guard<std::mutex> lock(bestLMH->m_robotPosesGraph.lock );
115  string filSSO = format("%s/ASSO/mostLikelyLMH_ASSO_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
116  COpenGLScene sceneSSO;
117  opengl::CSetOfObjects::Ptr sso3D = mrpt::make_aligned_shared<opengl::CSetOfObjects>();
119  sceneSSO.insert(sso3D);
120  CFileGZOutputStream(filSSO) << sceneSSO;
121 
122  if (1)
123  {
124  CMatrix A;
126  if (A.cols()>0)
127  {
128  A.adjustRange();
129  A.saveToTextFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.txt", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
130  CImage(A,true).saveToFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.png", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
131  }
132  }
133  } // end lock partitioner's CS
134 #endif
135 
136  } // end LMH's lock
137  }
138 
139  } // end of lock on LMHs_cs
140 
141 #if 1
142  {
143  // Save the whole HMT-SLAM state to a dump file
144  static int CNT = 0;
145  if ((CNT++ % 20) == 0)
146  {
147  string hmtmap_file(
148  format(
149  "%s/HMTSLAM_state/state_%05u.hmtslam",
150  m_options.LOG_OUTPUT_DIR.c_str(), nIteration));
151  MRPT_LOG_INFO_STREAM("[LOG] Saving: " << hmtmap_file.c_str());
152  CFileGZOutputStream f(hmtmap_file);
153  archiveFrom(f) << *this;
154  }
155  }
156 #endif
157 
158 #if 1
159  {
160  // Update the poses-graph in the HMT-map from the LMH to draw it:
161  static int CNT = 0;
162  if ((CNT++ % 5) == 0)
163  {
164  std::lock_guard<std::mutex> lockerLMH(bestLMH->threadLocks.m_lock);
165 
166  for (TNodeIDSet::const_iterator n = bestLMH->m_neighbors.begin();
167  n != bestLMH->m_neighbors.end(); ++n)
168  bestLMH->updateAreaFromLMH(*n);
169 
170  // Save global map for most likely hypothesis:
171  COpenGLScene sceneGlobalHMTMAP;
172  {
173  std::lock_guard<std::mutex> lock(m_map_cs);
175  "[LOG] HMT-map: " << m_map.nodeCount() << " nodes/ "
176  << m_map.arcCount() << " arcs");
177 
178  m_map.getAs3DScene(
179  sceneGlobalHMTMAP, // Scene
180  m_map.getFirstNode()->getID(), // Reference node
181  bestHypoID, // Hypothesis to get
182  3 // iterations
183  );
184  }
185 
186  string hmtmap_file(
187  format(
188  "%s/HMAP_3D/mostLikelyHMT_MAP_%05u.3Dscene",
189  m_options.LOG_OUTPUT_DIR.c_str(), nIteration));
190  MRPT_LOG_INFO_STREAM("[LOG] Saving " << hmtmap_file);
191  CFileGZOutputStream f(hmtmap_file);
192  archiveFrom(f) << sceneGlobalHMTMAP;
193  }
194  }
195 #endif
196 
197  // Save the memory usage:
198  unsigned long memUsage = mrpt::system::getMemoryUsage();
199 
200  FILE* f = os::fopen(
201  format("%s/log_MemoryUsage.txt", m_options.LOG_OUTPUT_DIR.c_str())
202  .c_str(),
203  "at");
204  if (f)
205  {
206  os::fprintf(f, "%u\t%f\n", nIteration, memUsage / (1024.0 * 1024.0));
207  os::fclose(f);
208  }
209 
210  double t_log = tictac.Tac();
212  "[LOG] Time for logging: " << mrpt::system::formatTimeInterval(t_log));
213 
214  MRPT_END
215 }
void getAdjacencyMatrix(MATRIX &outMatrix) const
Return a copy of the adjacency matrix.
#define MRPT_START
Definition: exceptions.h:262
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:25
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph...
GLenum GLsizei n
Definition: glext.h:5074
A high-performance stopwatch, with typical resolution of nanoseconds.
double m_log_w
Log-weight of this hypothesis.
STL namespace.
std::string formatTimeInterval(const double timeSeconds)
Returns a formated string with the given time difference (passed as the number of seconds)...
Definition: datetime.cpp:232
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream
Definition: CArchive.h:561
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::img::CImage CImage
Definition: utils/CImage.h:7
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
mrpt::io::CFileGZOutputStream CFileGZOutputStream
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs, const std::map< uint32_t, int64_t > *renameIndexes=NULL) const
Return a 3D representation of the graph: poses & links between them.
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
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
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
unsigned long getMemoryUsage()
Returns the memory occupied by this process, in bytes.
Definition: memory.cpp:110
#define MRPT_END
Definition: exceptions.h:266
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:59
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...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
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:296
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
const Scalar * const_iterator
Definition: eigen_plugins.h:27
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019