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)
161 std::lock_guard<std::mutex> lockerLMH(bestLMH->
threadLocks.m_lock);
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.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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)...