MRPT  2.0.4
CTopLCDetector_GridMatching.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-2020, 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 
15 #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->countMapsByClass<COccupancyGridMap2D>() >= 1);
66  ASSERT_(hMapCur->countMapsByClass<COccupancyGridMap2D>() >= 1);
67 
68  gridAligner.options.dumpToConsole();
69 
70  // Do the map align:
71  CPosePDF::Ptr alignRes = gridAligner.Align(
72  hMapCur.get(), // "ref" as seen from "cur"...The order is critical!!!
73  hMapRef.get(), initEstimate, info);
74 
75 #if 0
76  {
77  hMapCur->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map1.png", info.landmarks_map1.get());
78  hMapRef->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map2.png", info.landmarks_map2.get());
79  }
80 #endif
81 
82  // Transform the 2D SOG into a 3D SOG:
83  CPose3DPDF::Ptr res = CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*alignRes));
84 
85 // --------------------
86 // Debug output:
87 // --------------------
88 #if 1
89  const std::string dbg_dir =
90  m_hmtslam->m_options.LOG_OUTPUT_DIR + "/TBI_TESTS";
91  if (!m_hmtslam->m_options.LOG_OUTPUT_DIR.empty())
92  {
94  static int cnt = 0;
95  ++cnt;
96  const std::string filStat =
97  dbg_dir + format(
98  "/state_%05i_test_%i_%i.hmtslam", cnt,
99  (int)currentArea->getID(), (int)refArea->getID());
100  const std::string filRes =
101  dbg_dir + format(
102  "/state_%05i_test_%i_%i_result.txt", cnt,
103  (int)currentArea->getID(), (int)refArea->getID());
104 
105  m_hmtslam->logFmt(
106  mrpt::system::LVL_DEBUG, "[TLCD_gridmatch] DEBUG: Saving %s\n",
107  filStat.c_str());
108  CFileGZOutputStream f(filStat);
109  auto arch = mrpt::serialization::archiveFrom(f);
110  this->m_hmtslam->saveState(arch);
111 
112  m_hmtslam->logFmt(
113  mrpt::system::LVL_DEBUG, "[TLCD_gridmatch] DEBUG: Saving %s\n",
114  filRes.c_str());
115  CFileOutputStream f_res(filRes);
116  f_res.printf(
117  "# SOG modes: %i\n",
118  (int)std::dynamic_pointer_cast<CPosePDFSOG>(alignRes)->size());
119  f_res.printf("ICP goodness: ");
120  f_res.printf_vector("%f ", info.icp_goodness_all_sog_modes);
121  f_res.printf("\n");
122  }
123 #endif
124 
125  return res;
126 }
127 
128 /** Hook method for being warned about the insertion of a new poses into the
129  * maps.
130  * This should be independent of hypothesis IDs.
131  */
133  [[maybe_unused]] const TPoseID& poseID,
134  [[maybe_unused]] const CSensoryFrame* SF)
135 {
136 }
137 
138 // Initialization
140 // Load parameters from configuration source
142  const mrpt::config::CConfigFileBase& source, const std::string& section)
143 {
144  matchingOptions.loadFromConfigFile(source, section);
145 }
146 
147 // This method must display clearly all the contents of the structure in
148 // textual form, sending it to a CStream.
150  std::ostream& out) const
151 {
152  out << "\n----------- [CTopLCDetector_GridMatching::TOptions] ------------ "
153  "\n\n";
154  matchingOptions.dumpToTextStream(out);
155 }
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
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:161
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...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
size_t size(const MATRIXLIKE &m, const int dim)
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
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:506
The ICP algorithm return information.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
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:592
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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:447
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 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.
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:51
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
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") ...
A class for storing an occupancy grid map.
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:39
mrpt::vision::TStereoCalibResults out
~CTopLCDetector_GridMatching() override
Destructor.
mrpt::slam::CGridMapAligner::TConfigParams options
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
Saves data to a file and transparently compress the data using the given compression level...
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.
This class stores any customizable set of metric maps.
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
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:102



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020