48 int64_t CHMTSLAM::m_nextAreaLabel = 0;
49 TPoseID CHMTSLAM::m_nextPoseID = 0;
59 m_terminateThreads =
false;
60 m_terminationFlag_LSLAM = m_terminationFlag_TBI =
61 m_terminationFlag_3D_viewer =
false;
65 m_hThread_LSLAM = std::thread(&CHMTSLAM::thread_LSLAM,
this);
66 m_hThread_TBI = std::thread(&CHMTSLAM::thread_TBI,
this);
67 m_hThread_3D_viewer = std::thread(&CHMTSLAM::thread_3D_viewer,
this);
70 m_LSLAM_method =
nullptr;
74 registerLoopClosureDetector(
75 "gridmaps", &CTopLCDetector_GridMatching::createNewInstance);
76 registerLoopClosureDetector(
77 "fabmap", &CTopLCDetector_FabMap::createNewInstance);
90 m_terminateThreads =
true;
94 MRPT_LOG_DEBUG(
"[CHMTSLAM::destructor] Waiting for threads end...\n");
96 m_hThread_3D_viewer.join();
97 m_hThread_LSLAM.join();
104 if (!m_options.LOG_OUTPUT_DIR.empty())
118 catch (std::exception& e)
122 "Ignoring exception at ~CHMTSLAM():\n%s", e.what()));
137 delete m_LSLAM_method;
138 m_LSLAM_method =
nullptr;
143 std::lock_guard<std::mutex> lock(m_topLCdets_cs);
147 it != m_topLCdets.end(); ++it)
156 void CHMTSLAM::clearInputQueue()
160 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
162 while (!m_inputQueue.empty())
175 if (m_terminateThreads)
183 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
184 m_inputQueue.push(acts);
193 if (m_terminateThreads)
201 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
202 m_inputQueue.push(sf);
211 if (m_terminateThreads)
223 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
224 m_inputQueue.push(sf);
233 m_options.loadFromConfigFile(cfg,
"HMT-SLAM");
235 m_options.defaultMapsInitializers.loadFromConfigFile(cfg,
"MetricMaps");
237 m_options.pf_options.loadFromConfigFile(cfg,
"PARTICLE_FILTER");
239 m_options.KLD_params.loadFromConfigFile(cfg,
"KLD");
241 m_options.AA_options.loadFromConfigFile(cfg,
"GRAPH_CUT");
244 m_options.TLC_grid_options.loadFromConfigFile(cfg,
"TLC_GRIDMATCHING");
245 m_options.TLC_fabmap_options.loadFromConfigFile(cfg,
"TLC_FABMAP");
247 m_options.dumpToConsole();
263 CHMTSLAM::TOptions::TOptions()
268 SLAM_METHOD = lsmRBPF_2DLASER;
270 SLAM_MIN_DIST_BETWEEN_OBS = 1.0f;
271 SLAM_MIN_HEADING_BETWEEN_OBS =
DEG2RAD(25.0f);
273 MIN_ODOMETRY_STD_XY = 0;
274 MIN_ODOMETRY_STD_PHI = 0;
276 VIEW3D_AREA_SPHERES_HEIGHT = 10.0f;
277 VIEW3D_AREA_SPHERES_RADIUS = 1.0f;
281 TLC_detectors.clear();
283 stds_Q_no_odo.resize(3);
284 stds_Q_no_odo[0] = stds_Q_no_odo[1] = 0.10f;
285 stds_Q_no_odo[2] =
DEG2RAD(4.0f);
291 void CHMTSLAM::TOptions::loadFromConfigFile(
311 stds_Q_no_odo[2] =
RAD2DEG(stds_Q_no_odo[2]);
312 source.read_vector(section,
"stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
313 ASSERT_(stds_Q_no_odo.size() == 3)
315 stds_Q_no_odo[2] =
DEG2RAD(stds_Q_no_odo[2]);
318 source.read_string(section,
"TLC_detectors",
"",
true);
322 std::cout <<
"TLC_detectors: " << TLC_detectors.size() << std::endl;
325 AA_options.loadFromConfigFile(
source, section);
333 out.
printf(
"\n----------- [CHMTSLAM::TOptions] ------------ \n\n");
348 AA_options.dumpToTextStream(out);
349 pf_options.dumpToTextStream(out);
350 KLD_params.dumpToTextStream(out);
351 defaultMapsInitializers.dumpToTextStream(out);
352 TLC_grid_options.dumpToTextStream(out);
353 TLC_fabmap_options.dumpToTextStream(out);
359 bool CHMTSLAM::isInputQueueEmpty()
364 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
365 res = m_inputQueue.empty();
373 size_t CHMTSLAM::inputQueueSize()
377 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
378 res = m_inputQueue.size();
391 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
392 if (!m_inputQueue.empty())
394 obj = m_inputQueue.front();
404 void CHMTSLAM::initializeEmptyMap()
410 LMH_hyps.insert(newHypothID);
417 std::lock_guard<std::mutex> lock(m_map_cs);
425 mrpt::make_aligned_shared<CHMHMapNode>(&m_map);
426 firstAreaID = firstArea->getID();
428 firstArea->m_hypotheses = LMH_hyps;
432 firstArea->m_nodeType.setType(
"Area");
433 firstArea->m_label = generateUniqueAreaLabel();
434 firstArea->m_annotations.set(
436 firstArea->m_annotations.setElemental(
444 std::lock_guard<std::mutex> lock(m_LMHs_cs);
454 newLMH.
m_ID = newHypothID;
465 switch (m_options.SLAM_METHOD)
467 case lsmRBPF_2DLASER:
469 if (m_LSLAM_method)
delete m_LSLAM_method;
475 "Invalid selection for LSLAM method: %i",
476 (
int)m_options.SLAM_METHOD);
483 std::lock_guard<std::mutex> lock(m_topLCdets_cs);
487 it != m_topLCdets.end(); ++it)
495 d != m_options.TLC_detectors.end(); ++d)
496 m_topLCdets.push_back(loopClosureDetector_factory(*d));
502 m_LSLAM_queue.clear();
507 if (!m_options.LOG_OUTPUT_DIR.empty())
516 m_options.LOG_OUTPUT_DIR +
"/HMTSLAM_state");
525 return format(
"%li", (
long int)(m_nextAreaLabel++));
531 TPoseID CHMTSLAM::generatePoseID() {
return m_nextPoseID++; }
547 bool CHMTSLAM::abortedDueToErrors()
549 return m_terminationFlag_LSLAM || m_terminationFlag_TBI ||
550 m_terminationFlag_3D_viewer;
556 void CHMTSLAM::registerLoopClosureDetector(
559 m_registeredLCDetectors[
name] = ptrCreateObject;
570 m_registeredLCDetectors.find(
name);
571 if (it == m_registeredLCDetectors.end())
573 "Invalid value for TLC_detectors: %s",
name.c_str());
574 return it->second(
this);
628 std::lock_guard<std::mutex> lock_map(m_map_cs);
631 in >> m_nextAreaLabel >> m_nextPoseID >> m_nextHypID;
670 std::lock_guard<std::mutex> lock_map(m_map_cs);
673 out << m_nextAreaLabel << m_nextPoseID << m_nextHypID;
bool createDirectory(const std::string &dirName)
Creates a directory.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
#define COMMON_TOPOLOG_HYP
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
double m_log_w
Log-weight of this hypothesis.
#define MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT( variableName, variableType, variableTypeCast, configFileObject, sectionNameStr)
const Scalar * const_iterator
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
GLsizei GLsizei GLuint * obj
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
This class allows loading and storing values and vectors of different types from a configuration text...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_LOG_WARN(_STRING)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
This namespace contains representation of robot actions and observations.
std::shared_ptr< CMultiMetricMap > Ptr
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
std::shared_ptr< CSensoryFrame > Ptr
#define MRPT_LOG_DEBUG(_STRING)
std::shared_ptr< CObservation > Ptr
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
std::shared_ptr< CSerializable > Ptr
void tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
std::shared_ptr< CActionCollection > Ptr
The namespace for 3D scene representation and rendering.
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
GLuint const GLchar * name
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
GLsizei GLsizei GLchar * source
#define NODE_ANNOTATION_REF_POSEID
bool deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
Delete all the files in a given directory (nothing done if directory does not exists, or path is a file).
std::shared_ptr< CHMHMapNode > Ptr
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
This class stores any customizable set of metric maps.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.