41 void CHMTSLAM::generateLogFiles(
unsigned int nIteration)
55 std::lock_guard<std::mutex> lock(m_LMHs_cs);
61 string filLocalAreas =
format(
62 "%s/LSLAM_3D/mostLikelyLMH_LSLAM_%05u.3Dscene",
63 m_options.LOG_OUTPUT_DIR.c_str(), nIteration);
67 for (
auto& m : m_LMHs)
73 else if (m.second.m_log_w > bestLMH->
m_log_w)
80 bestHypoID = bestLMH->
m_ID;
83 std::lock_guard<std::mutex> lockerLMH(
89 std::make_shared<opengl::CSetOfObjects>();
90 maps3D->setName(
"metric-maps");
92 ->d->metricMaps.getAs3DObject(maps3D);
93 sceneLSLAM->insert(maps3D);
97 std::make_shared<opengl::CSetOfObjects>();
98 LSLAM_3D->setName(
"LSLAM_3D");
100 sceneLSLAM->insert(LSLAM_3D);
102 sceneLSLAM->enableFollowCamera(
true);
113 string filSSO =
format(
"%s/ASSO/mostLikelyLMH_ASSO_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
117 sceneSSO.insert(sso3D);
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 ) );
143 if ((CNT++ % 20) == 0)
145 string hmtmap_file(
format(
146 "%s/HMTSLAM_state/state_%05u.hmtslam",
147 m_options.LOG_OUTPUT_DIR.c_str(), nIteration));
159 if ((CNT++ % 5) == 0)
170 std::lock_guard<std::mutex> lock(m_map_cs);
172 "[LOG] HMT-map: " << m_map.nodeCount() <<
" nodes/ " 173 << m_map.arcCount() <<
" arcs");
177 m_map.getFirstNode()->getID(),
183 string hmtmap_file(
format(
184 "%s/HMAP_3D/mostLikelyHMT_MAP_%05u.3Dscene",
185 m_options.LOG_OUTPUT_DIR.c_str(), nIteration));
197 format(
"%s/log_MemoryUsage.txt", m_options.LOG_OUTPUT_DIR.c_str())
202 os::fprintf(f,
"%u\t%f\n", nIteration, memUsage / (1024.0 * 1024.0));
206 double t_log = tictac.Tac();
void getAdjacencyMatrix(MATRIX &outMatrix) const
Return a copy of the adjacency matrix.
struct mrpt::hmtslam::CLocalMetricHypothesis::ThreadLocks threadLocks
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)
int void fclose(FILE *f)
An OS-independent version of fclose.
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...
A high-performance stopwatch, with typical resolution of nanoseconds.
double m_log_w
Log-weight of this hypothesis.
std::string formatTimeInterval(const double timeSeconds)
Returns a formated string with the given time difference (passed as the number of seconds)...
mrpt::slam::CIncrementalMapPartitioner partitioner
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream, std::istream, std::ostream, std::stringstream.
#define ASSERT_(f)
Defines an assertion mechanism.
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).
std::mutex lock
CS to access the entire struct.
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.
#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.
The namespace for 3D scene representation and rendering.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
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.
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)...
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...