MRPT  2.0.3
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
mrpt::obs::CObservation2DRangeScan::m_intensity
mrpt::aligned_std_vector< int32_t > m_intensity
The intensity values of the scan.
Definition: CObservation2DRangeScan.h:65
mrpt::obs::CObservation2DRangeScan::setScanRange
void setScanRange(const size_t i, const float val)
Definition: CObservation2DRangeScan.cpp:507
mrpt::obs::CObservation2DRangeScan::loadFromVectors
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
Definition: CObservation2DRangeScan.cpp:558
mrpt::obs::CObservation2DRangeScan::maxRange
float maxRange
The maximum range allowed by the device, in meters (e.g.
Definition: CObservation2DRangeScan.h:119
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:116
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::obs::CObservation2DRangeScan::getScanRange
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.cpp:496
mrpt::obs::CObservation2DRangeScan::getScanSize
size_t getScanSize() const
Get number of scan rays.
Definition: CObservation2DRangeScan.cpp:557
mrpt::obs::CObservation2DRangeScan::internal_buildAuxPointsMap
void internal_buildAuxPointsMap(const void *options=nullptr) const
Internal method, used from buildAuxPointsMap()
Definition: CObservation2DRangeScan.cpp:425
mrpt::obs::T2DScanProperties
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
Definition: T2DScanProperties.h:20
mrpt::obs::CObservation2DRangeScan::TListExclusionAreas
std::vector< mrpt::math::CPolygon > TListExclusionAreas
Used in filterByExclusionAreas.
Definition: CObservation2DRangeScan.h:75
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::obs::CObservation2DRangeScan::hasIntensity
bool hasIntensity() const
Return true if scan has intensity.
Definition: CObservation2DRangeScan.cpp:252
mrpt::obs::CObservation2DRangeScan::buildAuxPointsMap
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call.
Definition: CObservation2DRangeScan.h:184
mrpt::obs::CObservation2DRangeScan::m_validRange
mrpt::aligned_std_vector< char > m_validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
Definition: CObservation2DRangeScan.h:68
mrpt::obs::CObservation2DRangeScan::resizeScan
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
Definition: CObservation2DRangeScan.cpp:541
mrpt::obs::CObservation2DRangeScan::isPlanarScan
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...
Definition: CObservation2DRangeScan.cpp:247
mrpt::obs::CObservation2DRangeScan::setScanRangeValidity
void setScanRangeValidity(const size_t i, const bool val)
Definition: CObservation2DRangeScan.cpp:534
mrpt::obs::CObservation2DRangeScan::deltaPitch
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
Definition: CObservation2DRangeScan.h:133
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation2DRangeScan::getDescriptionAsText
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Definition: CObservation2DRangeScan.cpp:453
CMetricMap.h
mrpt::obs::CObservation2DRangeScan::truncateByDistanceAndAngle
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 ...
Definition: CObservation2DRangeScan.cpp:56
mrpt::obs::CObservation2DRangeScan::setScanIntensity
void setScanIntensity(const size_t i, const int val)
Definition: CObservation2DRangeScan.cpp:523
mrpt::aligned_std_vector
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
Definition: aligned_std_vector.h:15
mrpt::obs::CObservation2DRangeScan::m_cachedMap
mrpt::maps::CMetricMap::Ptr m_cachedMap
A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap().
Definition: CObservation2DRangeScan.h:151
mrpt::obs::CObservation2DRangeScan::filterByExclusionAngles
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...
Definition: CObservation2DRangeScan.cpp:344
val
int val
Definition: mrpt_jpeglib.h:957
mrpt::obs::CObservation2DRangeScan::resizeScanAndAssign
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.
Definition: CObservation2DRangeScan.cpp:548
T2DScanProperties.h
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
Definition: CObservation2DRangeScan.h:114
mrpt::obs::CObservation2DRangeScan::setScanHasIntensity
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
Definition: CObservation2DRangeScan.cpp:253
mrpt::obs::CObservation2DRangeScan::TListExclusionAreasWithRanges
std::vector< std::pair< mrpt::math::CPolygon, std::pair< double, double > >> TListExclusionAreasWithRanges
Used in filterByExclusionAreas.
Definition: CObservation2DRangeScan.h:78
mrpt::obs::CObservation2DRangeScan::getSensorPose
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
Definition: CObservation2DRangeScan.h:205
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:142
mrpt::obs::CObservation2DRangeScan::getAuxPointsMap
const POINTSMAP * getAuxPointsMap() const
Returns the cached points map representation of the scan, if already build with buildAuxPointsMap(),...
Definition: CObservation2DRangeScan.h:166
DECLARE_MEXPLUS_FROM
DECLARE_MEXPLUS_FROM(mrpt::img::TCamera) namespace std
Definition: TCamera.h:227
aligned_std_vector.h
mrpt::obs::CObservation2DRangeScan::beamAperture
float beamAperture
The aperture of each beam, in radians, used to insert "thick" rays in the occupancy grid.
Definition: CObservation2DRangeScan.h:128
mrpt::maps::CMetricMap::Ptr
std::shared_ptr< CMetricMap > Ptr
Definition: CMetricMap.h:55
mrpt::obs::CObservation2DRangeScan::m_scan
mrpt::aligned_std_vector< float > m_scan
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.h:62
mrpt::obs::CObservation2DRangeScan::stdError
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
Definition: CObservation2DRangeScan.h:125
mrpt::obs::CObservation2DRangeScan::CObservation2DRangeScan
CObservation2DRangeScan()=default
Default constructor.
CPolygon.h
CPose3D.h
mrpt::obs::CObservation2DRangeScan::getScanIntensity
const int32_t & getScanIntensity(const size_t i) const
The intensity values of the scan.
Definition: CObservation2DRangeScan.cpp:513
CObservation.h
mrpt::obs::CObservation2DRangeScan::getScanProperties
void getScanProperties(T2DScanProperties &p) const
Fill out a T2DScanProperties structure with the parameters of this scan.
Definition: CObservation2DRangeScan.cpp:437
mrpt::obs::CObservation2DRangeScan::m_has_intensity
bool m_has_intensity
Whether the intensity values are present or not.
Definition: CObservation2DRangeScan.h:71
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
M_PIf
#define M_PIf
Definition: common.h:61
mrpt::obs::CObservation2DRangeScan::setSensorPose
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
Definition: CObservation2DRangeScan.h:209
CSerializable.h
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:122
mrpt::obs::CObservation2DRangeScan::getScanRangeValidity
bool getScanRangeValidity(const size_t i) const
It's false (=0) on no reflected rays, referenced to elements in scan.
Definition: CObservation2DRangeScan.cpp:529
mrpt::obs::CObservation2DRangeScan::filterByExclusionAreas
void filterByExclusionAreas(const TListExclusionAreas &areas)
Mark as invalid sensed points that fall within any of a set of "exclusion areas", given in coordinate...
Definition: CObservation2DRangeScan.cpp:323
DECLARE_MEX_CONVERSION
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class.
Definition: CSerializable.h:185



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020