49 int64_t CHMTSLAM::m_nextAreaLabel = 0;
50 TPoseID CHMTSLAM::m_nextPoseID = 0;
60 m_terminateThreads =
false;
61 m_terminationFlag_LSLAM = m_terminationFlag_TBI =
62 m_terminationFlag_3D_viewer =
false;
66 m_hThread_LSLAM = std::thread(&CHMTSLAM::thread_LSLAM,
this);
67 m_hThread_TBI = std::thread(&CHMTSLAM::thread_TBI,
this);
68 m_hThread_3D_viewer = std::thread(&CHMTSLAM::thread_3D_viewer,
this);
71 m_LSLAM_method =
nullptr;
75 registerLoopClosureDetector(
76 "gridmaps", &CTopLCDetector_GridMatching::createNewInstance);
77 registerLoopClosureDetector(
78 "fabmap", &CTopLCDetector_FabMap::createNewInstance);
91 m_terminateThreads =
true;
95 MRPT_LOG_DEBUG(
"[CHMTSLAM::destructor] Waiting for threads end...\n");
97 m_hThread_3D_viewer.join();
98 m_hThread_LSLAM.join();
105 if (!m_options.LOG_OUTPUT_DIR.empty())
119 catch (
const 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);
146 for (
auto& m_topLCdet : m_topLCdets)
delete m_topLCdet;
154 void CHMTSLAM::clearInputQueue()
158 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
160 while (!m_inputQueue.empty())
173 if (m_terminateThreads)
181 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
182 m_inputQueue.push(acts);
191 if (m_terminateThreads)
199 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
200 m_inputQueue.push(sf);
209 if (m_terminateThreads)
221 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
222 m_inputQueue.push(sf);
231 m_options.loadFromConfigFile(cfg,
"HMT-SLAM");
233 m_options.defaultMapsInitializers.loadFromConfigFile(cfg,
"MetricMaps");
235 m_options.pf_options.loadFromConfigFile(cfg,
"PARTICLE_FILTER");
237 m_options.KLD_params.loadFromConfigFile(cfg,
"KLD");
239 m_options.AA_options.loadFromConfigFile(cfg,
"GRAPH_CUT");
242 m_options.TLC_grid_options.loadFromConfigFile(cfg,
"TLC_GRIDMATCHING");
243 m_options.TLC_fabmap_options.loadFromConfigFile(cfg,
"TLC_FABMAP");
245 m_options.dumpToConsole();
251 void CHMTSLAM::loadOptions(
const std::string& configFile)
261 CHMTSLAM::TOptions::TOptions()
266 SLAM_METHOD = lsmRBPF_2DLASER;
268 SLAM_MIN_DIST_BETWEEN_OBS = 1.0f;
269 SLAM_MIN_HEADING_BETWEEN_OBS =
DEG2RAD(25.0f);
271 MIN_ODOMETRY_STD_XY = 0;
272 MIN_ODOMETRY_STD_PHI = 0;
274 VIEW3D_AREA_SPHERES_HEIGHT = 10.0f;
275 VIEW3D_AREA_SPHERES_RADIUS = 1.0f;
279 TLC_detectors.clear();
281 stds_Q_no_odo.resize(3);
282 stds_Q_no_odo[0] = stds_Q_no_odo[1] = 0.10f;
283 stds_Q_no_odo[2] =
DEG2RAD(4.0f);
289 void CHMTSLAM::TOptions::loadFromConfigFile(
309 stds_Q_no_odo[2] =
RAD2DEG(stds_Q_no_odo[2]);
310 source.
read_vector(section,
"stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
311 ASSERT_(stds_Q_no_odo.size() == 3);
313 stds_Q_no_odo[2] =
DEG2RAD(stds_Q_no_odo[2]);
315 std::string sTLC_detectors =
316 source.
read_string(section,
"TLC_detectors",
"",
true);
320 std::cout <<
"TLC_detectors: " << TLC_detectors.size() << std::endl;
323 AA_options.loadFromConfigFile(source, section);
329 void CHMTSLAM::TOptions::dumpToTextStream(std::ostream&
out)
const
331 out <<
"\n----------- [CHMTSLAM::TOptions] ------------ \n\n";
346 AA_options.dumpToTextStream(
out);
347 pf_options.dumpToTextStream(
out);
348 KLD_params.dumpToTextStream(
out);
349 defaultMapsInitializers.dumpToTextStream(
out);
350 TLC_grid_options.dumpToTextStream(
out);
351 TLC_fabmap_options.dumpToTextStream(
out);
357 bool CHMTSLAM::isInputQueueEmpty()
362 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
363 res = m_inputQueue.empty();
371 size_t CHMTSLAM::inputQueueSize()
375 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
376 res = m_inputQueue.size();
389 std::lock_guard<std::mutex> lock(m_inputQueue_cs);
390 if (!m_inputQueue.empty())
392 obj = m_inputQueue.front();
402 void CHMTSLAM::initializeEmptyMap()
408 LMH_hyps.insert(newHypothID);
415 std::lock_guard<std::mutex> lock(m_map_cs);
423 firstAreaID = firstArea->getID();
425 firstArea->m_hypotheses = LMH_hyps;
427 CMultiMetricMap::Create(m_options.defaultMapsInitializers);
429 firstArea->m_nodeType =
"Area";
430 firstArea->m_label = generateUniqueAreaLabel();
431 firstArea->m_annotations.set(
433 firstArea->m_annotations.setElemental(
441 std::lock_guard<std::mutex> lock(m_LMHs_cs);
451 newLMH.
m_ID = newHypothID;
462 switch (m_options.SLAM_METHOD)
464 case lsmRBPF_2DLASER:
466 if (m_LSLAM_method)
delete m_LSLAM_method;
472 "Invalid selection for LSLAM method: %i",
473 (
int)m_options.SLAM_METHOD);
480 std::lock_guard<std::mutex> lock(m_topLCdets_cs);
483 for (
auto& m_topLCdet : m_topLCdets)
delete m_topLCdet;
489 for (
auto d = m_options.TLC_detectors.begin();
490 d != m_options.TLC_detectors.end(); ++d)
491 m_topLCdets.push_back(loopClosureDetector_factory(*d));
497 m_LSLAM_queue.clear();
502 if (!m_options.LOG_OUTPUT_DIR.empty())
511 m_options.LOG_OUTPUT_DIR +
"/HMTSLAM_state");
518 std::string CHMTSLAM::generateUniqueAreaLabel()
520 return format(
"%li", (
long int)(m_nextAreaLabel++));
526 TPoseID CHMTSLAM::generatePoseID() {
return m_nextPoseID++; }
539 bool CHMTSLAM::abortedDueToErrors()
541 return m_terminationFlag_LSLAM || m_terminationFlag_TBI ||
542 m_terminationFlag_3D_viewer;
548 void CHMTSLAM::registerLoopClosureDetector(
551 m_registeredLCDetectors[name] = ptrCreateObject;
558 const std::string& name)
561 auto it = m_registeredLCDetectors.find(name);
562 if (it == m_registeredLCDetectors.end())
564 "Invalid value for TLC_detectors: %s", name.c_str());
565 return it->second(
this);
606 std::lock_guard<std::mutex> lock_map(m_map_cs);
609 in >> m_nextAreaLabel >> m_nextPoseID >> m_nextHypID;
622 uint8_t CHMTSLAM::serializeGetVersion()
const {
return 0; }
627 std::lock_guard<std::mutex> lock_map(m_map_cs);
630 out << m_nextAreaLabel << m_nextPoseID << m_nextHypID;