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 }
mrpt::hmtslam::CTopLCDetector_GridMatching::TOptions
Options for a TLC-detector of type gridmap-matching, used from CHMTSLAM.
Definition: CTopLCDetector_GridMatching.h:56
filesystem.h
mrpt::config::CLoadableOptions::dumpToConsole
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
Definition: CLoadableOptions.cpp:44
mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel
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.
Definition: CTopLCDetector_GridMatching.cpp:36
mrpt::hmtslam::CTopLCDetector_GridMatching::OnNewPose
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.
Definition: CTopLCDetector_GridMatching.cpp:140
mrpt::hmtslam::CTopLCDetector_GridMatching::TOptions::matchingOptions
mrpt::slam::CGridMapAligner::TConfigParams matchingOptions
Options for the grid-to-grid matching algorithm.
Definition: CTopLCDetector_GridMatching.h:63
mrpt::io
Definition: img/CImage.h:22
mrpt::hmtslam::CHMTSLAM
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
CFileOutputStream.h
mrpt::io::CStream::printf_vector
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
mrpt::hmtslam::CTopLCDetector_GridMatching::TOptions::TOptions
TOptions()
Initialization of default params.
Definition: CTopLCDetector_GridMatching.cpp:148
CFileGZOutputStream.h
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::hmtslam::CTopLCDetector_GridMatching::TOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CTopLCDetector_GridMatching.cpp:158
mrpt::slam::CGridMapAligner::options
mrpt::slam::CGridMapAligner::TConfigParams options
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
hmtslam-precomp.h
mrpt::slam::CGridMapAligner::TReturnInfo::icp_goodness_all_sog_modes
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...
Definition: CGridMapAligner.h:200
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::hmtslam::CHMTSLAM::TOptions::LOG_OUTPUT_DIR
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
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::hmtslam::CTopLCDetectorBase::m_hmtslam
CHMTSLAM * m_hmtslam
Definition: CTopLCDetectorBase.h:29
mrpt::hmtslam::CTopLCDetector_GridMatching::~CTopLCDetector_GridMatching
virtual ~CTopLCDetector_GridMatching()
Destructor.
Definition: CTopLCDetector_GridMatching.cpp:30
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
mrpt::hmtslam
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Definition: CHierarchicalMapMHPartition.h:30
mrpt::system::COutputLogger::logFmt
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
Definition: COutputLogger.cpp:80
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::io::CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level.
Definition: io/CFileGZOutputStream.h:26
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:40
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:141
res
GLuint res
Definition: glext.h:7268
mrpt::maps::CMultiMetricMap::Ptr
std::shared_ptr< CMultiMetricMap > Ptr
Definition: CMultiMetricMap.h:143
mrpt::serialization
Definition: aligned_serialization.h:14
mrpt::slam::CGridMapAligner
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map,...
Definition: CGridMapAligner.h:43
mrpt::hmtslam::CHMTSLAM::saveState
bool saveState(mrpt::serialization::CArchive &out) const
Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
Definition: CHMTSLAM_main.cpp:580
mrpt::io::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: io/CFileOutputStream.h:24
mrpt::hmtslam::CHMTSLAM::m_options
mrpt::hmtslam::CHMTSLAM::TOptions m_options
mrpt::hmtslam::CHMTSLAM::TOptions::TLC_grid_options
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:507
mrpt::system::LVL_DEBUG
@ LVL_DEBUG
Definition: system/COutputLogger.h:30
mrpt::hmtslam::TPoseID
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Definition: HMT_SLAM_common.h:66
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:561
mrpt::slam::CGridMapAligner::TReturnInfo
The ICP algorithm return information.
Definition: CGridMapAligner.h:153
mrpt::hmtslam::THypothesisID
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Definition: HMT_SLAM_common.h:61
mrpt::hmtslam::CHMHMapNode::Ptr
std::shared_ptr< CHMHMapNode > Ptr
Definition: CHMHMapNode.h:42
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:45
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::slam::CGridMapAligner::TReturnInfo::landmarks_map1
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences")
Definition: CGridMapAligner.h:180
mrpt::hmtslam::CTopLCDetector_GridMatching::TOptions::loadFromConfigFile
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.
Definition: CTopLCDetector_GridMatching.cpp:150
mrpt::io::CStream::printf
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
mrpt::maps
Definition: CBeacon.h:24
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align
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.
Definition: CMetricMapsAlignmentAlgorithm.cpp:25
CArchive.h
mrpt::system::createDirectory
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:158
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:44
mrpt::slam
Definition: CMultiMetricMapPDF.h:27
mrpt::slam::CGridMapAligner::TReturnInfo::landmarks_map2
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
Definition: CGridMapAligner.h:180
mrpt::hmtslam::CTopLCDetectorBase
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
Definition: CTopLCDetectorBase.h:26
size
GLsizeiptr size
Definition: glext.h:3923
NODE_ANNOTATION_METRIC_MAPS
#define NODE_ANNOTATION_METRIC_MAPS
Definition: HMT_SLAM_common.h:18



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST