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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 928d0abbd Sun Oct 13 02:28:49 2019 +0200 at dom oct 13 02:30:11 CEST 2019