Main MRPT website > C++ reference for MRPT 1.9.9
CTopLCDetector_FabMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://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  const THypothesisID& hypID, const CHMHMapNode::Ptr& currentArea,
65  const CHMHMapNode::Ptr& refArea, double& out_log_lik)
66 {
67  MRPT_UNUSED_PARAM(hypID);
68  MRPT_UNUSED_PARAM(currentArea);
69  MRPT_UNUSED_PARAM(refArea);
70  MRPT_UNUSED_PARAM(out_log_lik);
71  return CPose3DPDF::Ptr();
72 }
73 
74 /** Hook method for being warned about the insertion of a new poses into the
75  * maps.
76  * This should be independent of hypothesis IDs.
77  */
79  const TPoseID& poseID, const CSensoryFrame* SF)
80 {
81 #ifdef HTMSLAM_HAS_FABMAP
82 
83  vector<string> lstObsImages;
84 
85  size_t n = 0;
87  while ((obsIm = SF->getObservationByClass<CObservationImage>(n)))
88  {
89  string path;
90  obsIm->image.getExternalStorageFileAbsolutePath(path);
91  lstObsImages.push_back(path);
92  n++;
93  };
94 
95  if (lstObsImages.empty()) return; // Not all poses must have images.
96 
97  cout << "[OnNewPose] Adding new pose: " << poseID
98  << " # of images: " << lstObsImages.size() << endl;
99  THE_FABMAP->hmtslam_addNewPose(poseID, lstObsImages);
100 
101 #else
102  MRPT_UNUSED_PARAM(poseID);
103  MRPT_UNUSED_PARAM(SF);
104 #endif
105 }
106 
107 // Initialization
109  : vocab_path("./vocab"),
110  vocabName("vocab_name"),
111  p_obs_given_exists(0.39),
112  p_at_new_place(0.99),
113  df_lik_smooth(0.99)
114 {
115 }
116 
117 // Load parameters from configuration source
119  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
120 {
121  MRPT_LOAD_CONFIG_VAR(vocab_path, string, iniFile, section);
122  MRPT_LOAD_CONFIG_VAR(vocabName, string, iniFile, section);
123  MRPT_LOAD_CONFIG_VAR(p_obs_given_exists, double, iniFile, section);
124  MRPT_LOAD_CONFIG_VAR(p_at_new_place, double, iniFile, section);
125  MRPT_LOAD_CONFIG_VAR(df_lik_smooth, double, iniFile, section);
126 }
127 
128 // This method must display clearly all the contents of the structure in
129 // textual form, sending it to a CStream.
131 {
132  out << mrpt::format(
133  "\n----------- [CTopLCDetector_FabMap::TOptions] ------------ \n\n");
134 
135  LOADABLEOPTS_DUMP_VAR(vocab_path, string)
136  LOADABLEOPTS_DUMP_VAR(vocabName, string)
137  LOADABLEOPTS_DUMP_VAR(p_obs_given_exists, double)
138  LOADABLEOPTS_DUMP_VAR(p_at_new_place, double)
139  LOADABLEOPTS_DUMP_VAR(df_lik_smooth, double)
140 }
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
void OnNewPose(const TPoseID &poseID, const mrpt::obs::CSensoryFrame *SF)
Hook method for being warned about the insertion of a new poses into the maps.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:509
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
GLenum GLsizei n
Definition: glext.h:5074
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:68
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...
mrpt::poses::CPose3DPDF::Ptr computeTopologicalObservationModel(const THypothesisID &hypID, const CHMHMapNode::Ptr &currentArea, const CHMHMapNode::Ptr &refArea, double &out_log_lik)
This method must compute the topological observation model.
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:54
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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.
GLsizei const GLchar ** string
Definition: glext.h:4101
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.
T::Ptr getObservationByClass(const size_t &ith=0) const
Returns the i&#39;th observation of a given class (or of a descendant class), or nullptr if there is no s...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019