MRPT  1.9.9
CHMTSLAM_LOG.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hmtslam-precomp.h" // Precomp header
11 
15 #include <mrpt/system/CTicTac.h>
16 #include <mrpt/system/memory.h>
17 #include <mrpt/system/os.h>
18 
19 using namespace mrpt::slam;
20 using namespace mrpt::hmtslam;
21 using namespace mrpt::io;
22 using namespace mrpt::serialization;
23 using namespace mrpt::opengl;
24 using namespace mrpt::poses;
25 using namespace mrpt::system;
26 using namespace std;
27 
28 /*---------------------------------------------------------------
29 
30  CHMTSLAM_LOG
31 
32  Implements a 2D local SLAM method based on scan matching
33  between near observations and an EKF. A part of HMT-SLAM
34 
35 \param LMH The local metric hypothesis which must be updated by this SLAM
36 algorithm.
37 \param act The action to process (or nullptr).
38 \param sf The observations to process (or nullptr).
39 
40 --------------------------------------------------------------- */
41 void CHMTSLAM::generateLogFiles(unsigned int nIteration)
42 {
44 
45  // Speed up the storage of images (in opengl::CTexturedPlane's):
46  // CImage::DISABLE_ZIP_COMPRESSION = true;
47 
48  static CTicTac tictac;
49 
50  tictac.Tic();
51 
52  THypothesisID bestHypoID;
53  CLocalMetricHypothesis* bestLMH = nullptr;
54  {
55  std::lock_guard<std::mutex> lock(m_LMHs_cs);
56 
57  MRPT_LOG_INFO_STREAM("[LOG] Number of LMHs: " << m_LMHs.size());
58 
59  // Generate 3D view of local areas:
60  {
61  string filLocalAreas = format(
62  "%s/LSLAM_3D/mostLikelyLMH_LSLAM_%05u.3Dscene",
63  m_options.LOG_OUTPUT_DIR.c_str(), nIteration);
64  COpenGLScene::Ptr sceneLSLAM = std::make_shared<COpenGLScene>();
65 
66  // Look for the most likely LMH:
67  for (auto& m : m_LMHs)
68  {
69  if (!bestLMH)
70  {
71  bestLMH = &m.second;
72  }
73  else if (m.second.m_log_w > bestLMH->m_log_w)
74  {
75  bestLMH = &m.second;
76  }
77  }
78  ASSERT_(bestLMH != nullptr);
79 
80  bestHypoID = bestLMH->m_ID;
81 
82  {
83  std::lock_guard<std::mutex> lockerLMH(
84  bestLMH->threadLocks.m_lock);
85 
86  {
87  // Generate the metric maps 3D view...
89  std::make_shared<opengl::CSetOfObjects>();
90  maps3D->setName("metric-maps");
91  bestLMH->getMostLikelyParticle()
92  ->d->metricMaps.getAs3DObject(maps3D);
93  sceneLSLAM->insert(maps3D);
94 
95  // ...and the robot poses, areas, etc:
97  std::make_shared<opengl::CSetOfObjects>();
98  LSLAM_3D->setName("LSLAM_3D");
99  bestLMH->getAs3DScene(LSLAM_3D);
100  sceneLSLAM->insert(LSLAM_3D);
101 
102  sceneLSLAM->enableFollowCamera(true);
103 
104  MRPT_LOG_INFO_STREAM("[LOG] Saving " << filLocalAreas);
105  CFileGZOutputStream f(filLocalAreas);
106  archiveFrom(f) << *sceneLSLAM;
107  }
108 
109 // Save the SSO matrix:
110 #if 0
111  {
112  std::lock_guard<std::mutex> lock(bestLMH->m_robotPosesGraph.lock );
113  string filSSO = format("%s/ASSO/mostLikelyLMH_ASSO_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
114  COpenGLScene sceneSSO;
115  opengl::CSetOfObjects::Ptr sso3D = std::make_shared<opengl::CSetOfObjects>();
117  sceneSSO.insert(sso3D);
118  CFileGZOutputStream(filSSO) << sceneSSO;
119 
120  if (1)
121  {
122  CMatrixF A;
124  if (A.cols()>0)
125  {
126  A.adjustRange();
127  A.saveToTextFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.txt", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
128  CImage(A,true).saveToFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.png", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
129  }
130  }
131  } // end lock partitioner's CS
132 #endif
133 
134  } // end LMH's lock
135  }
136 
137  } // end of lock on LMHs_cs
138 
139 #if 1
140  {
141  // Save the whole HMT-SLAM state to a dump file
142  static int CNT = 0;
143  if ((CNT++ % 20) == 0)
144  {
145  string hmtmap_file(format(
146  "%s/HMTSLAM_state/state_%05u.hmtslam",
147  m_options.LOG_OUTPUT_DIR.c_str(), nIteration));
148  MRPT_LOG_INFO_STREAM("[LOG] Saving: " << hmtmap_file.c_str());
149  CFileGZOutputStream f(hmtmap_file);
150  archiveFrom(f) << *this;
151  }
152  }
153 #endif
154 
155 #if 1
156  {
157  // Update the poses-graph in the HMT-map from the LMH to draw it:
158  static int CNT = 0;
159  if ((CNT++ % 5) == 0)
160  {
161  std::lock_guard<std::mutex> lockerLMH(bestLMH->threadLocks.m_lock);
162 
163  for (auto n = bestLMH->m_neighbors.begin();
164  n != bestLMH->m_neighbors.end(); ++n)
165  bestLMH->updateAreaFromLMH(*n);
166 
167  // Save global map for most likely hypothesis:
168  COpenGLScene sceneGlobalHMTMAP;
169  {
170  std::lock_guard<std::mutex> lock(m_map_cs);
172  "[LOG] HMT-map: " << m_map.nodeCount() << " nodes/ "
173  << m_map.arcCount() << " arcs");
174 
175  m_map.getAs3DScene(
176  sceneGlobalHMTMAP, // Scene
177  m_map.getFirstNode()->getID(), // Reference node
178  bestHypoID, // Hypothesis to get
179  3 // iterations
180  );
181  }
182 
183  string hmtmap_file(format(
184  "%s/HMAP_3D/mostLikelyHMT_MAP_%05u.3Dscene",
185  m_options.LOG_OUTPUT_DIR.c_str(), nIteration));
186  MRPT_LOG_INFO_STREAM("[LOG] Saving " << hmtmap_file);
187  CFileGZOutputStream f(hmtmap_file);
188  archiveFrom(f) << sceneGlobalHMTMAP;
189  }
190  }
191 #endif
192 
193  // Save the memory usage:
194  unsigned long memUsage = mrpt::system::getMemoryUsage();
195 
196  FILE* f = os::fopen(
197  format("%s/log_MemoryUsage.txt", m_options.LOG_OUTPUT_DIR.c_str())
198  .c_str(),
199  "at");
200  if (f)
201  {
202  os::fprintf(f, "%u\t%f\n", nIteration, memUsage / (1024.0 * 1024.0));
203  os::fclose(f);
204  }
205 
206  double t_log = tictac.Tac();
208  "[LOG] Time for logging: " << mrpt::system::formatTimeInterval(t_log));
209 
210  MRPT_END
211 }
void getAdjacencyMatrix(MATRIX &outMatrix) const
Return a copy of the adjacency matrix.
#define MRPT_START
Definition: exceptions.h:241
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
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:5136
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:124
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:591
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::img::CImage CImage
Definition: utils/CImage.h:5
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
mrpt::io::CFileGZOutputStream CFileGZOutputStream
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:410
#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:114
#define MRPT_END
Definition: exceptions.h:245
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:58
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:257
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs, const std::map< uint32_t, int64_t > *renameIndexes=nullptr) const
Return a 3D representation of the graph: poses & links between them.
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:336
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
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: cb560b230 Wed Nov 13 08:06:48 2019 +0100 at miƩ nov 13 08:15:10 CET 2019