Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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::utils;
19 using namespace mrpt::obs;
20 using namespace mrpt::poses;
21 using namespace mrpt::synch;
22 using namespace mrpt::hmtslam;
23 using namespace std;
24 
25 CTopLCDetector_FabMap::CTopLCDetector_FabMap(CHMTSLAM *hmtslam) :
26  CTopLCDetectorBase(hmtslam),
27  m_fabmap(NULL)
28 {
29 #ifdef HTMSLAM_HAS_FABMAP
30  // Use already loaded options:
32 
33  unsigned int nVocabSize;
34  if (!fabmap::ParseOXV_PeekDimensions(o->vocab_path + o->vocabName + ".oxv",nVocabSize))
35  THROW_EXCEPTION_FMT("Error parsing vocabulary file: %s", std::string(o->vocab_path + o->vocabName + ".oxv").c_str() )
36 
37  m_fabmap = new fabmap::FabMapInstance( o->vocab_path,o->vocabName, o->p_obs_given_exists,0.1, o->p_at_new_place, nVocabSize, o->df_lik_smooth );
38 
39  // Restart msg:
40  cout << "[CTopLCDetector_FabMap::constructor] Resetting FabMap" << endl;
41  THE_FABMAP->hmtslam_restart();
42 #else
43  THROW_EXCEPTION("Please, recompile MRPT with FabMap to use this class.")
44 #endif
45 }
46 
48 {
49 #ifdef HTMSLAM_HAS_FABMAP
50  delete THE_FABMAP;
51  m_fabmap = NULL;
52 #endif
53 }
54 
55 /** This method must compute the topological observation model.
56  * \param out_log_lik The output, a log-likelihood.
57  * \return NULL, or a PDF of the estimated translation between the two areas (can be a multi-modal PDF).
58  */
60  const THypothesisID &hypID,
61  const CHMHMapNodePtr &currentArea,
62  const CHMHMapNodePtr &refArea,
63  double &out_log_lik
64  )
65 {
66  MRPT_UNUSED_PARAM(hypID); MRPT_UNUSED_PARAM(currentArea);
67  MRPT_UNUSED_PARAM(refArea); MRPT_UNUSED_PARAM(out_log_lik);
68  return CPose3DPDFPtr();
69 }
70 
71 /** Hook method for being warned about the insertion of a new poses into the maps.
72  * This should be independent of hypothesis IDs.
73  */
75  const TPoseID &poseID,
76  const CSensoryFrame *SF )
77 {
78 #ifdef HTMSLAM_HAS_FABMAP
79 
80  vector<string> lstObsImages;
81 
82  size_t n = 0;
83  CObservationImagePtr obsIm;
84  while ( (obsIm = SF->getObservationByClass<CObservationImage>(n)).present() )
85  {
86  string path;
87  obsIm->image.getExternalStorageFileAbsolutePath(path);
88  lstObsImages.push_back(path);
89  n++;
90  };
91 
92  if (lstObsImages.empty()) return; // Not all poses must have images.
93 
94  cout << "[OnNewPose] Adding new pose: " << poseID << " # of images: " << lstObsImages.size() << endl;
95  THE_FABMAP->hmtslam_addNewPose(poseID,lstObsImages);
96 
97 #else
99 #endif
100 }
101 
102 
103 
104 
105 // Initialization
107  vocab_path("./vocab"),
108  vocabName("vocab_name"),
109  p_obs_given_exists(0.39),
110  p_at_new_place(0.99),
111  df_lik_smooth(0.99)
112 {
113 }
114 
115 // Load parameters from configuration source
117  const mrpt::utils::CConfigFileBase &iniFile,
118  const std::string &section)
119 {
120  MRPT_LOAD_CONFIG_VAR(vocab_path,string, iniFile, section );
121  MRPT_LOAD_CONFIG_VAR(vocabName,string, iniFile, section );
122  MRPT_LOAD_CONFIG_VAR(p_obs_given_exists,double, iniFile, section );
123  MRPT_LOAD_CONFIG_VAR(p_at_new_place,double, iniFile, section );
124  MRPT_LOAD_CONFIG_VAR(df_lik_smooth,double, iniFile, section );
125 }
126 
127 // This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
129  out.printf("\n----------- [CTopLCDetector_FabMap::TOptions] ------------ \n\n");
130 
131  LOADABLEOPTS_DUMP_VAR(vocab_path, string)
132  LOADABLEOPTS_DUMP_VAR(vocabName, string)
133  LOADABLEOPTS_DUMP_VAR(p_obs_given_exists, double)
134  LOADABLEOPTS_DUMP_VAR(p_at_new_place, double)
135  LOADABLEOPTS_DUMP_VAR(df_lik_smooth, double)
136 }
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.
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:451
#define THROW_EXCEPTION(msg)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
This class allows loading and storing values and vectors of different types from a configuration text...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GLsizei n
Definition: glew.h:5051
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
mrpt::hmtslam::CHMTSLAM::TOptions m_options
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
#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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
Options for a TLC-detector of type FabMap, used from CHMTSLAM.
mrpt::poses::CPose3DPDFPtr computeTopologicalObservationModel(const THypothesisID &hypID, const CHMHMapNodePtr &currentArea, const CHMHMapNodePtr &refArea, double &out_log_lik)
This method must compute the topological observation model.
T::Ptr getObservationByClass(const size_t &ith=0) const
Returns the i'th observation of a given class (or of a descendant class), or NULL if there is no such...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018