MRPT  2.0.4
CTopLCDetector_FabMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hmtslam-precomp.h" // Precomp header
11 
12 #ifdef HTMSLAM_HAS_FABMAP
13 #include <FabMapLibInterface.h>
14 #define THE_FABMAP static_cast<fabmap::FabMapInstance*>(m_fabmap)
15 #endif
16 
17 using namespace mrpt;
18 using namespace mrpt::obs;
19 using namespace mrpt::poses;
20 using namespace mrpt::hmtslam;
21 using namespace std;
22 
23 CTopLCDetector_FabMap::CTopLCDetector_FabMap(CHMTSLAM* hmtslam)
24  : CTopLCDetectorBase(hmtslam), m_fabmap(nullptr)
25 {
26 #ifdef HTMSLAM_HAS_FABMAP
27  // Use already loaded options:
30 
31  unsigned int nVocabSize;
32  if (!fabmap::ParseOXV_PeekDimensions(
33  o->vocab_path + o->vocabName + ".oxv", nVocabSize))
35  "Error parsing vocabulary file: %s",
36  std::string(o->vocab_path + o->vocabName + ".oxv").c_str())
37 
38  m_fabmap = new fabmap::FabMapInstance(
39  o->vocab_path, o->vocabName, o->p_obs_given_exists, 0.1,
40  o->p_at_new_place, nVocabSize, o->df_lik_smooth);
41 
42  // Restart msg:
43  cout << "[CTopLCDetector_FabMap::constructor] Resetting FabMap" << endl;
44  THE_FABMAP->hmtslam_restart();
45 #else
46  THROW_EXCEPTION("Please, recompile MRPT with FabMap to use this class.");
47 #endif
48 }
49 
51 {
52 #ifdef HTMSLAM_HAS_FABMAP
53  delete THE_FABMAP;
54  m_fabmap = nullptr;
55 #endif
56 }
57 
58 /** This method must compute the topological observation model.
59  * \param out_log_lik The output, a log-likelihood.
60  * \return nullptr, or a PDF of the estimated translation between the two areas
61  * (can be a multi-modal PDF).
62  */
64  [[maybe_unused]] const THypothesisID& hypID,
65  [[maybe_unused]] const CHMHMapNode::Ptr& currentArea,
66  [[maybe_unused]] const CHMHMapNode::Ptr& refArea,
67  [[maybe_unused]] double& out_log_lik)
68 {
69  return CPose3DPDF::Ptr();
70 }
71 
72 /** Hook method for being warned about the insertion of a new poses into the
73  * maps.
74  * This should be independent of hypothesis IDs.
75  */
77  [[maybe_unused]] const TPoseID& poseID,
78  [[maybe_unused]] const CSensoryFrame* SF)
79 {
80 #ifdef HTMSLAM_HAS_FABMAP
81 
82  vector<string> lstObsImages;
83 
84  size_t n = 0;
86  while ((obsIm = SF->getObservationByClass<CObservationImage>(n)))
87  {
88  string path;
89  obsIm->image.getExternalStorageFileAbsolutePath(path);
90  lstObsImages.push_back(path);
91  n++;
92  };
93 
94  if (lstObsImages.empty()) return; // Not all poses must have images.
95 
96  cout << "[OnNewPose] Adding new pose: " << poseID
97  << " # of images: " << lstObsImages.size() << endl;
98  THE_FABMAP->hmtslam_addNewPose(poseID, lstObsImages);
99 #endif
100 }
101 
102 // Initialization
104  : vocab_path("./vocab"), vocabName("vocab_name")
105 
106 {
107 }
108 
109 // Load parameters from configuration source
111  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
112 {
113  MRPT_LOAD_CONFIG_VAR(vocab_path, string, iniFile, section);
114  MRPT_LOAD_CONFIG_VAR(vocabName, string, iniFile, section);
115  MRPT_LOAD_CONFIG_VAR(p_obs_given_exists, double, iniFile, section);
116  MRPT_LOAD_CONFIG_VAR(p_at_new_place, double, iniFile, section);
117  MRPT_LOAD_CONFIG_VAR(df_lik_smooth, double, iniFile, section);
118 }
119 
120 // This method must display clearly all the contents of the structure in
121 // textual form, sending it to a CStream.
123 {
124  out << "\n----------- [CTopLCDetector_FabMap::TOptions] ------------ \n\n";
125 
126  LOADABLEOPTS_DUMP_VAR(vocab_path, string)
127  LOADABLEOPTS_DUMP_VAR(vocabName, string)
128  LOADABLEOPTS_DUMP_VAR(p_obs_given_exists, double)
129  LOADABLEOPTS_DUMP_VAR(p_at_new_place, double)
130  LOADABLEOPTS_DUMP_VAR(df_lik_smooth, double)
131 }
mrpt::hmtslam::CTopLCDetector_FabMap::~CTopLCDetector_FabMap
~CTopLCDetector_FabMap() override
Destructor.
Definition: CTopLCDetector_FabMap.cpp:50
mrpt::hmtslam::CHMHMapNode::Ptr
std::shared_ptr< mrpt::hmtslam ::CHMHMapNode > Ptr
Definition: CHMHMapNode.h:39
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
mrpt::hmtslam::CHMTSLAM
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
mrpt::obs::CObservationImage::Ptr
std::shared_ptr< mrpt::obs ::CObservationImage > Ptr
Definition: CObservationImage.h:34
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::vocabName
std::string vocabName
Definition: CTopLCDetector_FabMap.h:65
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::df_lik_smooth
double df_lik_smooth
Definition: CTopLCDetector_FabMap.h:67
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
hmtslam-precomp.h
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::hmtslam::CTopLCDetectorBase::m_hmtslam
CHMTSLAM * m_hmtslam
Definition: CTopLCDetectorBase.h:26
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
mrpt::hmtslam
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Definition: CHierarchicalMapMHPartition.h:27
mrpt::hmtslam::CHMTSLAM::TOptions::TLC_fabmap_options
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:508
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::vocab_path
std::string vocab_path
Definition: CTopLCDetector_FabMap.h:65
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::TOptions
TOptions()
Initialization of default params.
Definition: CTopLCDetector_FabMap.cpp:103
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::hmtslam::CTopLCDetector_FabMap::m_fabmap
void * m_fabmap
Definition: CTopLCDetector_FabMap.h:21
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::p_obs_given_exists
double p_obs_given_exists
Definition: CTopLCDetector_FabMap.h:66
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions
Options for a TLC-detector of type FabMap, used from CHMTSLAM.
Definition: CTopLCDetector_FabMap.h:53
mrpt::hmtslam::CTopLCDetector_FabMap::OnNewPose
void OnNewPose(const TPoseID &poseID, const mrpt::obs::CSensoryFrame *SF) override
Hook method for being warned about the insertion of a new poses into the maps.
Definition: CTopLCDetector_FabMap.cpp:76
LOADABLEOPTS_DUMP_VAR
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
Definition: config/CLoadableOptions.h:101
mrpt::hmtslam::CHMTSLAM::m_options
mrpt::hmtslam::CHMTSLAM::TOptions m_options
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CTopLCDetector_FabMap.cpp:110
mrpt::hmtslam::TPoseID
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Definition: HMT_SLAM_common.h:63
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CTopLCDetector_FabMap.cpp:122
mrpt::hmtslam::THypothesisID
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Definition: HMT_SLAM_common.h:58
mrpt::hmtslam::CTopLCDetector_FabMap::computeTopologicalObservationModel
mrpt::poses::CPose3DPDF::Ptr computeTopologicalObservationModel(const THypothesisID &hypID, const CHMHMapNode::Ptr &currentArea, const CHMHMapNode::Ptr &refArea, double &out_log_lik) override
This method must compute the topological observation model.
Definition: CTopLCDetector_FabMap.cpp:63
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions::p_at_new_place
double p_at_new_place
Definition: CTopLCDetector_FabMap.h:66
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
mrpt::hmtslam::CTopLCDetectorBase
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
Definition: CTopLCDetectorBase.h:23
mrpt::obs::CObservationImage
Declares a class derived from "CObservation" that encapsules an image from a camera,...
Definition: CObservationImage.h:32



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020