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 }
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:508
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.
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
STL namespace.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
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.
This class allows loading and storing values and vectors of different types from a configuration text...
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
mrpt::hmtslam::CHMTSLAM::TOptions m_options
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
#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...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Options for a TLC-detector of type FabMap, used from CHMTSLAM.
mrpt::vision::TStereoCalibResults out
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
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.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020