Main MRPT website > C++ reference for MRPT 1.5.7
CTopLCDetector_GridMatching.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 
14 #include <mrpt/system/filesystem.h>
15 
16 using namespace mrpt::slam;
17 using namespace mrpt::hmtslam;
18 using namespace mrpt::utils;
19 using namespace mrpt::poses;
20 using namespace mrpt::obs;
21 using namespace mrpt::maps;
22 
23 
24 CTopLCDetector_GridMatching::CTopLCDetector_GridMatching( CHMTSLAM *hmtslam ) :
25  CTopLCDetectorBase(hmtslam)
26 {
27 
28 }
29 
31 {
32 
33 }
34 
35 /** This method must compute the topological observation model.
36  * \param out_log_lik The output, a log-likelihood.
37  * \return NULL, or a PDF of the estimated translation between the two areas (can be a multi-modal PDF).
38  */
40  const THypothesisID &hypID,
41  const CHMHMapNodePtr &currentArea,
42  const CHMHMapNodePtr &refArea,
43  double &out_log_lik
44  )
45 {
46  // ------------------------------------------------------------------------------
47  // Compute potential map transformations between the areas "areaID" and "a->first"
48  // For this, we use the grid-to-grid matching method described in the TRO paper...
49  // ------------------------------------------------------------------------------
50  out_log_lik = 0; // Nothing to modify, for now.
51 
52  CGridMapAligner gridAligner;
54  const CPose2D initEstimate(0,0,0); // It's actually ignored by the grid-matching
55 
56  // Use already loaded options:
58  gridAligner.options = o.matchingOptions;
59 
60  CMultiMetricMapPtr hMapCur = currentArea->m_annotations.getAs<CMultiMetricMap>(NODE_ANNOTATION_METRIC_MAPS, hypID, false);
61  CMultiMetricMapPtr hMapRef = refArea->m_annotations.getAs<CMultiMetricMap>(NODE_ANNOTATION_METRIC_MAPS, hypID, false);
62 
63  ASSERT_( hMapRef->m_gridMaps.size()>=1 )
64  ASSERT_( hMapCur->m_gridMaps.size()>=1 )
65 
66 #if 0
67  {
68  static int i = 0;
69  CFileOutputStream f(format("debug_%05i.hmtslam",++i));
70  f << *m_hmtslam;
71  }
72 #endif
73 
74  gridAligner.options.dumpToConsole();
75 
76  // Do the map align:
77  CPosePDFPtr alignRes = gridAligner.Align(
78  hMapCur.pointer(), // "ref" as seen from "cur"...The order is critical!!!
79  hMapRef.pointer(),
80  initEstimate,
81  NULL,
82  &info);
83 
84 #if 0
85  {
86  hMapCur->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map1.png", info.landmarks_map1.pointer());
87  hMapRef->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map2.png", info.landmarks_map2.pointer());
88  }
89 #endif
90 
91  // Transform the 2D SOG into a 3D SOG:
92  CPose3DPDFPtr res = CPose3DPDFPtr( CPose3DPDF::createFrom2D(*alignRes) );
93 
94  // --------------------
95  // Debug output:
96  // --------------------
97 #if 1
98  const std::string dbg_dir = m_hmtslam->m_options.LOG_OUTPUT_DIR + "/TBI_TESTS";
99  if (!m_hmtslam->m_options.LOG_OUTPUT_DIR.empty())
100  {
102  static int cnt = 0;
103  ++cnt;
104  const std::string filStat = dbg_dir+format("/state_%05i_test_%i_%i.hmtslam",cnt,(int)currentArea->getID(),(int)refArea->getID());
105  const std::string filRes = dbg_dir+format("/state_%05i_test_%i_%i_result.txt",cnt,(int)currentArea->getID(),(int)refArea->getID() );
106 
107 
108  m_hmtslam->logFmt(mrpt::utils::LVL_DEBUG,"[TLCD_gridmatch] DEBUG: Saving %s\n",filStat.c_str());
109  CFileGZOutputStream f(filStat);
110  this->m_hmtslam->saveState(f);
111 
112  m_hmtslam->logFmt(mrpt::utils::LVL_DEBUG,"[TLCD_gridmatch] DEBUG: Saving %s\n",filRes.c_str());
113  CFileOutputStream f_res(filRes);
114  f_res.printf("# SOG modes: %i\n",(int)CPosePDFSOGPtr(alignRes)->size() );
115  f_res.printf("ICP goodness: ");
116  f_res.printf_vector("%f ",info.icp_goodness_all_sog_modes);
117  f_res.printf("\n");
118  }
119 #endif
120 
121  return res;
122 }
123 
124 /** Hook method for being warned about the insertion of a new poses into the maps.
125  * This should be independent of hypothesis IDs.
126  */
128  const TPoseID &poseID,
129  const CSensoryFrame *SF )
130 {
132 }
133 
134 
135 // Initialization
137  matchingOptions ()
138 {
139 }
140 
141 // Load parameters from configuration source
144  const std::string &section)
145 {
146  matchingOptions.loadFromConfigFile(source,section);
147 }
148 
149 // This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
151  out.printf("\n----------- [CTopLCDetector_GridMatching::TOptions] ------------ \n\n");
152  matchingOptions.dumpToTextStream(out);
153 }
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::poses::CPosePDFPtr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=NULL, void *info=NULL)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
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.
mrpt::slam::CGridMapAligner::TConfigParams matchingOptions
Options for the grid-to-grid matching algorithm.
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:450
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
The ICP algorithm return information.
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
std::string LOG_OUTPUT_DIR
[LOGGING] If it is not an empty string (""), a directory with that name will be created and log files...
Definition: CHMTSLAM.h:405
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
This CStream derived class allow using a file as a write-only, binary stream.
virtual int void printf_vector(const char *fmt, const CONTAINER_TYPE &V, char separator=',')
Prints a vector in the format [A,B,C,...] using CStream::printf, and the fmt string for each vector e...
Definition: CStream.h:208
mrpt::maps::CMultiMetricMapPtr CMultiMetricMapPtr
Backward compatible typedef.
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
mrpt::hmtslam::CHMTSLAM::TOptions m_options
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
bool saveState(mrpt::utils::CStream &out) const
Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
Options for a TLC-detector of type gridmap-matching, used from CHMTSLAM.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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.
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.
backing_store_ptr info
Definition: jmemsys.h:170
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#define ASSERT_(f)
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
mrpt::slam::CGridMapAligner::TConfigParams options
GLsizeiptr size
Definition: glext.h:3779
GLuint res
Definition: glext.h:6298
This class stores any customizable set of metric maps.
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.
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019