Main MRPT website > C++ reference for MRPT 1.9.9
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-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 
14 #include <mrpt/system/filesystem.h>
16 
17 using namespace mrpt::slam;
18 using namespace mrpt::hmtslam;
19 using namespace mrpt::poses;
20 using namespace mrpt::obs;
21 using namespace mrpt::io;
22 using namespace mrpt::serialization;
23 using namespace mrpt::maps;
24 
25 CTopLCDetector_GridMatching::CTopLCDetector_GridMatching(CHMTSLAM* hmtslam)
26  : CTopLCDetectorBase(hmtslam)
27 {
28 }
29 
31 /** This method must compute the topological observation model.
32  * \param out_log_lik The output, a log-likelihood.
33  * \return nullptr, or a PDF of the estimated translation between the two areas
34  * (can be a multi-modal PDF).
35  */
37  const THypothesisID& hypID, const CHMHMapNode::Ptr& currentArea,
38  const CHMHMapNode::Ptr& refArea, double& out_log_lik)
39 {
40  // ------------------------------------------------------------------------------
41  // Compute potential map transformations between the areas "areaID" and
42  // "a->first"
43  // For this, we use the grid-to-grid matching method described in the TRO
44  // paper...
45  // ------------------------------------------------------------------------------
46  out_log_lik = 0; // Nothing to modify, for now.
47 
48  CGridMapAligner gridAligner;
50  const CPose2D initEstimate(
51  0, 0, 0); // It's actually ignored by the grid-matching
52 
53  // Use already loaded options:
56  gridAligner.options = o.matchingOptions;
57 
58  CMultiMetricMap::Ptr hMapCur =
59  currentArea->m_annotations.getAs<CMultiMetricMap>(
60  NODE_ANNOTATION_METRIC_MAPS, hypID, false);
61  CMultiMetricMap::Ptr hMapRef =
62  refArea->m_annotations.getAs<CMultiMetricMap>(
63  NODE_ANNOTATION_METRIC_MAPS, hypID, false);
64 
65  ASSERT_(hMapRef->m_gridMaps.size() >= 1);
66  ASSERT_(hMapCur->m_gridMaps.size() >= 1);
67 
68 #if 0
69  {
70  static int i = 0;
71  CFileOutputStream f(format("debug_%05i.hmtslam",++i));
72  f << *m_hmtslam;
73  }
74 #endif
75 
76  gridAligner.options.dumpToConsole();
77 
78  // Do the map align:
79  CPosePDF::Ptr alignRes = gridAligner.Align(
80  hMapCur.get(), // "ref" as seen from "cur"...The order is critical!!!
81  hMapRef.get(), initEstimate, nullptr, &info);
82 
83 #if 0
84  {
85  hMapCur->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map1.png", info.landmarks_map1.get());
86  hMapRef->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map2.png", info.landmarks_map2.get());
87  }
88 #endif
89 
90  // Transform the 2D SOG into a 3D SOG:
91  CPose3DPDF::Ptr res = CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*alignRes));
92 
93 // --------------------
94 // Debug output:
95 // --------------------
96 #if 1
97  const std::string dbg_dir =
98  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 =
105  dbg_dir + format(
106  "/state_%05i_test_%i_%i.hmtslam", cnt,
107  (int)currentArea->getID(), (int)refArea->getID());
108  const std::string filRes =
109  dbg_dir + format(
110  "/state_%05i_test_%i_%i_result.txt", cnt,
111  (int)currentArea->getID(), (int)refArea->getID());
112 
113  m_hmtslam->logFmt(
114  mrpt::system::LVL_DEBUG, "[TLCD_gridmatch] DEBUG: Saving %s\n",
115  filStat.c_str());
116  CFileGZOutputStream f(filStat);
117  auto arch = mrpt::serialization::archiveFrom(f);
118  this->m_hmtslam->saveState(arch);
119 
120  m_hmtslam->logFmt(
121  mrpt::system::LVL_DEBUG, "[TLCD_gridmatch] DEBUG: Saving %s\n",
122  filRes.c_str());
123  CFileOutputStream f_res(filRes);
124  f_res.printf(
125  "# SOG modes: %i\n",
126  (int)std::dynamic_pointer_cast<CPosePDFSOG>(alignRes)->size());
127  f_res.printf("ICP goodness: ");
128  f_res.printf_vector("%f ", info.icp_goodness_all_sog_modes);
129  f_res.printf("\n");
130  }
131 #endif
132 
133  return res;
134 }
135 
136 /** Hook method for being warned about the insertion of a new poses into the
137  * maps.
138  * This should be independent of hypothesis IDs.
139  */
141  const TPoseID& poseID, const CSensoryFrame* SF)
142 {
143  MRPT_UNUSED_PARAM(poseID);
144  MRPT_UNUSED_PARAM(SF);
145 }
146 
147 // Initialization
149 // Load parameters from configuration source
151  const mrpt::config::CConfigFileBase& source, const std::string& section)
152 {
153  matchingOptions.loadFromConfigFile(source, section);
154 }
155 
156 // This method must display clearly all the contents of the structure in
157 // textual form, sending it to a CStream.
159  std::ostream& out) const
160 {
161  out << mrpt::format(
162  "\n----------- [CTopLCDetector_GridMatching::TOptions] ------------ "
163  "\n\n");
164  matchingOptions.dumpToTextStream(out);
165 }
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:30
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:158
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
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.
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
mrpt::slam::CGridMapAligner::TConfigParams matchingOptions
Options for the grid-to-grid matching algorithm.
bool saveState(mrpt::serialization::CArchive &out) const
Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:507
The ICP algorithm return information.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream
Definition: CArchive.h:561
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
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:448
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.
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 namespace contains representation of robot actions and observations.
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.
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
mrpt::hmtslam::CHMTSLAM::TOptions m_options
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
This CStream derived class allow using a file as a write-only, binary stream.
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") ...
std::vector< double > icp_goodness_all_sog_modes
The ICP goodness of all potential SOG modes at the stage "sog2", thus before the removing of "bad" IC...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
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:40
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::slam::CGridMapAligner::TConfigParams options
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
GLsizeiptr size
Definition: glext.h:3923
GLuint res
Definition: glext.h:7268
Saves data to a file and transparently compress the data using the given compression level...
This class stores any customizable set of metric maps.
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
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 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: io/CStream.h:104
#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