Main MRPT website > C++ reference for MRPT 1.5.6
CMetricMapBuilderICP.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, 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 #ifndef CMetricMapBuilderICP_H
10 #define CMetricMapBuilderICP_H
11 
13 #include <mrpt/slam/CICP.h>
15 
16 #include <mrpt/slam/link_pragmas.h>
17 
18 
19 namespace mrpt
20 {
21 namespace slam
22 {
23  /** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic pose tracking algorithm.
24  * Map are stored as in files as binary dumps of "mrpt::maps::CSimpleMap" objects. The methods are
25  * thread-safe.
26  * \ingroup metric_slam_grp
27  */
29  {
30  public:
31  /** Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize".
32  */
34 
35  /** Destructor:
36  */
37  virtual ~CMetricMapBuilderICP();
38 
39  /** Algorithm configuration params
40  */
42  {
43  /** Initializer */
44  TConfigParams (mrpt::utils::VerbosityLevel &parent_verbosity_level);
45  TConfigParams &operator=(const TConfigParams &other); //Copy assignment
46 
47  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
48  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
49  /** (default:false) Match against the occupancy grid or the points map? The former is quicker but less precise. */
51 
52  double insertionLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be inserted in the map.
53  double insertionAngDistance; //!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be inserted in the map.
54  double localizationLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry).
55  double localizationAngDistance;//!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry).
56 
57  double minICPgoodnessToAccept; //!< Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
58 
59  mrpt::utils::VerbosityLevel &verbosity_level;
60 
61  /** What maps to create (at least one points map and/or a grid map are needed).
62  * For the expected format in the .ini file when loaded with loadFromConfigFile(), see documentation of TSetOfMetricMapInitializers.
63  */
65 
66  };
67 
68  TConfigParams ICP_options; //!< Options for the ICP-SLAM application \sa ICP_params
69  CICP::TConfigParams ICP_params; //!< Options for the ICP algorithm itself \sa ICP_options
70 
71  /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.
72  * This method MUST be called if using the default constructor, after loading the configuration into ICP_options. In particular, TConfigParams::mapInitializers
73  */
74  void initialize(
75  const mrpt::maps::CSimpleMap &initialMap = mrpt::maps::CSimpleMap(),
76  mrpt::poses::CPosePDF *x0 = NULL
77  ) MRPT_OVERRIDE;
78 
79  /** Returns a copy of the current best pose estimation as a pose PDF.
80  */
81  mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const MRPT_OVERRIDE;
82 
83  /** Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object.
84  */
85  void setCurrentMapFile( const char *mapFile );
86 
87  /** Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.
88  * \param action The estimation of the incremental pose change in the robot pose.
89  * \param in_SF The set of observations that robot senses at the new pose.
90  * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
91  * \sa processObservation
92  */
93  void processActionObservation(
94  mrpt::obs::CActionCollection &action,
95  mrpt::obs::CSensoryFrame &in_SF ) MRPT_OVERRIDE;
96 
97  /** The main method of this class: Process one odometry or sensor observation.
98  The new entry point of the algorithm (the old one was processActionObservation, which now is a wrapper to
99  this method).
100  * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
101  */
102  void processObservation(const mrpt::obs::CObservationPtr &obs);
103 
104  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map */
105  void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const MRPT_OVERRIDE;
106 
107  /** Returns the 2D points of current local map */
108  void getCurrentMapPoints( std::vector<float> &x, std::vector<float> &y);
109 
110  const mrpt::maps::CMultiMetricMap* getCurrentlyBuiltMetricMap() const MRPT_OVERRIDE;
111 
112  /** Returns just how many sensory-frames are stored in the currently build map */
113  unsigned int getCurrentlyBuiltMapSize() MRPT_OVERRIDE;
114 
115  /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
116  * \param file The output file name
117  * \param formatEMF_BMP Output format = true:EMF, false:BMP
118  */
119  void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true) MRPT_OVERRIDE;
120 
121  private:
122  /** The set of observations that leads to current map: */
123  mrpt::maps::CSimpleMap SF_Poses_seq;
124 
125  /** The metric map representation as a points map: */
126  mrpt::maps::CMultiMetricMap metricMap;
127 
128  /** Current map file. */
129  std::string currentMapFile;
130 
131  /** The pose estimation by the alignment algorithm (ICP). */
132  mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst; //!< Last pose estimation (Mean)
133  mrpt::math::CMatrixDouble33 m_lastPoseEst_cov; //!< Last pose estimation (covariance)
134 
135  /** The estimated robot path:
136  */
137  std::deque<mrpt::math::TPose2D> m_estRobotPath;
138  mrpt::poses::CPose2D m_auxAccumOdometry;
139 
140  /** Traveled distances from last map update / ICP-based localization. */
142  {
143  TDist() : lin(0),ang(0) { }
144  double lin; // meters
145  double ang; // degrees
147 
148  void updateDistances(const mrpt::poses::CPose2D &p);
149  void updatePose(const mrpt::poses::CPose2D &p);
150  };
154 
155  void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D & new_pose);
156  void resetRobotDisplacementCounters(const mrpt::poses::CPose2D & new_pose);
157 
158  };
159 
160  } // End of namespace
161 } // End of namespace
162 
163 #endif
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
double localizationAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be u...
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
The ICP algorithm configuration data.
Definition: CICP.h:57
double minICPgoodnessToAccept
Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
mrpt::aligned_containers< std::string, TDist >::map_t m_distSinceLastInsertion
Indexed by sensor label.
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
TConfigParams ICP_options
Options for the ICP-SLAM application.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
double insertionAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be i...
Declares a class for storing a collection of robot actions.
bool matchAgainstTheGrid
(default:false) Match against the occupancy grid or the points map? The former is quicker but less pr...
This class allows loading and storing values and vectors of different types from a configuration text...
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
This virtual class is the base for SLAM implementations.
GLfloat GLfloat p
Definition: glew.h:10113
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
double localizationLinDistance
Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (...
A class for very simple 2D SLAM based on ICP.
This class stores any customizable set of metric maps.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
double insertionLinDistance
Minimum robot linear (m) displacement for a new observation to be inserted in the map...
Traveled distances from last map update / ICP-based localization.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018