MRPT  1.9.9
CObservationVelodyneScan.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CObservationVelodyneScan_H
10 #define CObservationVelodyneScan_H
11 
13 #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).
31  *
32  * <h2>Main data fields:</h2><hr>
33  * - CObservationVelodyneScan::scan_packets with raw data packets.
34  * - CObservationVelodyneScan::point_cloud normally empty after grabbing for
35  * efficiency, can be generated calling \a
36  * CObservationVelodyneScan::generatePointCloud()
37  *
38  * Axes convention for point cloud (x,y,z) coordinates:
39  *
40  * <div align=center> <img src="velodyne_axes.jpg"> </div>
41  *
42  * If it can be assumed that the sensor moves SLOWLY through the environment
43  * (i.e. its pose can be approximated to be the same since the beginning to the
44  * end of one complete scan)
45  * then this observation can be converted / loaded into the following other
46  * classes:
47  * - Maps of points (these require first generating the pointcloud in this
48  * observation object with
49  * mrpt::obs::CObservationVelodyneScan::generatePointCloud() ):
50  * - mrpt::maps::CPointsMap::loadFromVelodyneScan() (available in all
51  * derived classes)
52  * - and the generic method:mrpt::maps::CPointsMap::insertObservation()
53  * - mrpt::opengl::CPointCloud and mrpt::opengl::CPointCloudColoured is
54  * supported by first converting
55  * this scan to a mrpt::maps::CPointsMap-derived class, then loading it into
56  * the opengl object.
57  *
58  * Otherwise, the following API exists for accurate reconstruction of the
59  * sensor path in SE(3) over time:
60  * - CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory()
61  *
62  * Note that this object has \b two timestamp fields:
63  * - The standard CObservation::timestamp field in the base class, which
64  * should contain the accurate satellite-based UTC timestamp, and
65  * - the field CObservationVelodyneScan::originalReceivedTimestamp, with the
66  * local computer-based timestamp based on the reception of the message in the
67  * computer.
68  * Both timestamps correspond to the firing of the <b>first</b> laser in the
69  * <b>first</b> CObservationVelodyneScan::scan_packets packet.
70  *
71  * \note New in MRPT 1.4.0
72  * \sa CObservation, CPointsMap, CVelodyneScanner
73  * \ingroup mrpt_obs_grp
74  */
76 {
78 
79  public:
80  /** @name Raw scan fixed parameters
81  @{ */
82  static const int SIZE_BLOCK = 100;
83  static const int RAW_SCAN_SIZE = 3;
84  static const int SCANS_PER_BLOCK = 32;
86 
87  static const float ROTATION_RESOLUTION; // = 0.01f; /**< degrees */
89  36000; /**< hundredths of degrees */
90 
91  static const float DISTANCE_MAX; /**< meters */
92  static const float DISTANCE_RESOLUTION; /**< meters */
93  static const float DISTANCE_MAX_UNITS;
94 
95  /** Blocks 0-31 */
96  static const uint16_t UPPER_BANK = 0xeeff;
97  /** Blocks 32-63 */
98  static const uint16_t LOWER_BANK = 0xddff;
99 
100  static const int PACKET_SIZE = 1206;
101  static const int POS_PACKET_SIZE = 512;
102  static const int BLOCKS_PER_PACKET = 12;
103  static const int PACKET_STATUS_SIZE = 4;
105 
106  static const uint8_t RETMODE_STRONGEST = 0x37;
107  static const uint8_t RETMODE_LAST = 0x38;
108  static const uint8_t RETMODE_DUAL = 0x39;
109 /** @} */
110 
111 /** @name Scan data
112  @{ */
113 
114 #pragma pack(push, 1)
116  {
119  };
120  /** Raw Velodyne data block.
121  * Each block contains data from either the upper or lower laser
122  * bank. The device returns three times as many upper bank blocks. */
123  struct raw_block_t
124  {
125  uint16_t header; ///< Block id: UPPER_BANK or LOWER_BANK
126  uint16_t rotation; ///< 0-35999, divide by 100 to get degrees
128  };
129 
130  /** One unit of data from the scanner (the payload of one UDP DATA packet)
131  */
133  {
135  /** us from top of hour */
137  /** 0x37: strongest, 0x38: last, 0x39: dual return */
139  /** 0x21: HDL-32E, 0x22: VLP-16 */
141  };
142 
143  /** Payload of one POSITION packet */
145  {
146  char unused1[198];
149  /** the full $GPRMC message, as received by Velodyne, terminated with
150  * "\r\n\0" */
151  char NMEA_GPRMC[72 + 234];
152  };
153 #pragma pack(pop)
154 
155  /** The maximum range allowed by the device, in meters (e.g. 100m). Stored
156  * here by the driver while capturing based on the sensor model. */
157  double minRange{1.0}, maxRange{130.0};
158  /** The 6D pose of the sensor on the robot/vehicle frame of reference */
160  /** The main content of this object: raw data packet from the LIDAR. \sa
161  * point_cloud */
162  std::vector<TVelodyneRawPacket> scan_packets;
163  /** The calibration data for the LIDAR device. See
164  * mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::VelodyneCalibration for
165  * details. */
167  /** The local computer-based timestamp based on the reception of the message
168  * in the computer. \sa has_satellite_timestamp, CObservation::timestamp in
169  * the base class, which should contain the accurate satellite-based UTC
170  * timestamp. */
172  /** If true, CObservation::timestamp has been generated from accurate
173  * satellite clock. Otherwise, no GPS data is available and timestamps are
174  * based on the local computer clock. */
176 
178  const override; // See base class docs
179 
180  /** See \a point_cloud and \a scan_packets */
181  struct TPointCloud
182  {
183  std::vector<float> x, y, z;
184  /** Color [0,255] */
185  std::vector<uint8_t> intensity;
186  /** Timestamp for each point (if `generatePerPointTimestamp`=true in
187  * `TGeneratePointCloudParameters`), or empty vector if not populated
188  * (default). */
189  std::vector<mrpt::system::TTimeStamp> timestamp;
190  /** Original azimuth of each point (if `generatePerPointAzimuth`=true,
191  * empty otherwise ) */
192  std::vector<float> azimuth;
193 
194  inline size_t size() const { return x.size(); }
195  /** Sets all vectors to zero length */
196  void clear();
197  /** Like clear(), but also enforcing freeing memory */
198  void clear_deep();
199  };
200 
201  /** Optionally, raw data can be converted into a 3D point cloud (local
202  * coordinates wrt the sensor, not the vehicle)
203  * with intensity (graylevel) information. See axes convention in
204  * mrpt::obs::CObservationVelodyneScan \sa generatePointCloud()
205  */
207  /** @} */
208 
209  /** @name Related to conversion to 3D point cloud
210  * @{ */
212  {
214  /** Minimum azimuth, in degrees (Default=0). Points will be generated
215  * only the the area of interest [minAzimuth, maxAzimuth] */
216  double minAzimuth_deg{.0};
217  /** Minimum azimuth, in degrees (Default=360). Points will be generated
218  * only the the area of interest [minAzimuth, maxAzimuth] */
219  double maxAzimuth_deg{360.};
220  /** Minimum (default=1.0f) and maximum (default: Infinity)
221  * distances/ranges for a point to be considered. Points must pass this
222  * (application specific) condition and also the minRange/maxRange
223  * values in CObservationVelodyneScan (sensor-specific). */
224  float minDistance{1.0f}, maxDistance{std::numeric_limits<float>::max()};
225  /** The limits of the 3D box (default=infinity) in sensor (not vehicle)
226  * local coordinates for the ROI filter \sa filterByROI */
227  float ROI_x_min{-std::numeric_limits<float>::max()},
228  ROI_x_max{std::numeric_limits<float>::max()},
229  ROI_y_min{-std::numeric_limits<float>::max()},
230  ROI_y_max{std::numeric_limits<float>::max()},
231  ROI_z_min{-std::numeric_limits<float>::max()},
232  ROI_z_max{std::numeric_limits<float>::max()};
233  /** The limits of the 3D box (default=0) in sensor (not vehicle) local
234  * coordinates for the nROI filter \sa filterBynROI */
237  /** (Default:2.0 meters) Minimum distance between a point and its two
238  * neighbors to be considered an invalid point. */
240 
241  /** Enable ROI filter (Default:false): add points inside a given 3D box
242  */
243  bool filterByROI{false};
244  /** Enable nROI filter (Default:false): do NOT add points inside a given
245  * 3D box */
246  bool filterBynROI{false};
247  /** (Default:false) Simple filter to remove spurious returns (e.g. Sun
248  * reflected on large water extensions) */
250  /** (Default:true) In VLP16 dual mode, keep both or just one of the
251  * returns. */
252  bool dualKeepStrongest{true}, dualKeepLast{true};
253  /** (Default:false) If `true`, populate the vector timestamp */
255  /** (Default:false) If `true`, populate the vector azimuth */
257  };
258 
259  /** Generates the point cloud into the point cloud data fields in \a
260  * CObservationVelodyneScan::point_cloud
261  * where it is stored in local coordinates wrt the sensor (neither the
262  * vehicle nor the world).
263  * So, this method does not take into account the possible motion of the
264  * sensor through the world as it collects LIDAR scans.
265  * For high dynamics, see the more costly API
266  * generatePointCloudAlongSE3Trajectory()
267  * \note Points with ranges out of [minRange,maxRange] are discarded; as
268  * well, other filters are available in \a params.
269  * \sa generatePointCloudAlongSE3Trajectory(),
270  * TGeneratePointCloudParameters
271  */
272  void generatePointCloud(
273  const TGeneratePointCloudParameters& params =
274  TGeneratePointCloudParameters());
275 
276  /** Results for generatePointCloudAlongSE3Trajectory() */
278  {
279  /** Number of points in the observation */
280  size_t num_points{0};
281  /** Number of points for which a valid interpolated SE(3) pose could be
282  * determined */
284  };
285 
286  /** An alternative to generatePointCloud() for cases where the motion of the
287  * sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored.
288  * \param[in] vehicle_path Timestamped sequence of known poses for the
289  * VEHICLE. Recall that the sensor has a relative pose wrt the vehicle
290  * according to CObservationVelodyneScan::getSensorPose() &
291  * CObservationVelodyneScan::setSensorPose()
292  * \param[out] out_points The generated points, in the same coordinates
293  * frame than \a vehicle_path. Points are APPENDED to the list, so prevous
294  * contents are kept.
295  * \param[out] results_stats Stats
296  * \param[in] params Filtering and other parameters
297  * \sa generatePointCloud(), TGeneratePointCloudParameters
298  */
300  const mrpt::poses::CPose3DInterpolator& vehicle_path,
301  std::vector<mrpt::math::TPointXYZIu8>& out_points,
302  TGeneratePointCloudSE3Results& results_stats,
303  const TGeneratePointCloudParameters& params =
304  TGeneratePointCloudParameters());
305 
306  /** @} */
307 
308  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
309  {
310  out_sensorPose = sensorPose;
311  } // See base class docs
312  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
313  {
314  sensorPose = newSensorPose;
315  } // See base class docs
317  std::ostream& o) const override; // See base class docs
318 
319 }; // End of class def.
320 
321 } // namespace obs
322 
323 namespace typemeta
324 { // Specialization must occur in the same namespace
325 MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservationVelodyneScan, ::mrpt::obs)
326 }
327 
328 } // namespace mrpt
329 
330 #endif
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...
unsigned __int16 uint16_t
Definition: rptypes.h:44
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.
void generatePointCloud(const TGeneratePointCloudParameters &params=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
bool generatePerPointAzimuth
(Default:false) If true, populate the vector azimuth
mrpt::obs::VelodyneCalibration calibration
The calibration data for the LIDAR device.
void clear_deep()
Like clear(), but also enforcing freeing memory.
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.
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
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 char uint8_t
Definition: rptypes.h:41
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...
double minRange
The maximum range allowed by the device, in meters (e.g.
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...
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:138
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 class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
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.
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
static const float DISTANCE_RESOLUTION
meters
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).
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:3538
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:47
GLenum const GLfloat * params
Definition: glext.h:3534
static const float ROTATION_RESOLUTION
degrees
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 ...
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020