An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
The main entry points for a user are pushAction() and pushObservations(). Several parameters can be modified through m_options.
The mathematical models of this approach have been reported in:
More information in the wiki page: http://www.mrpt.org/HMT-SLAM . A complete working application can be found in "MRPT/apps/hmt-slam".
The complete state of the SLAM framework is serializable, so it can be saved and restore to/from a binary dump. This class implements mrpt::serialization::CSerializable, so it can be saved with "stream << slam_object;" and restored with "stream >> slam_object;". Alternatively, the methods CHMTSLAM::saveState and CHMTSLAM::loadState can be invoked, which in turn call internally to the CSerializable interface.
Definition at line 68 of file CHMTSLAM.h.
#include <mrpt/hmtslam/CHMTSLAM.h>
Classes | |
struct | TMessageLSLAMfromAA |
Message definition: More... | |
struct | TMessageLSLAMfromTBI |
Message definition: More... | |
struct | TMessageLSLAMtoTBI |
Message definition: More... | |
struct | TOptions |
A variety of options and configuration params (private, use loadOptions). More... | |
Public Member Functions | |
void * | operator new (size_t size) |
void * | operator new[] (size_t size) |
void | operator delete (void *ptr) noexcept |
void | operator delete[] (void *ptr) noexcept |
void | operator delete (void *memory, void *ptr) noexcept |
void * | operator new (size_t size, const std::nothrow_t &) noexcept |
void | operator delete (void *ptr, const std::nothrow_t &) noexcept |
CHMTSLAM () | |
Default constructor. More... | |
CHMTSLAM (const CHMTSLAM &) | |
const CHMTSLAM & | operator= (const CHMTSLAM &) |
virtual | ~CHMTSLAM () |
Destructor. More... | |
bool | abortedDueToErrors () |
Return true if an exception has been caught in any thread leading to the end of the mapping application: no more actions/observations will be processed from now on. More... | |
void | generateLogFiles (unsigned int nIteration) |
Called from LSLAM thread when log files must be created. More... | |
void | getAs3DScene (mrpt::opengl::COpenGLScene &outScene) |
Gets a 3D representation of the current state of the whole mapping framework. More... | |
virtual mxArray * | writeToMatlab () const |
Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class. More... | |
High-level map management | |
void | loadOptions (const std::string &configFile) |
Loads the options from a config file. More... | |
void | loadOptions (const mrpt::config::CConfigFileBase &cfgSource) |
Loads the options from a config source. More... | |
void | initializeEmptyMap () |
Initializes the whole HMT-SLAM framework, reseting to an empty map (It also clears the logs directory) - this must be called AFTER loading the options with CHMTSLAM::loadOptions. More... | |
bool | saveState (mrpt::serialization::CArchive &out) const |
Save the state of the whole HMT-SLAM framework to some binary stream (e.g. More... | |
bool | loadState (mrpt::serialization::CArchive &in) |
Load the state of the whole HMT-SLAM framework from some binary stream (e.g. More... | |
Static Public Member Functions | |
static void * | operator new (size_t size, void *ptr) |
Public Attributes | |
The important data. | |
CHierarchicalMHMap | m_map |
The hiearchical, multi-hypothesis graph-based map. More... | |
mrpt::aligned_std_map< THypothesisID, CLocalMetricHypothesis > | m_LMHs |
The list of LMHs at each instant. More... | |
Static Public Attributes | |
static mrpt::system::TConsoleColor | logging_levels_to_colors [NUMBER_OF_VERBOSITY_LEVELS] |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More... | |
static std::string | logging_levels_to_names [NUMBER_OF_VERBOSITY_LEVELS] |
Map from VerbosityLevels to their corresponding names. More... | |
Protected Member Functions | |
CSerializable virtual methods | |
uint8_t | serializeGetVersion () const override |
Must return the current versioning number of the object. More... | |
void | serializeTo (mrpt::serialization::CArchive &out) const override |
Pure virtual method for writing (serializing) to an abstract archive. More... | |
void | serializeFrom (mrpt::serialization::CArchive &in, uint8_t serial_version) override |
Pure virtual method for reading (deserializing) from an abstract archive. More... | |
HMT-SLAM sub-processes. | |
void | LSLAM_process_message (const mrpt::serialization::CMessage &msg) |
Auxiliary method within thread_LSLAM. More... | |
void | LSLAM_process_message_from_AA (const TMessageLSLAMfromAA &myMsg) |
No critical section locks are assumed at the entrance of this method. More... | |
void | LSLAM_process_message_from_TBI (const TMessageLSLAMfromTBI &myMsg) |
No critical section locks are assumed at the entrance of this method. More... | |
void | perform_TLC (CLocalMetricHypothesis &LMH, const CHMHMapNode::TNodeID areaInLMH, const CHMHMapNode::TNodeID areaLoopClosure, const mrpt::poses::CPose3DPDFGaussian &pose1wrt2) |
Topological Loop Closure: Performs all the required operations to close a loop between two areas which have been determined to be the same. More... | |
Static Protected Member Functions | |
static TMessageLSLAMfromAA::Ptr | areaAbstraction (CLocalMetricHypothesis *LMH, const TPoseIDList &newPoseIDs) |
The Area Abstraction (AA) method, invoked from LSLAM. More... | |
static TMessageLSLAMfromTBI::Ptr | TBI_main_method (CLocalMetricHypothesis *LMH, const CHMHMapNode::TNodeID &areaID) |
The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM. More... | |
static std::string | generateUniqueAreaLabel () |
Generates a new and unique area textual label (currently this generates "0","1",...) More... | |
static TPoseID | generatePoseID () |
Generates a new and unique pose ID. More... | |
static THypothesisID | generateHypothesisID () |
Generates a new and unique hypothesis ID. More... | |
Protected Attributes | |
bool | m_terminateThreads |
Termination flag for signaling all threads to terminate. More... | |
bool | m_terminationFlag_LSLAM |
Threads termination flags: More... | |
bool | m_terminationFlag_TBI |
bool | m_terminationFlag_3D_viewer |
mrpt::hmtslam::CHMTSLAM::TOptions | m_options |
VerbosityLevel | m_min_verbosity_level |
Provided messages with VerbosityLevel smaller than this value shall be ignored. More... | |
The different SLAM algorithms that can be invoked from the LSLAM | |
thread. | |
CLSLAMAlgorithmBase * | m_LSLAM_method |
An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" or "loadState". More... | |
Static Protected Attributes | |
static int64_t | m_nextAreaLabel = 0 |
static TPoseID | m_nextPoseID = 0 |
static THypothesisID | m_nextHypID = COMMON_TOPOLOG_HYP + 1 |
Private Member Functions | |
std::string | generateStringFromFormat (const char *fmt, va_list argp) const |
Helper method for generating a std::string instance from printf-like arguments. More... | |
Private Attributes | |
std::string | m_logger_name |
std::deque< TMsg > | m_history |
std::deque< output_logger_callback_t > | m_listCallbacks |
Friends | |
class | CLocalMetricHypothesis |
class | CLSLAM_RBPF_2DLASER |
class | CTopLCDetector_GridMatching |
class | CTopLCDetector_FabMap |
RTTI stuff | |
using | Ptr = std::shared_ptr< CHMTSLAM > |
using | ConstPtr = std::shared_ptr< const CHMTSLAM > |
using | UniquePtr = std::unique_ptr< CHMTSLAM > |
using | ConstUniquePtr = std::unique_ptr< const CHMTSLAM > |
static mrpt::rtti::CLASSINIT | _init_CHMTSLAM |
static const mrpt::rtti::TRuntimeClassId | runtimeClassId |
static constexpr const char * | className = "CHMTSLAM" |
static const mrpt::rtti::TRuntimeClassId * | _GetBaseClass () |
static constexpr auto | getClassName () |
static const mrpt::rtti::TRuntimeClassId & | GetRuntimeClassIdStatic () |
static mrpt::rtti::CObject * | CreateObject () |
template<typename... Args> | |
static Ptr | Create (Args &&... args) |
template<typename... Args> | |
static UniquePtr | CreateUnique (Args &&... args) |
virtual const mrpt::rtti::TRuntimeClassId * | GetRuntimeClass () const override |
Returns information about the class of an object in runtime. More... | |
virtual mrpt::rtti::CObject * | clone () const override |
Returns a deep copy (clone) of the object, indepently of its class. More... | |
Inter-thread communication queues: | |
using | CMessageQueue = mrpt::containers::CThreadSafeQueue< mrpt::serialization::CMessage > |
LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA. More... | |
CMessageQueue | m_LSLAM_queue |
Related to the input queue: | |
enum | TLSlamMethod { lsmRBPF_2DLASER = 1 } |
std::queue< mrpt::serialization::CSerializable::Ptr > | m_inputQueue |
The queue of pending actions/observations supplied by the user waiting for being processed. More... | |
std::mutex | m_inputQueue_cs |
Critical section for accessing m_inputQueue. More... | |
std::mutex | m_map_cs |
Critical section for accessing m_map. More... | |
std::mutex | m_LMHs_cs |
Critical section for accessing m_LMHs. More... | |
void | clearInputQueue () |
Empty the input queue. More... | |
bool | isInputQueueEmpty () |
Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions) More... | |
size_t | inputQueueSize () |
Returns the number of objects waiting for processing in the input queue. More... | |
void | pushAction (const mrpt::obs::CActionCollection::Ptr &acts) |
Here the user can enter an action into the system (will go to the SLAM process). More... | |
void | pushObservations (const mrpt::obs::CSensoryFrame::Ptr &sf) |
Here the user can enter observations into the system (will go to the SLAM process). More... | |
void | pushObservation (const mrpt::obs::CObservation::Ptr &obs) |
Here the user can enter an observation into the system (will go to the SLAM process). More... | |
mrpt::serialization::CSerializable::Ptr | getNextObjectFromInputQueue () |
Used from the LSLAM thread to retrieve the next object from the queue. More... | |
Threads stuff | |
std::thread | m_hThread_LSLAM |
Threads handles. More... | |
std::thread | m_hThread_TBI |
std::thread | m_hThread_3D_viewer |
void | thread_LSLAM () |
The function for the "Local SLAM" thread. More... | |
void | thread_TBI () |
The function for the "TBI" thread. More... | |
void | thread_3D_viewer () |
The function for the "3D viewer" thread. More... | |
The different Loop-Closure modules that are to be executed in the | |
using | TLopLCDetectorFactory = CTopLCDetectorBase *(*)(CHMTSLAM *) |
std::map< std::string, TLopLCDetectorFactory > | m_registeredLCDetectors |
std::deque< CTopLCDetectorBase * > | m_topLCdets |
The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState". More... | |
std::mutex | m_topLCdets_cs |
The critical section for accessing m_topLCdets. More... | |
void | registerLoopClosureDetector (const std::string &name, CTopLCDetectorBase *(*ptrCreateObject)(CHMTSLAM *)) |
Must be invoked before calling initializeEmptyMap, so LC objects can be created. More... | |
CTopLCDetectorBase * | loopClosureDetector_factory (const std::string &name) |
The class factory for topological loop closure detectors. More... | |
Logging methods | |
bool | logging_enable_console_output |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More... | |
bool | logging_enable_keep_record |
[Default=false] Enables storing all messages into an internal list. More... | |
void | logStr (const VerbosityLevel level, const std::string &msg_str) const |
Main method to add the specified message string to the logger. More... | |
void | logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3 |
Alternative logging method, which mimics the printf behavior. More... | |
void void | logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const |
Log the given message only if the condition is satisfied. More... | |
void | setLoggerName (const std::string &name) |
Set the name of the COutputLogger instance. More... | |
std::string | getLoggerName () const |
Return the name of the COutputLogger instance. More... | |
void | setMinLoggingLevel (const VerbosityLevel level) |
Set the minimum logging level for which the incoming logs are going to be taken into account. More... | |
void | setVerbosityLevel (const VerbosityLevel level) |
alias of setMinLoggingLevel() More... | |
VerbosityLevel | getMinLoggingLevel () const |
bool | isLoggingLevelVisible (VerbosityLevel level) const |
void | getLogAsString (std::string &log_contents) const |
Fill the provided string with the contents of the logger's history in std::string representation. More... | |
std::string | getLogAsString () const |
Get the history of COutputLogger instance in a string representation. More... | |
void | writeLogToFile (const std::string *fname_in=NULL) const |
Write the contents of the COutputLogger instance to an external file. More... | |
void | dumpLogToConsole () const |
Dump the current contents of the COutputLogger instance in the terminal window. More... | |
std::string | getLoggerLastMsg () const |
Return the last Tmsg instance registered in the logger history. More... | |
void | getLoggerLastMsg (std::string &msg_str) const |
Fill inputtted string with the contents of the last message in history. More... | |
void | loggerReset () |
Reset the contents of the logger instance. More... | |
void | logRegisterCallback (output_logger_callback_t userFunc) |
bool | logDeregisterCallback (output_logger_callback_t userFunc) |
|
protected |
LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA.
Definition at line 162 of file CHMTSLAM.h.
using mrpt::hmtslam::CHMTSLAM::ConstPtr = std::shared_ptr<const CHMTSLAM > |
Definition at line 76 of file CHMTSLAM.h.
using mrpt::hmtslam::CHMTSLAM::ConstUniquePtr = std::unique_ptr<const CHMTSLAM > |
Definition at line 76 of file CHMTSLAM.h.
using mrpt::hmtslam::CHMTSLAM::Ptr = std::shared_ptr< CHMTSLAM > |
A type for the associated smart pointer
Definition at line 76 of file CHMTSLAM.h.
|
protected |
Definition at line 310 of file CHMTSLAM.h.
using mrpt::hmtslam::CHMTSLAM::UniquePtr = std::unique_ptr< CHMTSLAM > |
Definition at line 76 of file CHMTSLAM.h.
Enumerator | |
---|---|
lsmRBPF_2DLASER |
Definition at line 230 of file CHMTSLAM.h.
CHMTSLAM::CHMTSLAM | ( | ) |
Default constructor.
debug_out_stream | If debug output messages should be redirected to any other stream apart from std::cout |
Definition at line 56 of file CHMTSLAM_main.cpp.
|
inline |
Definition at line 366 of file CHMTSLAM.h.
References THROW_EXCEPTION.
|
virtual |
Destructor.
Definition at line 87 of file CHMTSLAM_main.cpp.
References mrpt::format(), MRPT_LOG_DEBUG, and MRPT_LOG_WARN.
|
staticprotected |
bool CHMTSLAM::abortedDueToErrors | ( | ) |
Return true if an exception has been caught in any thread leading to the end of the mapping application: no more actions/observations will be processed from now on.
Definition at line 549 of file CHMTSLAM_main.cpp.
|
staticprotected |
The Area Abstraction (AA) method, invoked from LSLAM.
LMH | (IN) The LMH which to this query applies. |
newPoseIDs | (IN) The new poseIDs to be added to the graph partitioner. |
Definition at line 32 of file CHMTSLAM_AA.cpp.
References mrpt::slam::CIncrementalMapPartitioner::addMapFrame(), ASSERT_, mrpt::hmtslam::CLocalMetricHypothesis::getPoseParticles(), mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::idx2pose, mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::lock, mrpt::system::LVL_DEBUG, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, mrpt::hmtslam::CLocalMetricHypothesis::m_parent, mrpt::hmtslam::CLocalMetricHypothesis::m_robotPosesGraph, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, MRPT_END, MRPT_START, mrpt::slam::CIncrementalMapPartitioner::options, mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::partitioner, and mrpt::slam::CIncrementalMapPartitioner::updatePartitions().
void CHMTSLAM::clearInputQueue | ( | ) |
Empty the input queue.
Definition at line 157 of file CHMTSLAM_main.cpp.
|
overridevirtual |
Returns a deep copy (clone) of the object, indepently of its class.
Implements mrpt::rtti::CObject.
|
inlinestatic |
Definition at line 76 of file CHMTSLAM.h.
|
static |
|
inlinestatic |
Definition at line 76 of file CHMTSLAM.h.
|
inherited |
Dump the current contents of the COutputLogger instance in the terminal window.
Definition at line 179 of file COutputLogger.cpp.
|
inlineinherited |
Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer).
Definition at line 169 of file CObject.h.
References mrpt::rtti::CObject::clone().
Referenced by mrpt::obs::CRawlog::addActions(), and mrpt::obs::CRawlog::addObservations().
|
staticprotected |
Generates a new and unique hypothesis ID.
Definition at line 537 of file CHMTSLAM_main.cpp.
void CHMTSLAM::generateLogFiles | ( | unsigned int | nIteration | ) |
Called from LSLAM thread when log files must be created.
Definition at line 42 of file CHMTSLAM_LOG.cpp.
References mrpt::serialization::archiveFrom(), ASSERT_, mrpt::system::os::fclose(), mrpt::system::os::fopen(), mrpt::format(), mrpt::system::formatTimeInterval(), mrpt::system::os::fprintf(), mrpt::slam::CIncrementalMapPartitioner::getAdjacencyMatrix(), mrpt::hmtslam::CLocalMetricHypothesis::getAs3DScene(), mrpt::slam::CIncrementalMapPartitioner::getAs3DScene(), mrpt::system::getMemoryUsage(), mrpt::bayes::CParticleFilterData< T, STORAGE >::getMostLikelyParticle(), mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::idx2pose, mrpt::opengl::COpenGLScene::insert(), mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::lock, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w, mrpt::hmtslam::CLocalMetricHypothesis::m_neighbors, mrpt::hmtslam::CLocalMetricHypothesis::m_robotPosesGraph, MRPT_END, MRPT_LOG_INFO_STREAM, MRPT_START, mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::partitioner, mrpt::img::CImage::saveToFile(), mrpt::hmtslam::CLocalMetricHypothesis::threadLocks, mrpt::system::CTicTac::Tic(), and mrpt::hmtslam::CLocalMetricHypothesis::updateAreaFromLMH().
|
staticprotected |
Generates a new and unique pose ID.
Definition at line 533 of file CHMTSLAM_main.cpp.
Referenced by mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().
|
privateinherited |
Helper method for generating a std::string instance from printf-like arguments.
Definition at line 99 of file COutputLogger.cpp.
References mrpt::system::os::vsnprintf().
|
staticprotected |
Generates a new and unique area textual label (currently this generates "0","1",...)
Definition at line 525 of file CHMTSLAM_main.cpp.
References mrpt::format().
void CHMTSLAM::getAs3DScene | ( | mrpt::opengl::COpenGLScene & | outScene | ) |
Gets a 3D representation of the current state of the whole mapping framework.
Definition at line 541 of file CHMTSLAM_main.cpp.
References MRPT_UNUSED_PARAM.
|
inlinestaticconstexpr |
Definition at line 76 of file CHMTSLAM.h.
|
inherited |
Get the history of COutputLogger instance in a string representation.
Definition at line 148 of file COutputLogger.cpp.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
|
inherited |
Fill the provided string with the contents of the logger's history in std::string representation.
Definition at line 143 of file COutputLogger.cpp.
|
inherited |
Return the last Tmsg instance registered in the logger history.
Definition at line 184 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::getAsString().
|
inherited |
Fill inputtted string with the contents of the last message in history.
Definition at line 190 of file COutputLogger.cpp.
|
inherited |
Return the name of the COutputLogger instance.
Definition at line 132 of file COutputLogger.cpp.
Referenced by mrpt::system::COutputLogger::TMsg::TMsg().
|
inlineinherited |
Definition at line 200 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::maps::CRandomFieldGridMap2D::isEnabledVerbose(), and mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().
|
protected |
Used from the LSLAM thread to retrieve the next object from the queue.
Definition at line 387 of file CHMTSLAM_main.cpp.
|
overridevirtual |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::serialization::CSerializable.
|
static |
void CHMTSLAM::initializeEmptyMap | ( | ) |
Initializes the whole HMT-SLAM framework, reseting to an empty map (It also clears the logs directory) - this must be called AFTER loading the options with CHMTSLAM::loadOptions.
Definition at line 405 of file CHMTSLAM_main.cpp.
References mrpt::hmtslam::CLocalMetricHypothesis::clearRobotPoses(), COMMON_TOPOLOG_HYP, mrpt::system::createDirectory(), mrpt::system::deleteFilesInDirectory(), mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w, mrpt::hmtslam::CLocalMetricHypothesis::m_neighbors, mrpt::hmtslam::CLocalMetricHypothesis::m_parent, NODE_ANNOTATION_METRIC_MAPS, NODE_ANNOTATION_REF_POSEID, POSEID_INVALID, and THROW_EXCEPTION_FMT.
size_t CHMTSLAM::inputQueueSize | ( | ) |
Returns the number of objects waiting for processing in the input queue.
Definition at line 374 of file CHMTSLAM_main.cpp.
bool CHMTSLAM::isInputQueueEmpty | ( | ) |
Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions)
Definition at line 360 of file CHMTSLAM_main.cpp.
|
inlineinherited |
Definition at line 201 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().
void CHMTSLAM::loadOptions | ( | const mrpt::config::CConfigFileBase & | cfgSource | ) |
Loads the options from a config source.
Definition at line 232 of file CHMTSLAM_main.cpp.
void CHMTSLAM::loadOptions | ( | const std::string & | configFile | ) |
Loads the options from a config file.
Definition at line 254 of file CHMTSLAM_main.cpp.
References ASSERT_, and mrpt::system::fileExists().
bool CHMTSLAM::loadState | ( | mrpt::serialization::CArchive & | in | ) |
Load the state of the whole HMT-SLAM framework from some binary stream (e.g.
a file).
Definition at line 593 of file CHMTSLAM_main.cpp.
|
inherited |
Log the given message only if the condition is satisfied.
Definition at line 120 of file COutputLogger.cpp.
|
inherited |
Definition at line 287 of file COutputLogger.cpp.
References getAddress().
|
inherited |
Alternative logging method, which mimics the printf behavior.
Handy for not having to first use mrpt::format to pass a std::string message to logStr
Definition at line 80 of file COutputLogger.cpp.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), CGraphSlamHandler< GRAPH_T >::execute(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), CGraphSlamHandler< GRAPH_T >::initOutputDir(), CGraphSlamHandler< GRAPH_T >::initVisualization(), mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), CGraphSlamHandler< GRAPH_T >::readConfigFname(), CGraphSlamHandler< GRAPH_T >::saveResults(), CGraphSlamHandler< GRAPH_T >::setResultsDirName(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), and CGraphSlamHandler< GRAPH_T >::~CGraphSlamHandler().
|
inherited |
Reset the contents of the logger instance.
Called upon construction.
Definition at line 195 of file COutputLogger.cpp.
References mrpt::system::LVL_INFO.
|
inherited |
Definition at line 274 of file COutputLogger.cpp.
|
inherited |
Main method to add the specified message string to the logger.
Definition at line 61 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.
Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().
CTopLCDetectorBase * CHMTSLAM::loopClosureDetector_factory | ( | const std::string & | name | ) |
The class factory for topological loop closure detectors.
Possible values are enumerated in TOptions::TLC_detectors
std::exception | On unknown name. |
Definition at line 567 of file CHMTSLAM_main.cpp.
References MRPT_END, MRPT_START, and THROW_EXCEPTION_FMT.
|
protected |
Auxiliary method within thread_LSLAM.
Definition at line 282 of file CHMTSLAM_LSLAM.cpp.
References MRPT_END, MRPT_START, and MRPT_UNUSED_PARAM.
|
protected |
No critical section locks are assumed at the entrance of this method.
Definition at line 312 of file CHMTSLAM_LSLAM.cpp.
References ARC_ANNOTATION_DELTA, ARC_ANNOTATION_DELTA_SRC_POSEID, ARC_ANNOTATION_DELTA_TRG_POSEID, mrpt::serialization::archiveFrom(), AREAID_INVALID, ASSERT_, begin(), mrpt::hmtslam::CLocalMetricHypothesis::changeCoordinateOrigin(), mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::poses::CPose3DPDFGaussian::cov, mrpt::DEG2RAD(), mrpt::poses::CPose3D::distanceEuclidean6D(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::distanceTo(), mrpt::hmtslam::CLocalMetricHypothesis::dumpAsText(), mrpt::opengl::COpenGLScene::enableFollowCamera(), mrpt::containers::erase_return_next(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::ESS(), mrpt::containers::list_searchable< T >::find(), mrpt::format(), mrpt::hmtslam::CLocalMetricHypothesis::getAs3DScene(), mrpt::poses::CPose3DPDFGaussian::getMean(), mrpt::hmtslam::CLocalMetricHypothesis::getMeans(), mrpt::bayes::CParticleFilterData< T, STORAGE >::getMostLikelyParticle(), mrpt::hmtslam::CLocalMetricHypothesis::getRelativePose(), mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::hypothesisID, mrpt::containers::list_searchable< T >::insert(), mrpt::opengl::COpenGLScene::insert(), mrpt::obs::CSensoryFrame::insertObservationsInto(), mrpt::poses::CPose3DPDFGaussian::inverse(), mrpt::system::LVL_INFO, mrpt::hmtslam::CLocalMetricHypothesis::m_areasPendingTBI, mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, mrpt::hmtslam::CLocalMetricHypothesis::m_neighbors, mrpt::hmtslam::CLocalMetricHypothesis::m_nodeIDmemberships, mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_posesPendingAddPartitioner, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, mrpt::poses::CPose3DPDFGaussian::mean, MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_START, NODE_ANNOTATION_METRIC_MAPS, NODE_ANNOTATION_POSES_GRAPH, NODE_ANNOTATION_REF_POSEID, mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::partitions, mrpt::hmtslam::TPoseInfo::pdf, POSEID_INVALID, mrpt::RAD2DEG(), mrpt::hmtslam::CLocalMetricHypothesis::removeAreaFromLMH(), mrpt::hmtslam::TPoseInfo::sf, mrpt::square(), mrpt::system::stringListAsString(), mrpt::system::CTicTac::Tac(), THROW_EXCEPTION, THROW_EXCEPTION_FMT, mrpt::system::CTicTac::Tic(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y(), and mrpt::poses::CPose3D::yaw().
|
protected |
No critical section locks are assumed at the entrance of this method.
Definition at line 1948 of file CHMTSLAM_LSLAM.cpp.
References ASSERT_, mrpt::math::chi2inv(), mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::poses::CPose3DPDFGaussian::cov, mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::cur_area, mrpt::DEG2RAD(), mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::hypothesisID, mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::loopClosureData, mrpt::system::LVL_INFO, mrpt::poses::CPose3DPDFGaussian::mahalanobisDistanceTo(), mrpt::poses::CPose3DPDFGaussian::mean, MRPT_END, MRPT_START, mrpt::system::pause(), mrpt::RAD2DEG(), mrpt::square(), mrpt::system::CTicTac::Tac(), and mrpt::system::CTicTac::Tic().
Definition at line 76 of file CHMTSLAM.h.
Definition at line 76 of file CHMTSLAM.h.
Definition at line 76 of file CHMTSLAM.h.
Definition at line 76 of file CHMTSLAM.h.
|
inline |
Definition at line 76 of file CHMTSLAM.h.
|
inlinenoexcept |
Definition at line 76 of file CHMTSLAM.h.
Definition at line 76 of file CHMTSLAM.h.
|
inline |
Definition at line 76 of file CHMTSLAM.h.
Definition at line 370 of file CHMTSLAM.h.
References THROW_EXCEPTION.
|
protected |
Topological Loop Closure: Performs all the required operations to close a loop between two areas which have been determined to be the same.
Definition at line 34 of file CHMTSLAM_perform_TLC.cpp.
References ASSERT_, mrpt::poses::CPose3DPDFParticles::getMean(), mrpt::hmtslam::CLocalMetricHypothesis::getPoseParticles(), mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::lock, mrpt::system::LVL_DEBUG, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, mrpt::hmtslam::CLocalMetricHypothesis::m_neighbors, mrpt::hmtslam::CLocalMetricHypothesis::m_nodeIDmemberships, mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_posesPendingAddPartitioner, mrpt::hmtslam::CLocalMetricHypothesis::m_robotPosesGraph, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, mrpt::poses::CPose3DPDFGaussian::mean, MRPT_END, MRPT_START, NODE_ANNOTATION_POSES_GRAPH, NODE_ANNOTATION_REF_POSEID, mrpt::hmtslam::TPoseInfo::pdf, mrpt::hmtslam::CLocalMetricHypothesis::rebuildMetricMaps(), mrpt::hmtslam::TPoseInfo::sf, mrpt::poses::CPose3DPDFParticles::size(), and mrpt::hmtslam::CLocalMetricHypothesis::updateAreaFromLMH().
void CHMTSLAM::pushAction | ( | const mrpt::obs::CActionCollection::Ptr & | acts | ) |
Here the user can enter an action into the system (will go to the SLAM process).
This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
Definition at line 174 of file CHMTSLAM_main.cpp.
void CHMTSLAM::pushObservation | ( | const mrpt::obs::CObservation::Ptr & | obs | ) |
Here the user can enter an observation into the system (will go to the SLAM process).
This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
Definition at line 210 of file CHMTSLAM_main.cpp.
void CHMTSLAM::pushObservations | ( | const mrpt::obs::CSensoryFrame::Ptr & | sf | ) |
Here the user can enter observations into the system (will go to the SLAM process).
This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
Definition at line 192 of file CHMTSLAM_main.cpp.
void CHMTSLAM::registerLoopClosureDetector | ( | const std::string & | name, |
CTopLCDetectorBase *(*)(CHMTSLAM *) | ptrCreateObject | ||
) |
Must be invoked before calling initializeEmptyMap, so LC objects can be created.
Definition at line 558 of file CHMTSLAM_main.cpp.
bool CHMTSLAM::saveState | ( | mrpt::serialization::CArchive & | out | ) | const |
Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
a file).
Definition at line 580 of file CHMTSLAM_main.cpp.
Referenced by mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel().
|
overrideprotectedvirtual |
Pure virtual method for reading (deserializing) from an abstract archive.
Users don't call this method directly. Instead, use stream >> object;
.
in | The input binary stream where the object data must read from. |
version | The version of the object stored in the stream: use this version number in your code to know how to read the incoming data. |
std::exception | On any I/O error |
Implements mrpt::serialization::CSerializable.
Definition at line 609 of file CHMTSLAM_main.cpp.
References MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION.
|
overrideprotectedvirtual |
Must return the current versioning number of the object.
Start in zero for new classes, and increments each time there is a change in the stored format.
Implements mrpt::serialization::CSerializable.
Definition at line 633 of file CHMTSLAM_main.cpp.
|
overrideprotectedvirtual |
Pure virtual method for writing (serializing) to an abstract archive.
Users don't call this method directly. Instead, use stream << object;
.
std::exception | On any I/O error |
Implements mrpt::serialization::CSerializable.
Definition at line 634 of file CHMTSLAM_main.cpp.
|
inherited |
Set the name of the COutputLogger instance.
Definition at line 127 of file COutputLogger.cpp.
Referenced by mrpt::slam::CMetricMapBuilderICP::CMetricMapBuilderICP(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D(), mrpt::slam::CMonteCarloLocalization3D::CMonteCarloLocalization3D(), and mrpt::graphslam::CWindowManager::initCWindowManager().
|
inherited |
Set the minimum logging level for which the incoming logs are going to be taken into account.
String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance
Definition at line 133 of file COutputLogger.cpp.
Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), mrpt::hwdrivers::CHokuyoURG::initialize(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams().
|
inherited |
alias of setMinLoggingLevel()
Definition at line 138 of file COutputLogger.cpp.
Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::comms::CServerTCPSocket::CServerTCPSocket(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::math::ransac_detect_2D_lines().
|
staticprotected |
The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM.
LMH | (IN) The LMH which to this query applies. |
areaID | (IN) The area ID to consider for potential loop-closures. |
Definition at line 103 of file CHMTSLAM_TBI.cpp.
References ASSERT_, mrpt::poses::CPose3DPDFSOG::clear(), mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::TBI_info::delta_new_cur, IS_CLASS, mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::TBI_info::log_lik, mrpt::system::LVL_DEBUG, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, mrpt::hmtslam::CLocalMetricHypothesis::m_neighbors, mrpt::hmtslam::CLocalMetricHypothesis::m_parent, MRPT_END, MRPT_START, and mrpt::hmtslam::CLocalMetricHypothesis::updateAreaFromLMH().
|
protected |
The function for the "3D viewer" thread.
Definition at line 31 of file CHMTSLAM_3D_viewer.cpp.
References mrpt::system::LVL_DEBUG, mrpt::system::LVL_ERROR, and MRPT_TODO.
|
protected |
The function for the "Local SLAM" thread.
Definition at line 46 of file CHMTSLAM_LSLAM.cpp.
References ASSERT_, CLASS_ID, mrpt::random::getRandomGenerator(), mrpt::system::LVL_DEBUG, MRPT_TODO, mrpt::random::CRandomGenerator::randomize(), mrpt::system::CTicTac::Tac(), THROW_EXCEPTION, and mrpt::system::CTicTac::Tic().
|
protected |
The function for the "TBI" thread.
Definition at line 31 of file CHMTSLAM_TBI.cpp.
References mrpt::random::getRandomGenerator(), mrpt::system::LVL_DEBUG, mrpt::system::LVL_ERROR, MRPT_TODO, and mrpt::random::CRandomGenerator::randomize().
|
inherited |
Write the contents of the COutputLogger instance to an external file.
Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided
Definition at line 154 of file COutputLogger.cpp.
References ASSERTMSG_, and mrpt::format().
|
inlinevirtualinherited |
Introduces a pure virtual method responsible for writing to a mxArray
Matlab object, typically a MATLAB struct
whose contents are documented in each derived class.
mxArray
(caller is responsible of memory freeing) or nullptr is class does not support conversion to MATLAB. Definition at line 70 of file CSerializable.h.
|
friend |
Definition at line 71 of file CHMTSLAM.h.
|
friend |
Definition at line 72 of file CHMTSLAM.h.
|
friend |
Definition at line 74 of file CHMTSLAM.h.
|
friend |
Definition at line 73 of file CHMTSLAM.h.
|
staticprotected |
Definition at line 76 of file CHMTSLAM.h.
|
staticconstexpr |
Definition at line 76 of file CHMTSLAM.h.
|
inherited |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.
Definition at line 239 of file system/COutputLogger.h.
|
inherited |
[Default=false] Enables storing all messages into an internal list.
Definition at line 242 of file system/COutputLogger.h.
|
staticinherited |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.
Implementation file for the COutputLogger header class.
Handy for coloring the input based on the verbosity of the message
Definition at line 124 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().
|
staticinherited |
Map from VerbosityLevels to their corresponding names.
Handy for printing the current message VerbosityLevel along with the actual content
Definition at line 129 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::TMsg::getAsString().
|
mutableprivateinherited |
Definition at line 312 of file system/COutputLogger.h.
|
protected |
Definition at line 268 of file CHMTSLAM.h.
|
protected |
Threads handles.
Definition at line 268 of file CHMTSLAM.h.
|
protected |
Definition at line 268 of file CHMTSLAM.h.
|
protected |
The queue of pending actions/observations supplied by the user waiting for being processed.
Definition at line 243 of file CHMTSLAM.h.
|
mutableprotected |
Critical section for accessing m_inputQueue.
Definition at line 246 of file CHMTSLAM.h.
|
privateinherited |
Definition at line 314 of file system/COutputLogger.h.
mrpt::aligned_std_map<THypothesisID, CLocalMetricHypothesis> mrpt::hmtslam::CHMTSLAM::m_LMHs |
The list of LMHs at each instant.
Definition at line 418 of file CHMTSLAM.h.
|
mutableprotected |
Critical section for accessing m_LMHs.
Definition at line 252 of file CHMTSLAM.h.
|
privateinherited |
Definition at line 310 of file system/COutputLogger.h.
|
protected |
An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" or "loadState".
Definition at line 302 of file CHMTSLAM.h.
|
protected |
Definition at line 164 of file CHMTSLAM.h.
CHierarchicalMHMap mrpt::hmtslam::CHMTSLAM::m_map |
The hiearchical, multi-hypothesis graph-based map.
Definition at line 416 of file CHMTSLAM.h.
|
mutableprotected |
Critical section for accessing m_map.
Definition at line 249 of file CHMTSLAM.h.
|
protectedinherited |
Provided messages with VerbosityLevel smaller than this value shall be ignored.
Definition at line 252 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().
|
staticprotected |
Definition at line 355 of file CHMTSLAM.h.
|
staticprotected |
Definition at line 357 of file CHMTSLAM.h.
|
staticprotected |
Definition at line 356 of file CHMTSLAM.h.
|
protected |
|
protected |
Definition at line 312 of file CHMTSLAM.h.
|
protected |
Termination flag for signaling all threads to terminate.
Definition at line 338 of file CHMTSLAM.h.
|
protected |
Definition at line 343 of file CHMTSLAM.h.
|
protected |
Threads termination flags:
Definition at line 342 of file CHMTSLAM.h.
|
protected |
Definition at line 342 of file CHMTSLAM.h.
|
protected |
The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState".
Definition at line 316 of file CHMTSLAM.h.
|
protected |
The critical section for accessing m_topLCdets.
Definition at line 319 of file CHMTSLAM.h.
|
staticprotected |
Definition at line 76 of file CHMTSLAM.h.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST |