MRPT  2.0.4
CObservation2DRangeScan.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #include <mrpt/maps/CMetricMap.h>
13 #include <mrpt/math/CPolygon.h>
14 #include <mrpt/obs/CObservation.h>
16 #include <mrpt/poses/CPose3D.h>
18 
19 // Add for declaration of mexplus::from template specialization
21 
22 namespace mrpt
23 {
24 namespace obs
25 {
26 /** A "CObservation"-derived class that represents a 2D range scan measurement
27  * (typically from a laser scanner).
28  * The data structures are generic enough to hold a wide variety of 2D
29  * scanners and "3D" planar rotating 2D lasers.
30  *
31  * These are the most important data fields:
32  * - These three fields are private data member (since MRPT 1.5.0) for
33  * safety and to ensure data consistency. Read them with the
34  * backwards-compatible proxies `scan`, `intensity`, `validRange` or (preferred)
35  * with the new `get_*`, `set_*` and `resize()` methods:
36  * - CObservation2DRangeScan::scan -> A vector of float values with all
37  * the range measurements (in meters).
38  * - CObservation2DRangeScan::validRange -> A vector (of <b>identical
39  * size</b> to <i>scan<i>), has non-zeros for those ranges than are valid (i.e.
40  * will be zero for non-reflected rays, etc.)
41  * - CObservation2DRangeScan::intensity -> A vector (of <b>identical
42  * size</b> to <i>scan<i>) a unitless int values representing the relative
43  * strength of each return. Higher values indicate a more intense return. This
44  * is useful for filtering out low intensity(noisy) returns or detecting intense
45  * landmarks.
46  * - CObservation2DRangeScan::aperture -> The field-of-view of the scanner,
47  * in radians (typically, M_PI = 180deg).
48  * - CObservation2DRangeScan::sensorPose -> The 6D location of the sensor on
49  * the robot reference frame (default=at the origin).
50  *
51  * \sa CObservation, CPointsMap, T2DScanProperties
52  * \ingroup mrpt_obs_grp
53  */
55 {
57  // This must be added for declaration of MEX-related functions
59  private:
60  /** The range values of the scan, in meters. Must have same length than \a
61  * validRange */
63  /** The intensity values of the scan. If available, must have same length
64  * than \a validRange */
66  /** It's false (=0) on no reflected rays, referenced to elements in \a scan
67  */
69  /** Whether the intensity values are present or not. If not, space is saved
70  * during serialization. */
71  bool m_has_intensity{false};
72 
73  public:
74  /** Used in filterByExclusionAreas */
75  using TListExclusionAreas = std::vector<mrpt::math::CPolygon>;
76  /** Used in filterByExclusionAreas */
78  std::vector<std::pair<mrpt::math::CPolygon, std::pair<double, double>>>;
79 
80  /** Default constructor */
81  CObservation2DRangeScan() = default;
82 
83  /** @name Scan data
84  @{ */
85  /** Resizes all data vectors to allocate a given number of scan rays */
86  void resizeScan(const size_t len);
87  /** Resizes all data vectors to allocate a given number of scan rays and
88  * assign default values. */
90  const size_t len, const float rangeVal, const bool rangeValidity,
91  const int32_t rangeIntensity = 0);
92  /** Get number of scan rays */
93  size_t getScanSize() const;
94 
95  /** The range values of the scan, in meters. Must have same length than \a
96  * validRange */
97  const float& getScanRange(const size_t i) const;
98  float& getScanRange(const size_t i);
99  void setScanRange(const size_t i, const float val);
100 
101  /** The intensity values of the scan. If available, must have same length
102  * than \a validRange */
103  const int32_t& getScanIntensity(const size_t i) const;
104  int32_t& getScanIntensity(const size_t i);
105  void setScanIntensity(const size_t i, const int val);
106 
107  /** It's false (=0) on no reflected rays, referenced to elements in \a scan
108  */
109  bool getScanRangeValidity(const size_t i) const;
110  void setScanRangeValidity(const size_t i, const bool val);
111 
112  /** The "aperture" or field-of-view of the range finder, in radians
113  * (typically M_PI = 180 degrees). */
114  float aperture{M_PIf};
115  /** The scanning direction: true=counterclockwise; false=clockwise */
116  bool rightToLeft{true};
117  /** The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
118  */
119  float maxRange{80.0f};
120  /** The 6D pose of the sensor on the robot at the moment of starting the
121  * scan. */
123  /** The "sigma" error of the device in meters, used while inserting the scan
124  * in an occupancy grid. */
125  float stdError{0.01f};
126  /** The aperture of each beam, in radians, used to insert "thick" rays in
127  * the occupancy grid. */
128  float beamAperture{0};
129  /** If the laser gathers data by sweeping in the pitch/elevation angle, this
130  * holds the increment in "pitch" (=-"elevation") between the beginning and
131  * the end of the scan (the sensorPose member stands for the pose at the
132  * beginning of the scan). */
133  double deltaPitch{0};
134 
135  /** Fill out a T2DScanProperties structure with the parameters of this scan
136  */
137  void getScanProperties(T2DScanProperties& p) const;
138  /** @} */
139 
140  void loadFromVectors(
141  size_t nRays, const float* scanRanges, const char* scanValidity);
142 
143  /** @name Cached points map
144  @{ */
145  protected:
146  /** A points map, build only under demand by the methods getAuxPointsMap()
147  * and buildAuxPointsMap().
148  * It's a generic smart pointer to avoid depending here in the library
149  * mrpt-obs on classes on other libraries.
150  */
152  /** Internal method, used from buildAuxPointsMap() */
153  void internal_buildAuxPointsMap(const void* options = nullptr) const;
154 
155  public:
156  /** Returns the cached points map representation of the scan, if already
157  * build with buildAuxPointsMap(), or nullptr otherwise.
158  * Usage:
159  * \code
160  * mrpt::maps::CPointsMap *map =
161  * obs->getAuxPointsMap<mrpt::maps::CPointsMap>();
162  * \endcode
163  * \sa buildAuxPointsMap
164  */
165  template <class POINTSMAP>
166  inline const POINTSMAP* getAuxPointsMap() const
167  {
168  return static_cast<const POINTSMAP*>(m_cachedMap.get());
169  }
170 
171  /** Returns a cached points map representing this laser scan, building it
172  * upon the first call.
173  * \param options Can be nullptr to use default point maps' insertion
174  * options, or a pointer to a "CPointsMap::TInsertionOptions" structure to
175  * override some params.
176  * Usage:
177  * \code
178  * mrpt::maps::CPointsMap *map =
179  * obs->buildAuxPointsMap<mrpt::maps::CPointsMap>(&options or nullptr);
180  * \endcode
181  * \sa getAuxPointsMap
182  */
183  template <class POINTSMAP>
184  inline const POINTSMAP* buildAuxPointsMap(
185  const void* options = nullptr) const
186  {
188  return static_cast<const POINTSMAP*>(m_cachedMap.get());
189  }
190 
191  /** @} */
192 
193  /** Return true if the laser scanner is "horizontal", so it has an absolute
194  * value of "pitch" and "roll" less or equal to the given tolerance (in
195  * rads, default=0) (with the normal vector either upwards or downwards).
196  */
197  bool isPlanarScan(const double tolerance = 0) const;
198 
199  /** Return true if scan has intensity */
200  bool hasIntensity() const;
201  /** Marks this scan as having or not intensity data. */
202  void setScanHasIntensity(bool setHasIntensityFlag);
203 
204  // See base class docs
205  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
206  {
207  out_sensorPose = sensorPose;
208  }
209  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
210  {
211  sensorPose = newSensorPose;
212  }
213  void getDescriptionAsText(std::ostream& o) const override;
214 
215  /** A general method to truncate the scan by defining a minimum valid
216  distance and a maximum valid angle as well as minimun and maximum heights
217  (NOTE: the laser z-coordinate must be provided).
218  */
220  float min_distance, float max_angle, float min_height = 0,
221  float max_height = 0, float h = 0);
222 
223  /** Mark as invalid sensed points that fall within any of a set of
224  * "exclusion areas", given in coordinates relative to the vehicle (taking
225  * into account "sensorPose").
226  * \sa C2DRangeFinderAbstract::loadExclusionAreas
227  */
228  void filterByExclusionAreas(const TListExclusionAreas& areas);
229 
230  /** Mark as invalid sensed points that fall within any of a set of
231  * "exclusion areas", given in coordinates relative to the vehicle (taking
232  * into account "sensorPose"), AND such as the Z coordinate of the point
233  * falls in the range [min,max] associated to each exclusion polygon.
234  * \sa C2DRangeFinderAbstract::loadExclusionAreas
235  */
237 
238  /** Mark as invalid the ranges in any of a given set of "forbiden angle
239  * ranges", given as pairs<min_angle,max_angle>.
240  * \sa C2DRangeFinderAbstract::loadExclusionAreas
241  */
243  const std::vector<std::pair<double, double>>& angles);
244 
245 }; // End of class def.
246 
247 } // namespace obs
248 namespace typemeta
249 {
250 // Specialization must occur in the same namespace
251 MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservation2DRangeScan, ::mrpt::obs)
252 } // namespace typemeta
253 
254 } // namespace mrpt
void filterByExclusionAngles(const std::vector< std::pair< double, double >> &angles)
Mark as invalid the ranges in any of a given set of "forbiden angle ranges", given as pairs<min_angle...
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
float beamAperture
The aperture of each beam, in radians, used to insert "thick" rays in the occupancy grid...
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
void getScanProperties(T2DScanProperties &p) const
Fill out a T2DScanProperties structure with the parameters of this scan.
void setScanRange(const size_t i, const float val)
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
size_t getScanSize() const
Get number of scan rays.
float maxRange
The maximum range allowed by the device, in meters (e.g.
std::vector< mrpt::math::CPolygon > TListExclusionAreas
Used in filterByExclusionAreas.
mrpt::maps::CMetricMap::Ptr m_cachedMap
A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap().
bool isPlanarScan(const double tolerance=0) const
Return true if the laser scanner is "horizontal", so it has an absolute value of "pitch" and "roll" l...
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
void internal_buildAuxPointsMap(const void *options=nullptr) const
Internal method, used from buildAuxPointsMap()
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class...
DECLARE_MEXPLUS_FROM(mrpt::img::TCamera) namespace std
Definition: TCamera.h:227
void setScanIntensity(const size_t i, const int val)
mrpt::aligned_std_vector< float > m_scan
The range values of the scan, in meters.
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
int val
Definition: mrpt_jpeglib.h:957
bool m_has_intensity
Whether the intensity values are present or not.
void resizeScanAndAssign(const size_t len, const float rangeVal, const bool rangeValidity, const int32_t rangeIntensity=0)
Resizes all data vectors to allocate a given number of scan rays and assign default values...
const int32_t & getScanIntensity(const size_t i) const
The intensity values of the scan.
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:142
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
void truncateByDistanceAndAngle(float min_distance, float max_angle, float min_height=0, float max_height=0, float h=0)
A general method to truncate the scan by defining a minimum valid distance and a maximum valid angle ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
std::vector< std::pair< mrpt::math::CPolygon, std::pair< double, double > >> TListExclusionAreasWithRanges
Used in filterByExclusionAreas.
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
mrpt::aligned_std_vector< int32_t > m_intensity
The intensity values of the scan.
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
const POINTSMAP * getAuxPointsMap() const
Returns the cached points map representation of the scan, if already build with buildAuxPointsMap(), or nullptr otherwise.
mrpt::aligned_std_vector< char > m_validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool hasIntensity() const
Return true if scan has intensity.
void filterByExclusionAreas(const TListExclusionAreas &areas)
Mark as invalid sensed points that fall within any of a set of "exclusion areas", given in coordinate...
CObservation2DRangeScan()=default
Default constructor.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void setScanRangeValidity(const size_t i, const bool val)
bool getScanRangeValidity(const size_t i) const
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020