MRPT  1.9.9
CObservationVelodyneScan.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 
11 #include <mrpt/obs/CObservation.h>
14 #include <mrpt/poses/CPose3D.h>
16 #include <vector>
17 
18 namespace mrpt
19 {
20 namespace poses
21 {
22 class CPose3DInterpolator;
23 }
24 namespace obs
25 {
26 /** A `CObservation`-derived class for RAW DATA (and optionally, point cloud) of
27  * scans from 3D Velodyne LIDAR scanners.
28  * A scan comprises one or more "velodyne packets" (refer to Velodyne user
29  * manual). Normally, a full 360 degrees sweep is included in one observation
30  * object. Note that if a pointcloud is generated inside this class, each point
31  * is annotated with per-point information about its exact azimuth and laser_id
32  * (ring number), an information that is loss when inserting the observation in
33  * a regular mrpt::maps::CPointsMap.
34  *
35  * <h2>Main data fields:</h2><hr>
36  * - CObservationVelodyneScan::scan_packets with raw data packets.
37  * - CObservationVelodyneScan::point_cloud normally empty after grabbing for
38  * efficiency, can be generated calling \a
39  * CObservationVelodyneScan::generatePointCloud()
40  *
41  * Dual return mode is supported (see mrpt::hwdrivers::CVelodyneScanner).
42  *
43  * Axes convention for point cloud (x,y,z) coordinates:
44  *
45  * <div align=center> <img src="velodyne_axes.jpg"> </div>
46  *
47  * If it can be assumed that the sensor moves SLOWLY through the environment
48  * (i.e. its pose can be approximated to be the same since the beginning to the
49  * end of one complete scan)
50  * then this observation can be converted / loaded into the following other
51  * classes:
52  * - Maps of points (these require first generating the pointcloud in this
53  * observation object with
54  * mrpt::obs::CObservationVelodyneScan::generatePointCloud() ):
55  * - mrpt::maps::CPointsMap::loadFromVelodyneScan() (available in all
56  * derived classes)
57  * - and the generic method:mrpt::maps::CPointsMap::insertObservation()
58  * - mrpt::opengl::CPointCloud and mrpt::opengl::CPointCloudColoured is
59  * supported by first converting
60  * this scan to a mrpt::maps::CPointsMap-derived class, then loading it into
61  * the opengl object.
62  *
63  * Otherwise, the following API exists for accurate reconstruction of the
64  * sensor path in SE(3) over time:
65  * - CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory()
66  *
67  * Note that this object has \b two timestamp fields:
68  * - The standard CObservation::timestamp field in the base class, which
69  * should contain the accurate satellite-based UTC timestamp, and
70  * - the field CObservationVelodyneScan::originalReceivedTimestamp, with the
71  * local computer-based timestamp based on the reception of the message in the
72  * computer.
73  * Both timestamps correspond to the firing of the <b>first</b> laser in the
74  * <b>first</b> CObservationVelodyneScan::scan_packets packet.
75  *
76  * \note New in MRPT 1.4.0
77  * \sa CObservation, mrpt::maps::CPointsMap, mrpt::hwdrivers::CVelodyneScanner
78  * \ingroup mrpt_obs_grp
79  */
81 {
83 
84  public:
85  /** @name Raw scan fixed parameters
86  @{ */
87  static const int SIZE_BLOCK = 100;
88  static const int RAW_SCAN_SIZE = 3;
89  static const int SCANS_PER_BLOCK = 32;
91 
92  static const float ROTATION_RESOLUTION; // = 0.01f; /**< degrees */
94  36000; /**< hundredths of degrees */
95 
96  static const float DISTANCE_MAX; /**< meters */
97  static const float DISTANCE_RESOLUTION; /**< meters */
98  static const float DISTANCE_MAX_UNITS;
99 
100  /** Blocks 0-31 */
101  static const uint16_t UPPER_BANK = 0xeeff;
102  /** Blocks 32-63 */
103  static const uint16_t LOWER_BANK = 0xddff;
104 
105  static const int SCANS_PER_FIRING = 16;
106 
107  static const int PACKET_SIZE = 1206;
108  static const int POS_PACKET_SIZE = 512;
109  static const int BLOCKS_PER_PACKET = 12;
110  static const int PACKET_STATUS_SIZE = 4;
112 
113  static const uint8_t RETMODE_STRONGEST = 0x37;
114  static const uint8_t RETMODE_LAST = 0x38;
115  static const uint8_t RETMODE_DUAL = 0x39;
116  /** @} */
117 
118  /** @name Scan data
119  @{ */
120 
121 #pragma pack(push, 1)
123  {
126  };
127  /** Raw Velodyne data block.
128  * Each block contains data from either the upper or lower laser
129  * bank. The device returns three times as many upper bank blocks. */
130  struct raw_block_t
131  {
132  uint16_t header; ///< Block id: UPPER_BANK or LOWER_BANK
133  uint16_t rotation; ///< 0-35999, divide by 100 to get degrees
135  };
136 
137  /** One unit of data from the scanner (the payload of one UDP DATA packet)
138  */
140  {
142  /** us from top of hour */
144  /** 0x37: strongest, 0x38: last, 0x39: dual return */
146  /** 0x21: HDL-32E, 0x22: VLP-16 */
148  };
149 
150  /** Payload of one POSITION packet */
152  {
153  char unused1[198];
156  /** the full $GPRMC message, as received by Velodyne, terminated with
157  * "\r\n\0" */
158  char NMEA_GPRMC[72 + 234];
159  };
160 #pragma pack(pop)
161 
162  /** The maximum range allowed by the device, in meters (e.g. 100m). Stored
163  * here by the driver while capturing based on the sensor model. */
164  double minRange{1.0}, maxRange{130.0};
165  /** The 6D pose of the sensor on the robot/vehicle frame of reference */
167  /** The main content of this object: raw data packet from the LIDAR. \sa
168  * point_cloud */
169  std::vector<TVelodyneRawPacket> scan_packets;
170  /** The calibration data for the LIDAR device. See
171  * mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::VelodyneCalibration for
172  * details. */
174  /** The local computer-based timestamp based on the reception of the message
175  * in the computer. \sa has_satellite_timestamp, CObservation::timestamp in
176  * the base class, which should contain the accurate satellite-based UTC
177  * timestamp. */
179  /** If true, CObservation::timestamp has been generated from accurate
180  * satellite clock. Otherwise, no GPS data is available and timestamps are
181  * based on the local computer clock. */
183 
185  const override; // See base class docs
186 
187  /** See \a point_cloud and \a scan_packets */
188  struct TPointCloud
189  {
190  std::vector<float> x, y, z;
191  /** Color [0,255] */
192  std::vector<uint8_t> intensity;
193  /** Timestamp for each point (if `generatePerPointTimestamp`=true in
194  * `TGeneratePointCloudParameters`), or empty vector if not populated
195  * (default). */
196  std::vector<mrpt::system::TTimeStamp> timestamp;
197  /** Original azimuth of each point (if `generatePerPointAzimuth`=true,
198  * empty otherwise ) */
199  std::vector<float> azimuth;
200  /** Laser ID ("ring number") for each point (0-15 for a VLP-16, etc.) */
201  std::vector<int16_t> laser_id;
202  /** The list of point indices (in x,y,z,...) generated for each laserID
203  * (ring number). Generated only if `generatePointsForLaserID`=true in
204  * `TGeneratePointCloudParameters`*/
205  std::vector<std::vector<uint64_t>> pointsForLaserID;
206 
207  inline std::size_t size() const { return x.size(); }
208  void reserve(std::size_t n);
209  /** Sets all vectors to zero length */
210  void clear();
211  /** Like clear(), but also enforcing freeing memory */
212  void clear_deep();
213  };
214 
215  /** Optionally, raw data can be converted into a 3D point cloud (local
216  * coordinates wrt the sensor, not the vehicle)
217  * with intensity (graylevel) information. See axes convention in
218  * mrpt::obs::CObservationVelodyneScan \sa generatePointCloud()
219  */
221  /** @} */
222 
223  /** @name Related to conversion to 3D point cloud
224  * @{ */
226  {
228  /** Minimum azimuth, in degrees (Default=0). Points will be generated
229  * only the the area of interest [minAzimuth, maxAzimuth] */
230  double minAzimuth_deg{.0};
231  /** Minimum azimuth, in degrees (Default=360). Points will be generated
232  * only the the area of interest [minAzimuth, maxAzimuth] */
233  double maxAzimuth_deg{360.};
234  /** Minimum (default=1.0f) and maximum (default: Infinity)
235  * distances/ranges for a point to be considered. Points must pass this
236  * (application specific) condition and also the minRange/maxRange
237  * values in CObservationVelodyneScan (sensor-specific). */
238  float minDistance{1.0f}, maxDistance{std::numeric_limits<float>::max()};
239  /** The limits of the 3D box (default=infinity) in sensor (not vehicle)
240  * local coordinates for the ROI filter \sa filterByROI */
241  float ROI_x_min{-std::numeric_limits<float>::max()},
242  ROI_x_max{std::numeric_limits<float>::max()},
243  ROI_y_min{-std::numeric_limits<float>::max()},
244  ROI_y_max{std::numeric_limits<float>::max()},
245  ROI_z_min{-std::numeric_limits<float>::max()},
246  ROI_z_max{std::numeric_limits<float>::max()};
247  /** The limits of the 3D box (default=0) in sensor (not vehicle) local
248  * coordinates for the nROI filter \sa filterBynROI */
251  /** (Default:2.0 meters) Minimum distance between a point and its two
252  * neighbors to be considered an invalid point. */
254 
255  /** Enable ROI filter (Default:false): add points inside a given 3D box
256  */
257  bool filterByROI{false};
258  /** Enable nROI filter (Default:false): do NOT add points inside a given
259  * 3D box */
260  bool filterBynROI{false};
261  /** (Default:false) Simple filter to remove spurious returns (e.g. Sun
262  * reflected on large water extensions) */
264  /** (Default:true) In VLP16 dual mode, keep both or just one of the
265  * returns. */
266  bool dualKeepStrongest{true}, dualKeepLast{true};
267  /** (Default:false) If `true`, populate the vector timestamp */
269  /** (Default:false) If `true`, populate the vector azimuth */
271  /** (Default:false) If `true`, populate pointsForLaserID */
273  };
274 
275  /** Derive from this class to generate pointclouds into custom containers.
276  * \sa generatePointCloud() */
278  {
279  virtual void reserve(std::size_t n) {}
280  virtual void resizeLaserCount([[maybe_unused]] std::size_t n) {}
281 
282  /** Process the insertion of a new (x,y,z) point to the cloud, in
283  * sensor-centric coordinates, with the exact timestamp of that LIDAR
284  * ray */
285  virtual void add_point(
286  float pt_x, float pt_y, float pt_z, uint8_t pt_intensity,
287  const mrpt::system::TTimeStamp& tim, const float azimuth,
288  uint16_t laser_id) = 0;
289  };
290 
291  /** Generates the point cloud into the point cloud data fields in \a
292  * CObservationVelodyneScan::point_cloud
293  * where it is stored in local coordinates wrt the sensor (neither the
294  * vehicle nor the world).
295  * So, this method does not take into account the possible motion of the
296  * sensor through the world as it collects LIDAR scans.
297  * For high dynamics, see the more costly API
298  * generatePointCloudAlongSE3Trajectory()
299  * \note Points with ranges out of [minRange,maxRange] are discarded; as
300  * well, other filters are available in \a params.
301  * \sa generatePointCloudAlongSE3Trajectory(),
302  * TGeneratePointCloudParameters
303  */
304  void generatePointCloud(
305  const TGeneratePointCloudParameters& params =
306  TGeneratePointCloudParameters());
307 
308  /** \overload For custom data storage as destination of the pointcloud. */
309  void generatePointCloud(
310  PointCloudStorageWrapper& dest,
311  const TGeneratePointCloudParameters& params =
312  TGeneratePointCloudParameters());
313 
314  /** Results for generatePointCloudAlongSE3Trajectory() */
316  {
317  /** Number of points in the observation */
318  size_t num_points{0};
319  /** Number of points for which a valid interpolated SE(3) pose could be
320  * determined */
322  };
323 
324  /** An alternative to generatePointCloud() for cases where the motion of the
325  * sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored.
326  * \param[in] vehicle_path Timestamped sequence of known poses for the
327  * VEHICLE. Recall that the sensor has a relative pose wrt the vehicle
328  * according to CObservationVelodyneScan::getSensorPose() &
329  * CObservationVelodyneScan::setSensorPose()
330  * \param[out] out_points The generated points, in the same coordinates
331  * frame than \a vehicle_path. Points are APPENDED to the list, so prevous
332  * contents are kept.
333  * \param[out] results_stats Stats
334  * \param[in] params Filtering and other parameters
335  * \sa generatePointCloud(), TGeneratePointCloudParameters
336  */
338  const mrpt::poses::CPose3DInterpolator& vehicle_path,
339  std::vector<mrpt::math::TPointXYZIu8>& out_points,
340  TGeneratePointCloudSE3Results& results_stats,
341  const TGeneratePointCloudParameters& params =
342  TGeneratePointCloudParameters());
343 
344  /** @} */
345 
346  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
347  {
348  out_sensorPose = sensorPose;
349  } // See base class docs
350  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
351  {
352  sensorPose = newSensorPose;
353  } // See base class docs
355  std::ostream& o) const override; // See base class docs
356 
357 }; // End of class def.
358 
359 } // namespace obs
360 
361 namespace typemeta
362 { // Specialization must occur in the same namespace
363 MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservationVelodyneScan, ::mrpt::obs)
364 }
365 
366 } // namespace mrpt
float ROI_x_min
The limits of the 3D box (default=infinity) in sensor (not vehicle) local coordinates for the ROI fil...
Velodyne calibration data, for usage in mrpt::obs::CObservationVelodyneScan.
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
unsigned __int16 uint16_t
Definition: rptypes.h:47
bool filterOutIsolatedPoints
(Default:false) Simple filter to remove spurious returns (e.g.
std::vector< mrpt::system::TTimeStamp > timestamp
Timestamp for each point (if generatePerPointTimestamp=true in TGeneratePointCloudParameters), or empty vector if not populated (default).
std::vector< float > azimuth
Original azimuth of each point (if generatePerPointAzimuth=true, empty otherwise ) ...
static const uint16_t LOWER_BANK
Blocks 32-63.
mrpt::system::TTimeStamp originalReceivedTimestamp
The local computer-based timestamp based on the reception of the message in the computer.
static const float DISTANCE_RESOLUTION
meters
GLenum GLsizei n
Definition: glext.h:5136
bool generatePerPointAzimuth
(Default:false) If true, populate the vector azimuth
mrpt::obs::VelodyneCalibration calibration
The calibration data for the LIDAR device.
bool has_satellite_timestamp
If true, CObservation::timestamp has been generated from accurate satellite clock.
bool filterByROI
Enable ROI filter (Default:false): add points inside a given 3D box.
void generatePointCloud(const TGeneratePointCloudParameters &params=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
char NMEA_GPRMC[72+234]
the full $GPRMC message, as received by Velodyne, terminated with "\r\n\0"
static const uint16_t ROTATION_MAX_UNITS
hundredths of degrees
unsigned char uint8_t
Definition: rptypes.h:44
double maxAzimuth_deg
Minimum azimuth, in degrees (Default=360).
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor...
void generatePointCloudAlongSE3Trajectory(const mrpt::poses::CPose3DInterpolator &vehicle_path, std::vector< mrpt::math::TPointXYZIu8 > &out_points, TGeneratePointCloudSE3Results &results_stats, const TGeneratePointCloudParameters &params=TGeneratePointCloudParameters())
An alternative to generatePointCloud() for cases where the motion of the sensor as it grabs one scan ...
double minRange
The maximum range allowed by the device, in meters (e.g.
static const float ROTATION_RESOLUTION
degrees
std::vector< int16_t > laser_id
Laser ID ("ring number") for each point (0-15 for a VLP-16, etc.)
mrpt::system::TTimeStamp getOriginalReceivedTimeStamp() const override
By default, returns CObservation::timestamp but in sensors capable of satellite (or otherwise) accura...
float isolatedPointsFilterDistance
(Default:2.0 meters) Minimum distance between a point and its two neighbors to be considered an inval...
virtual void add_point(float pt_x, float pt_y, float pt_z, uint8_t pt_intensity, const mrpt::system::TTimeStamp &tim, const float azimuth, uint16_t laser_id)=0
Process the insertion of a new (x,y,z) point to the cloud, in sensor-centric coordinates, with the exact timestamp of that LIDAR ray.
float minDistance
Minimum (default=1.0f) and maximum (default: Infinity) distances/ranges for a point to be considered...
This namespace contains representation of robot actions and observations.
size_t num_correctly_inserted_points
Number of points for which a valid interpolated SE(3) pose could be determined.
uint16_t header
Block id: UPPER_BANK or LOWER_BANK.
bool generatePerPointTimestamp
(Default:false) If true, populate the vector timestamp
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:142
void clear_deep()
Like clear(), but also enforcing freeing memory.
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...
bool generatePointsForLaserID
(Default:false) If true, populate pointsForLaserID
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
One unit of data from the scanner (the payload of one UDP DATA packet)
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
static const uint16_t UPPER_BANK
Blocks 0-31.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
virtual void resizeLaserCount([[maybe_unused]] std::size_t n)
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
std::vector< std::vector< uint64_t > > pointsForLaserID
The list of point indices (in x,y,z,...) generated for each laserID (ring number).
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
Derive from this class to generate pointclouds into custom containers.
uint16_t rotation
0-35999, divide by 100 to get degrees
std::vector< TVelodyneRawPacket > scan_packets
The main content of this object: raw data packet from the LIDAR.
GLenum GLint x
Definition: glext.h:3542
bool dualKeepStrongest
(Default:true) In VLP16 dual mode, keep both or just one of the returns.
float nROI_x_min
The limits of the 3D box (default=0) in sensor (not vehicle) local coordinates for the nROI filter...
unsigned __int32 uint32_t
Definition: rptypes.h:50
GLenum const GLfloat * params
Definition: glext.h:3538
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
bool filterBynROI
Enable nROI filter (Default:false): do NOT add points inside a given 3D box.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019