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-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/obs/CObservation.h>
15 #include <mrpt/poses/CPose3D.h>
17 #include <vector>
18 
19 namespace mrpt
20 {
21 namespace poses
22 {
23 class CPose3DInterpolator;
24 }
25 namespace obs
26 {
27 /** A `CObservation`-derived class for RAW DATA (and optionally, point cloud) of
28  * scans from 3D Velodyne LIDAR scanners.
29  * A scan comprises one or more "velodyne packets" (refer to Velodyne user
30  * manual). Normally, a full 360 degrees sweep is included in one observation
31  * object. Note that if a pointcloud is generated inside this class, each point
32  * is annotated with per-point information about its exact azimuth and laser_id
33  * (ring number), an information that is loss when inserting the observation in
34  * a regular mrpt::maps::CPointsMap.
35  *
36  * <h2>Main data fields:</h2><hr>
37  * - CObservationVelodyneScan::scan_packets with raw data packets.
38  * - CObservationVelodyneScan::point_cloud normally empty after grabbing for
39  * efficiency, can be generated calling \a
40  * CObservationVelodyneScan::generatePointCloud()
41  *
42  * Dual return mode is supported (see mrpt::hwdrivers::CVelodyneScanner).
43  *
44  * Axes convention for point cloud (x,y,z) coordinates:
45  *
46  * <div align=center> <img src="velodyne_axes.jpg"> </div>
47  *
48  * If it can be assumed that the sensor moves SLOWLY through the environment
49  * (i.e. its pose can be approximated to be the same since the beginning to the
50  * end of one complete scan)
51  * then this observation can be converted / loaded into the following other
52  * classes:
53  * - Maps of points (these require first generating the pointcloud in this
54  * observation object with
55  * mrpt::obs::CObservationVelodyneScan::generatePointCloud() ):
56  * - mrpt::maps::CPointsMap::loadFromVelodyneScan() (available in all
57  * derived classes)
58  * - and the generic method:mrpt::maps::CPointsMap::insertObservation()
59  * - mrpt::opengl::CPointCloud and mrpt::opengl::CPointCloudColoured is
60  * supported by first converting
61  * this scan to a mrpt::maps::CPointsMap-derived class, then loading it into
62  * the opengl object.
63  *
64  * Otherwise, the following API exists for accurate reconstruction of the
65  * sensor path in SE(3) over time:
66  * - CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory()
67  *
68  * Note that this object has \b two timestamp fields:
69  * - The standard CObservation::timestamp field in the base class, which
70  * should contain the accurate satellite-based UTC timestamp, and
71  * - the field CObservationVelodyneScan::originalReceivedTimestamp, with the
72  * local computer-based timestamp based on the reception of the message in the
73  * computer.
74  * Both timestamps correspond to the firing of the <b>first</b> laser in the
75  * <b>first</b> CObservationVelodyneScan::scan_packets packet.
76  *
77  * \note New in MRPT 1.4.0
78  * \sa CObservation, mrpt::maps::CPointsMap, mrpt::hwdrivers::CVelodyneScanner
79  * \ingroup mrpt_obs_grp
80  */
82 {
84 
85  public:
86  /** @name Raw scan fixed parameters
87  @{ */
88  static const int SIZE_BLOCK = 100;
89  static const int RAW_SCAN_SIZE = 3;
90  static const int SCANS_PER_BLOCK = 32;
92 
93  static const uint16_t ROTATION_MAX_UNITS =
94  36000; /**< hundredths of degrees */
95 
96  static constexpr float ROTATION_RESOLUTION = 0.01f; /**< degrees */
97  static constexpr float DISTANCE_MAX = 130.0f; /**< meters */
98  static constexpr float DISTANCE_RESOLUTION = 0.002f; /**< meters */
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  {
124  private:
125  uint16_t m_distance;
126  uint8_t m_intensity;
127 
128  public:
129  inline uint16_t distance() const
130  {
132  }
133  inline uint8_t intensity() const { return m_intensity; }
134  };
135  /** Raw Velodyne data block.
136  * Each block contains data from either the upper or lower laser
137  * bank. The device returns three times as many upper bank blocks. */
138  struct raw_block_t
139  {
140  private:
141  uint16_t m_header; ///< Block id: UPPER_BANK or LOWER_BANK
142  uint16_t m_rotation; ///< 0-35999, divide by 100 to get degrees
143  public:
145 
146  inline uint16_t header() const
147  {
149  }
150  inline uint16_t rotation() const
151  {
153  }
154  };
155 
156  /** One unit of data from the scanner (the payload of one UDP DATA packet)
157  */
159  {
160  public:
162 
163  private:
164  /** us from top of hour */
165  uint32_t m_gps_timestamp;
166 
167  public:
168  /** 0x37: strongest, 0x38: last, 0x39: dual return */
170  /** 0x21: HDL-32E, 0x22: VLP-16 */
172 
173  inline uint32_t gps_timestamp() const
174  {
176  }
177  };
178 
179  /** Payload of one POSITION packet */
181  {
182  public:
183  char unused1[198];
184 
185  private:
186  uint32_t m_gps_timestamp;
187  uint32_t m_unused2;
188 
189  public:
190  /** the full $GPRMC message, as received by Velodyne, terminated with
191  * "\r\n\0" */
192  char NMEA_GPRMC[72 + 234];
193 
194  inline uint32_t gps_timestamp() const
195  {
197  }
198  inline uint32_t unused2() const
199  {
201  }
202  };
203 #pragma pack(pop)
204 
205  /** The maximum range allowed by the device, in meters (e.g. 100m). Stored
206  * here by the driver while capturing based on the sensor model. */
207  double minRange{1.0}, maxRange{130.0};
208  /** The 6D pose of the sensor on the robot/vehicle frame of reference */
210  /** The main content of this object: raw data packet from the LIDAR. \sa
211  * point_cloud */
212  std::vector<TVelodyneRawPacket> scan_packets;
213  /** The calibration data for the LIDAR device. See
214  * mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::VelodyneCalibration for
215  * details. */
217  /** The local computer-based timestamp based on the reception of the message
218  * in the computer. \sa has_satellite_timestamp, CObservation::timestamp in
219  * the base class, which should contain the accurate satellite-based UTC
220  * timestamp. */
222  /** If true, CObservation::timestamp has been generated from accurate
223  * satellite clock. Otherwise, no GPS data is available and timestamps are
224  * based on the local computer clock. */
226 
228  const override; // See base class docs
229 
230  /** See \a point_cloud and \a scan_packets */
231  struct TPointCloud
232  {
233  std::vector<float> x, y, z;
234  /** Color [0,255] */
235  std::vector<uint8_t> intensity;
236  /** Timestamp for each point (if `generatePerPointTimestamp`=true in
237  * `TGeneratePointCloudParameters`), or empty vector if not populated
238  * (default). */
239  std::vector<mrpt::system::TTimeStamp> timestamp;
240  /** Original azimuth of each point (if `generatePerPointAzimuth`=true,
241  * empty otherwise ) */
242  std::vector<float> azimuth;
243  /** Laser ID ("ring number") for each point (0-15 for a VLP-16, etc.) */
244  std::vector<int16_t> laser_id;
245  /** The list of point indices (in x,y,z,...) generated for each laserID
246  * (ring number). Generated only if `generatePointsForLaserID`=true in
247  * `TGeneratePointCloudParameters`*/
248  std::vector<std::vector<uint64_t>> pointsForLaserID;
249 
250  inline std::size_t size() const { return x.size(); }
251  void reserve(std::size_t n);
252  /** Sets all vectors to zero length */
253  void clear();
254  /** Like clear(), but also enforcing freeing memory */
255  void clear_deep();
256  };
257 
258  /** Optionally, raw data can be converted into a 3D point cloud (local
259  * coordinates wrt the sensor, not the vehicle)
260  * with intensity (graylevel) information. See axes convention in
261  * mrpt::obs::CObservationVelodyneScan \sa generatePointCloud()
262  */
264  /** @} */
265 
266  /** @name Related to conversion to 3D point cloud
267  * @{ */
269  {
271  /** Minimum azimuth, in degrees (Default=0). Points will be generated
272  * only the the area of interest [minAzimuth, maxAzimuth] */
273  double minAzimuth_deg{.0};
274  /** Minimum azimuth, in degrees (Default=360). Points will be generated
275  * only the the area of interest [minAzimuth, maxAzimuth] */
276  double maxAzimuth_deg{360.};
277  /** Minimum (default=1.0f) and maximum (default: Infinity)
278  * distances/ranges for a point to be considered. Points must pass this
279  * (application specific) condition and also the minRange/maxRange
280  * values in CObservationVelodyneScan (sensor-specific). */
281  float minDistance{1.0f}, maxDistance{std::numeric_limits<float>::max()};
282  /** The limits of the 3D box (default=infinity) in sensor (not vehicle)
283  * local coordinates for the ROI filter \sa filterByROI */
284  float ROI_x_min{-std::numeric_limits<float>::max()},
285  ROI_x_max{std::numeric_limits<float>::max()},
286  ROI_y_min{-std::numeric_limits<float>::max()},
287  ROI_y_max{std::numeric_limits<float>::max()},
288  ROI_z_min{-std::numeric_limits<float>::max()},
289  ROI_z_max{std::numeric_limits<float>::max()};
290  /** The limits of the 3D box (default=0) in sensor (not vehicle) local
291  * coordinates for the nROI filter \sa filterBynROI */
294  /** (Default:2.0 meters) Minimum distance between a point and its two
295  * neighbors to be considered an invalid point. */
297 
298  /** Enable ROI filter (Default:false): add points inside a given 3D box
299  */
300  bool filterByROI{false};
301  /** Enable nROI filter (Default:false): do NOT add points inside a given
302  * 3D box */
303  bool filterBynROI{false};
304  /** (Default:false) Simple filter to remove spurious returns (e.g. Sun
305  * reflected on large water extensions) */
307  /** (Default:true) In VLP16 dual mode, keep both or just one of the
308  * returns. */
309  bool dualKeepStrongest{true}, dualKeepLast{true};
310  /** (Default:false) If `true`, populate the vector timestamp */
312  /** (Default:false) If `true`, populate the vector azimuth */
314  /** (Default:false) If `true`, populate pointsForLaserID */
316  };
317 
318  /** Derive from this class to generate pointclouds into custom containers.
319  * \sa generatePointCloud() */
321  {
322  virtual void reserve(std::size_t n) {}
323  virtual void resizeLaserCount([[maybe_unused]] std::size_t n) {}
324 
325  /** Process the insertion of a new (x,y,z) point to the cloud, in
326  * sensor-centric coordinates, with the exact timestamp of that LIDAR
327  * ray */
328  virtual void add_point(
329  float pt_x, float pt_y, float pt_z, uint8_t pt_intensity,
330  const mrpt::system::TTimeStamp& tim, const float azimuth,
331  uint16_t laser_id) = 0;
332  };
333 
334  /** Generates the point cloud into the point cloud data fields in \a
335  * CObservationVelodyneScan::point_cloud
336  * where it is stored in local coordinates wrt the sensor (neither the
337  * vehicle nor the world).
338  * So, this method does not take into account the possible motion of the
339  * sensor through the world as it collects LIDAR scans.
340  * For high dynamics, see the more costly API
341  * generatePointCloudAlongSE3Trajectory()
342  * \note Points with ranges out of [minRange,maxRange] are discarded; as
343  * well, other filters are available in \a params.
344  * \sa generatePointCloudAlongSE3Trajectory(),
345  * TGeneratePointCloudParameters
346  */
347  void generatePointCloud(
348  const TGeneratePointCloudParameters& params =
349  TGeneratePointCloudParameters());
350 
351  /** \overload For custom data storage as destination of the pointcloud. */
352  void generatePointCloud(
353  PointCloudStorageWrapper& dest,
354  const TGeneratePointCloudParameters& params =
355  TGeneratePointCloudParameters());
356 
357  /** Results for generatePointCloudAlongSE3Trajectory() */
359  {
360  /** Number of points in the observation */
361  size_t num_points{0};
362  /** Number of points for which a valid interpolated SE(3) pose could be
363  * determined */
365  };
366 
367  /** An alternative to generatePointCloud() for cases where the motion of the
368  * sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored.
369  * \param[in] vehicle_path Timestamped sequence of known poses for the
370  * VEHICLE. Recall that the sensor has a relative pose wrt the vehicle
371  * according to CObservationVelodyneScan::getSensorPose() &
372  * CObservationVelodyneScan::setSensorPose()
373  * \param[out] out_points The generated points, in the same coordinates
374  * frame than \a vehicle_path. Points are APPENDED to the list, so prevous
375  * contents are kept.
376  * \param[out] results_stats Stats
377  * \param[in] params Filtering and other parameters
378  * \sa generatePointCloud(), TGeneratePointCloudParameters
379  */
381  const mrpt::poses::CPose3DInterpolator& vehicle_path,
382  std::vector<mrpt::math::TPointXYZIu8>& out_points,
383  TGeneratePointCloudSE3Results& results_stats,
384  const TGeneratePointCloudParameters& params =
385  TGeneratePointCloudParameters());
386 
387  /** @} */
388 
389  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
390  {
391  out_sensorPose = sensorPose;
392  } // See base class docs
393  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
394  {
395  sensorPose = newSensorPose;
396  } // See base class docs
398  std::ostream& o) const override; // See base class docs
399 
400 }; // End of class def.
401 
402 } // namespace obs
403 
404 namespace typemeta
405 { // Specialization must occur in the same namespace
406 MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservationVelodyneScan, ::mrpt::obs)
407 }
408 
409 } // namespace mrpt
static constexpr float DISTANCE_MAX
meters
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...
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.
bool generatePerPointAzimuth
(Default:false) If true, populate the vector azimuth
mrpt::obs::VelodyneCalibration calibration
The calibration data for the LIDAR device.
mrpt::vision::TStereoCalibParams params
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
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.
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...
uint16_t m_header
Block id: UPPER_BANK or LOWER_BANK.
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.
static constexpr float ROTATION_RESOLUTION
degrees
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.
T toNativeEndianness(const T &v_in)
Definition: reverse_bytes.h:54
size_t num_correctly_inserted_points
Number of points for which a valid interpolated SE(3) pose could be determined.
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.
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:85
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).
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Derive from this class to generate pointclouds into custom containers.
std::vector< TVelodyneRawPacket > scan_packets
The main content of this object: raw data packet from the LIDAR.
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...
static constexpr float DISTANCE_RESOLUTION
meters
uint16_t m_rotation
0-35999, divide by 100 to get degrees
#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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020