Go to the documentation of this file.
9 #ifndef CMetricMapBuilderICP_H
10 #define CMetricMapBuilderICP_H
52 std::ostream& out)
const override;
156 const std::string& file,
bool formatEMF_BMP =
true)
override;
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
mrpt::maps::CSimpleMap SF_Poses_seq
The set of observations that leads to current map:
std::shared_ptr< CObservation > Ptr
void updateDistances(const mrpt::poses::CPose2D &p)
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) override
Appends a new action and observations to update this map: See the description of the class at the top...
mrpt::system::VerbosityLevel & verbosity_level
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
bool m_there_has_been_an_odometry
The ICP algorithm configuration data.
void getCurrentMapPoints(std::vector< float > &x, std::vector< float > &y)
Returns the 2D points of current local map.
mrpt::math::CMatrixDouble33 m_lastPoseEst_cov
Last pose estimation (covariance)
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) override
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
A class for very simple 2D SLAM based on ICP.
mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst
The pose estimation by the alignment algorithm (ICP).
mrpt::aligned_std_map< std::string, TDist > m_distSinceLastInsertion
Indexed by sensor label.
std::map< KEY, VALUE, std::less< KEY >, mrpt::aligned_allocator_cpp11< std::pair< const KEY, VALUE > >> aligned_std_map
CMetricMapBuilderICP()
Default constructor - Upon construction, you can set the parameters in ICP_options,...
std::string currentMapFile
Current map file.
Declares a class for storing a collection of robot actions.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool matchAgainstTheGrid
(default:false) Match against the occupancy grid or the points map? The former is quicker but less pr...
void updatePose(const mrpt::poses::CPose2D &p)
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
std::deque< mrpt::math::TPose2D > m_estRobotPath
The estimated robot path:
GLsizei GLsizei GLchar * source
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
TConfigParams & operator=(const TConfigParams &other)
double minICPgoodnessToAccept
Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0....
VerbosityLevel
Enumeration of available verbosity levels.
This class allows loading and storing values and vectors of different types from a configuration text...
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.
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.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void resetRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
TConfigParams(mrpt::system::VerbosityLevel &parent_verbosity_level)
Initializer.
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
Traveled distances from last map update / ICP-based localization.
double insertionLinDistance
Minimum robot linear (m) displacement for a new observation to be inserted in the map.
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
double localizationLinDistance
Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (...
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
std::shared_ptr< CPose3DPDF > Ptr
Algorithm configuration params.
GLsizei const GLchar ** string
mrpt::poses::CPose2D m_auxAccumOdometry
mrpt::maps::CMultiMetricMap metricMap
The metric map representation as a points map:
double insertionAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be i...
TConfigParams ICP_options
Options for the ICP-SLAM application.
This virtual class is the base for SLAM implementations.
mrpt::poses::CPose2D last_update
void setCurrentMapFile(const char *mapFile)
Sets the "current map file", thus that map will be loaded if it exists or a new one will be created i...
double localizationAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be u...
virtual ~CMetricMapBuilderICP()
Destructor:
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 | |