MRPT  1.9.9
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-2019, 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  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"), vocabName("vocab_name")
110 
111 {
112 }
113 
114 // Load parameters from configuration source
116  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
117 {
118  MRPT_LOAD_CONFIG_VAR(vocab_path, string, iniFile, section);
119  MRPT_LOAD_CONFIG_VAR(vocabName, string, iniFile, section);
120  MRPT_LOAD_CONFIG_VAR(p_obs_given_exists, double, iniFile, section);
121  MRPT_LOAD_CONFIG_VAR(p_at_new_place, double, iniFile, section);
122  MRPT_LOAD_CONFIG_VAR(df_lik_smooth, double, iniFile, section);
123 }
124 
125 // This method must display clearly all the contents of the structure in
126 // textual form, sending it to a CStream.
128 {
129  out << mrpt::format(
130  "\n----------- [CTopLCDetector_FabMap::TOptions] ------------ \n\n");
131 
132  LOADABLEOPTS_DUMP_VAR(vocab_path, string)
133  LOADABLEOPTS_DUMP_VAR(vocabName, string)
134  LOADABLEOPTS_DUMP_VAR(p_obs_given_exists, double)
135  LOADABLEOPTS_DUMP_VAR(p_at_new_place, double)
136  LOADABLEOPTS_DUMP_VAR(df_lik_smooth, double)
137 }
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.
GLenum GLsizei n
Definition: glext.h:5136
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.
GLsizei const GLchar ** string
Definition: glext.h:4116
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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: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.
#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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019