Main MRPT website > C++ reference for MRPT 1.4.0
maps/CBeaconMap.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-2016, 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 CBeaconMap_H
10#define CBeaconMap_H
11
13#include <mrpt/maps/CBeacon.h>
15#include <mrpt/math/CMatrix.h>
17#include <mrpt/obs/obs_frwds.h>
18
20
21namespace mrpt
22{
23namespace maps
24{
26
27 /** A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
28 * <br>
29 * The individual beacons are defined as mrpt::maps::CBeacon objects.
30 * <br>
31 * When invoking CBeaconMap::insertObservation(), landmarks will be extracted and fused into the map.
32 * The only currently supported observation type is mrpt::obs::CObservationBeaconRanges.
33 * See insertionOptions and likelihoodOptions for parameters used when creating and fusing beacon landmarks.
34 * <br>
35 * Use "TInsertionOptions::insertAsMonteCarlo" to select between 2 different behaviors:
36 * - Initial PDF of beacons: MonteCarlo, after convergence, pass to Gaussians; or
37 * - Initial PDF of beacons: SOG, after convergence, a single Gaussian.
38 *
39 * Refer to the papers: []
40 *
41 * \ingroup mrpt_maps_grp
42 * \sa CMetricMap
43 */
44 class MAPS_IMPEXP CBeaconMap : public mrpt::maps::CMetricMap
45 {
46 // This must be added to any CSerializable derived class:
48
49 public:
50 typedef std::deque<CBeacon> TSequenceBeacons;
51 typedef std::deque<CBeacon>::iterator iterator;
52 typedef std::deque<CBeacon>::const_iterator const_iterator;
53
54 protected:
55 TSequenceBeacons m_beacons; //!< The individual beacons
56
57 // See docs in base class
59 virtual bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
60 double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
61
62 public:
63 /** Constructor */
65
66 void resize(const size_t N); //!< Resize the number of SOG modes
67
68 /** Access to individual beacons */
69 const CBeacon& operator [](size_t i) const {
70 ASSERT_(i<m_beacons.size())
71 return m_beacons[i];
72 }
73 /** Access to individual beacons */
74 const CBeacon& get(size_t i) const{
75 ASSERT_(i<m_beacons.size())
76 return m_beacons[i];
77 }
78 /** Access to individual beacons */
79 CBeacon& operator [](size_t i) {
80 ASSERT_(i<m_beacons.size())
81 return m_beacons[i];
82 }
83 /** Access to individual beacons */
84 CBeacon& get(size_t i) {
85 ASSERT_(i<m_beacons.size())
86 return m_beacons[i];
87 }
88
89 iterator begin() { return m_beacons.begin(); }
90 const_iterator begin() const { return m_beacons.begin(); }
91 iterator end() { return m_beacons.end(); }
92 const_iterator end() const { return m_beacons.end(); }
93
94 /** Inserts a copy of the given mode into the SOG */
95 void push_back(const CBeacon& m) {
96 m_beacons.push_back( m );
97 }
98
99 // See docs in base class
100 float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
101
102 /** With this struct options are provided to the likelihood computations */
104 {
105 public:
106 /** Initilization of default parameters
107 */
109
110 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
111 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
112
113 /** The standard deviation used for Beacon ranges likelihood (default=0.08m).
114 */
115 float rangeStd;
116 } likelihoodOptions;
117
118 /** This struct contains data for choosing the method by which new beacons are inserted in the map.
119 */
121 {
122 public:
123 /** Initilization of default parameters */
125 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
126 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
127
128 /** Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussians (see mrpt::maps::CBeacon).
129 * \sa MC_performResampling
130 */
132
133 /** Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation: the default values (both 0), makes the beacons to be in the same horizontal plane that the sensors, that is, 2D SLAM - the min/max values are -90/90.
134 */
135 float maxElevation_deg,minElevation_deg;
136
137 /** Number of particles per meter of range, i.e. per meter of the "radius of the ring".
138 */
140
141 /** The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (default=0,4).
142 */
144
145 /** Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing a given sample (default=5).
146 */
148
149 /** If set to false (default), the samples will be generated the first time a beacon is observed, and their weights just updated subsequently - if set to "true", fewer samples will be required since the particles will be resamples when necessary, and a small "noise" will be added to avoid depletion.
150 */
152
153 /** The std.dev. of the Gaussian noise to be added to each sample after resampling, only if MC_performResampling=true.
154 */
156
157 /** Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mode (default=20).
158 */
160
161 /** A parameter for initializing 2D/3D SOGs
162 */
164
165 /** Constant used to compute the std. dev. int the tangent direction when creating the Gaussians.
166 */
168
169 } insertionOptions;
170
171 /** Save to a MATLAB script which displays 3D error ellipses for the map.
172 * \param file The name of the file to save the script to.
173 * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
174 * \param stdCount The ellipsoids will be drawn from the center to a given confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95 confidence intervals)
175 *
176 * \return Returns false if any error occured, true elsewere.
177 */
179 const std::string &file,
180 const char *style="b",
181 float confInterval = 0.95f ) const;
182
183
184 /** Returns the stored landmarks count.
185 */
186 size_t size() const;
187
188 // See docs in base class
190 const mrpt::maps::CMetricMap * otherMap,
191 const mrpt::poses::CPose2D & otherMapPose,
192 mrpt::utils::TMatchingPairList & correspondences,
193 const TMatchingParams & params,
194 TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
195
196 /** Perform a search for correspondences between "this" and another lansmarks map:
197 * Firsly, the landmarks' descriptor is used to find correspondences, then inconsistent ones removed by
198 * looking at their 3D poses.
199 * \param otherMap [IN] The other map.
200 * \param correspondences [OUT] The matched pairs between maps.
201 * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
202 * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence.
203 */
205 const mrpt::maps::CBeaconMap *otherMap,
206 mrpt::utils::TMatchingPairList &correspondences,
207 float &correspondencesRatio,
208 std::vector<bool> &otherCorrespondences) const;
209
210 /** Changes the reference system of the map to a given 3D pose.
211 */
213
214 /** Changes the reference system of the map "otherMap" and save the result in "this" map.
215 */
217
218
219 /** Returns true if the map is empty/no observation has been inserted.
220 */
221 bool isEmpty() const MRPT_OVERRIDE;
222
223 /** Simulates a reading toward each of the beacons in the landmarks map, if any.
224 * \param in_robotPose This robot pose is used to simulate the ranges to each beacon.
225 * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot
226 * \param out_Observations The results will be stored here. NOTICE that the fields "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance" and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before calling this function.
227 * An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range.
228 */
229 void simulateBeaconReadings(
230 const mrpt::poses::CPose3D &in_robotPose,
231 const mrpt::poses::CPoint3D &in_sensorLocationOnRobot,
232 mrpt::obs::CObservationBeaconRanges &out_Observations ) const;
233
234 /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
235 * In the case of this class, these files are generated:
236 * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses.
237 * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D.
238 * - "filNamePrefix"+"_covs.m": A textual representation (see saveToTextFile)
239 */
240 void saveMetricMapRepresentationToFile(const std::string &filNamePrefix ) const MRPT_OVERRIDE;
241
242 /** Save a text file with a row per beacon, containing this 11 elements:
243 * - X Y Z: Mean values
244 * - VX VY VZ: Variances of each dimension (C11, C22, C33)
245 * - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
246 * - C12, C13, C23: Cross covariances
247 */
248 void saveToTextFile(const std::string &fil) const;
249
250 void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE; //!< Returns a 3D object representing the map.
251
252 const CBeacon * getBeaconByID( CBeacon::TBeaconID id ) const; //!< Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
253 CBeacon * getBeaconByID( CBeacon::TBeaconID id ); //!< Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
254
255
257 mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts; //!< Observations insertion options
258 mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts; //!< Probabilistic observation likelihood options
260
261 }; // End of class def.
263
264
265 } // End of namespace
266} // End of namespace
267
268#endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ... MAP_DEFINITION_END() block inside the declaration of each metric map...
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions.
Definition: maps/CBeacon.h:41
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian,...
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map,...
virtual void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
size_t size() const
Returns the stored landmarks count.
const CBeacon & get(size_t i) const
Access to individual beacons.
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
const_iterator begin() const
const_iterator end() const
bool saveToMATLABScript3D(const std::string &file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
std::deque< CBeacon >::const_iterator const_iterator
CBeacon & get(size_t i)
Access to individual beacons.
void computeMatchingWith3DLandmarks(const mrpt::maps::CBeaconMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: Firsly,...
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg, const mrpt::maps::CBeaconMap *otherMap)
Changes the reference system of the map "otherMap" and save the result in "this" map.
std::deque< CBeacon > TSequenceBeacons
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const MRPT_OVERRIDE
Computes the matching between this and another 2D point map, which includes finding:
std::deque< CBeacon >::iterator iterator
void push_back(const CBeacon &m)
Inserts a copy of the given mode into the SOG.
TSequenceBeacons m_beacons
The individual beacons.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
A list of TMatchingPair.
Definition: TMatchingPair.h:67
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
This struct contains data for choosing the method by which new beacons are inserted in the map.
bool MC_performResampling
If set to false (default), the samples will be generated the first time a beacon is observed,...
float SOG_maxDistBetweenGaussians
A parameter for initializing 2D/3D SOGs.
float MC_thresholdNegligible
Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing ...
float maxElevation_deg
Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation:...
float SOG_separationConstant
Constant used to compute the std.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
float MC_maxStdToGauss
The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (def...
float SOG_thresholdNegligible
Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mod...
TInsertionOptions()
Initilization of default parameters.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
unsigned int MC_numSamplesPerMeter
Number of particles per meter of range, i.e.
bool insertAsMonteCarlo
Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussian...
With this struct options are provided to the likelihood computations.
TLikelihoodOptions()
Initilization of default parameters.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
float rangeStd
The standard deviation used for Beacon ranges likelihood (default=0.08m).
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
Additional results from the determination of matchings between point clouds, etc.,...
Parameters for the determination of matchings between point clouds, etc.
Parameters for CMetricMap::compute3DMatchingRatio()



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Dec 25 21:25:12 UTC 2022