Go to the documentation of this file.
25 CTopLCDetector_GridMatching::CTopLCDetector_GridMatching(
CHMTSLAM* hmtslam)
65 ASSERT_(hMapRef->m_gridMaps.size() >= 1);
66 ASSERT_(hMapCur->m_gridMaps.size() >= 1);
81 hMapRef.get(), initEstimate,
nullptr, &info);
85 hMapCur->m_gridMaps[0]->saveAsBitmapFileWithLandmarks(
"map1.png", info.
landmarks_map1.get());
86 hMapRef->m_gridMaps[0]->saveAsBitmapFileWithLandmarks(
"map2.png", info.
landmarks_map2.get());
106 "/state_%05i_test_%i_%i.hmtslam", cnt,
107 (
int)currentArea->getID(), (
int)refArea->getID());
110 "/state_%05i_test_%i_%i_result.txt", cnt,
111 (
int)currentArea->getID(), (
int)refArea->getID());
126 (
int)std::dynamic_pointer_cast<CPosePDFSOG>(alignRes)->
size());
127 f_res.
printf(
"ICP goodness: ");
153 matchingOptions.loadFromConfigFile(
source, section);
159 std::ostream& out)
const
162 "\n----------- [CTopLCDetector_GridMatching::TOptions] ------------ "
164 matchingOptions.dumpToTextStream(out);
Options for a TLC-detector of type gridmap-matching, used from CHMTSLAM.
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
mrpt::poses::CPose3DPDF::Ptr computeTopologicalObservationModel(const THypothesisID &hypID, const CHMHMapNode::Ptr ¤tArea, const CHMHMapNode::Ptr &refArea, double &out_log_lik)
This method must compute the topological observation model.
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.
mrpt::slam::CGridMapAligner::TConfigParams matchingOptions
Options for the grid-to-grid matching algorithm.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
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...
TOptions()
Initialization of default params.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
mrpt::slam::CGridMapAligner::TConfigParams options
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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...
This namespace contains representation of robot actions and observations.
std::string LOG_OUTPUT_DIR
[LOGGING] If it is not an empty string (""), a directory with that name will be created and log files...
GLsizei GLsizei GLchar * source
virtual ~CTopLCDetector_GridMatching()
Destructor.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
This class allows loading and storing values and vectors of different types from a configuration text...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Saves data to a file and transparently compress the data using the given compression level.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
This class stores any customizable set of metric maps.
std::shared_ptr< CMultiMetricMap > Ptr
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map,...
bool saveState(mrpt::serialization::CArchive &out) const
Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
This CStream derived class allow using a file as a write-only, binary stream.
mrpt::hmtslam::CHMTSLAM::TOptions m_options
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
The ICP algorithm return information.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
std::shared_ptr< CHMHMapNode > Ptr
std::shared_ptr< CPose3DPDF > Ptr
GLsizei const GLchar ** string
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences")
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) 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.
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.
bool createDirectory(const std::string &dirName)
Creates a directory.
std::shared_ptr< CPosePDF > Ptr
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
#define NODE_ANNOTATION_METRIC_MAPS
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 | |