Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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 namespace poses { class CPose3DInterpolator; }
21 namespace obs
22 {
24 
25  /** A `CObservation`-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LIDAR scanners.
26  * A scan comprises one or more "velodyne packets" (refer to Velodyne user manual).
27  *
28  * <h2>Main data fields:</h2><hr>
29  * - CObservationVelodyneScan::scan_packets with raw data packets.
30  * - CObservationVelodyneScan::point_cloud normally empty after grabbing for efficiency, can be generated calling \a CObservationVelodyneScan::generatePointCloud()
31  *
32  * Axes convention for point cloud (x,y,z) coordinates:
33  *
34  * <div align=center> <img src="velodyne_axes.jpg"> </div>
35  *
36  * If it can be assumed that the sensor moves SLOWLY through the environment (i.e. its pose can be approximated to be the same since the beginning to the end of one complete scan)
37  * then this observation can be converted / loaded into the following other classes:
38  * - Maps of points (these require first generating the pointcloud in this observation object with mrpt::obs::CObservationVelodyneScan::generatePointCloud() ):
39  * - mrpt::maps::CPointsMap::loadFromVelodyneScan() (available in all derived classes)
40  * - and the generic method:mrpt::maps::CPointsMap::insertObservation()
41  * - mrpt::opengl::CPointCloud and mrpt::opengl::CPointCloudColoured is supported by first converting
42  * this scan to a mrpt::maps::CPointsMap-derived class, then loading it into the opengl object.
43  *
44  * Otherwise, the following API exists for accurate reconstruction of the sensor path in SE(3) over time:
45  * - CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory()
46  *
47  * Note that this object has \b two timestamp fields:
48  * - The standard CObservation::timestamp field in the base class, which should contain the accurate satellite-based UTC timestamp, and
49  * - the field CObservationVelodyneScan::originalReceivedTimestamp, with the local computer-based timestamp based on the reception of the message in the computer.
50  * Both timestamps correspond to the firing of the <b>first</b> laser in the <b>first</b> CObservationVelodyneScan::scan_packets packet.
51  *
52  * \note New in MRPT 1.4.0
53  * \sa CObservation, CPointsMap, CVelodyneScanner
54  * \ingroup mrpt_obs_grp
55  */
57  {
58  // This must be added to any CSerializable derived class:
60 
61  public:
64 
65  /** @name Raw scan fixed parameters
66  @{ */
67  static const int SIZE_BLOCK = 100;
68  static const int RAW_SCAN_SIZE = 3;
69  static const int SCANS_PER_BLOCK = 32;
70  static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
71 
72  static const float ROTATION_RESOLUTION; // = 0.01f; /**< degrees */
73  static const uint16_t ROTATION_MAX_UNITS = 36000; /**< hundredths of degrees */
74 
75  static const float DISTANCE_MAX; /**< meters */
76  static const float DISTANCE_RESOLUTION; /**< meters */
77  static const float DISTANCE_MAX_UNITS;
78 
79  static const uint16_t UPPER_BANK = 0xeeff; //!< Blocks 0-31
80  static const uint16_t LOWER_BANK = 0xddff; //!< Blocks 32-63
81 
82  static const int PACKET_SIZE = 1206;
83  static const int POS_PACKET_SIZE = 512;
84  static const int BLOCKS_PER_PACKET = 12;
85  static const int PACKET_STATUS_SIZE = 4;
86  static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
87 
88  static const uint8_t RETMODE_STRONGEST = 0x37;
89  static const uint8_t RETMODE_LAST = 0x38;
90  static const uint8_t RETMODE_DUAL = 0x39;
91  /** @} */
92 
93  /** @name Scan data
94  @{ */
95 
96 #pragma pack(push,1)
98  uint16_t distance;
99  uint8_t intensity;
100  };
101  /** Raw Velodyne data block.
102  * Each block contains data from either the upper or lower laser
103  * bank. The device returns three times as many upper bank blocks. */
105  {
106  uint16_t header; ///< Block id: UPPER_BANK or LOWER_BANK
107  uint16_t rotation; ///< 0-35999, divide by 100 to get degrees
108  laser_return_t laser_returns[SCANS_PER_BLOCK];
109  } ;
110 
111  /** One unit of data from the scanner (the payload of one UDP DATA packet) */
113  {
114  raw_block_t blocks[BLOCKS_PER_PACKET];
115  uint32_t gps_timestamp; //!< us from top of hour
116  uint8_t laser_return_mode; //!< 0x37: strongest, 0x38: last, 0x39: dual return
117  uint8_t velodyne_model_ID; //!< 0x21: HDL-32E, 0x22: VLP-16
118  };
119 
120  /** Payload of one POSITION packet */
122  {
123  char unused1[198];
124  uint32_t gps_timestamp;
125  uint32_t unused2;
126  char NMEA_GPRMC[72+234]; //!< the full $GPRMC message, as received by Velodyne, terminated with "\r\n\0"
127  };
128 #pragma pack(pop)
129 
130  double minRange,maxRange; //!< The maximum range allowed by the device, in meters (e.g. 100m). Stored here by the driver while capturing based on the sensor model.
131  mrpt::poses::CPose3D sensorPose; //!< The 6D pose of the sensor on the robot/vehicle frame of reference
132  std::vector<TVelodyneRawPacket> scan_packets; //!< The main content of this object: raw data packet from the LIDAR. \sa point_cloud
133  mrpt::obs::VelodyneCalibration calibration; //!< The calibration data for the LIDAR device. See mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::VelodyneCalibration for details.
134  mrpt::system::TTimeStamp originalReceivedTimestamp; //!< The local computer-based timestamp based on the reception of the message in the computer. \sa has_satellite_timestamp, CObservation::timestamp in the base class, which should contain the accurate satellite-based UTC timestamp.
135  bool has_satellite_timestamp; //!< If true, CObservation::timestamp has been generated from accurate satellite clock. Otherwise, no GPS data is available and timestamps are based on the local computer clock.
136 
138 
139  /** See \a point_cloud and \a scan_packets */
141  {
142  std::vector<float> x,y,z;
143  std::vector<uint8_t> intensity; //!< Color [0,255]
144 
145  inline size_t size() const {
146  return x.size();
147  }
148  inline void clear() {
149  x.clear();
150  y.clear();
151  z.clear();
152  intensity.clear();
153  }
154  inline void clear_deep() {
155  { std::vector<float> dumm; x.swap(dumm); }
156  { std::vector<float> dumm; y.swap(dumm); }
157  { std::vector<float> dumm; z.swap(dumm); }
158  { std::vector<uint8_t> dumm; intensity.swap(dumm); }
159  }
160  };
161 
162  /** Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor, not the vehicle)
163  * with intensity (graylevel) information. See axes convention in mrpt::obs::CObservationVelodyneScan \sa generatePointCloud()
164  */
166  /** @} */
167 
168  /** @name Related to conversion to 3D point cloud
169  * @{ */
171  {
172  double minAzimuth_deg; //!< Minimum azimuth, in degrees (Default=0). Points will be generated only the the area of interest [minAzimuth, maxAzimuth]
173  double maxAzimuth_deg; //!< Minimum azimuth, in degrees (Default=360). Points will be generated only the the area of interest [minAzimuth, maxAzimuth]
174  float minDistance,maxDistance; //!< Minimum (default=1.0f) and maximum (default: Infinity) distances/ranges for a point to be considered. Points must pass this (application specific) condition and also the minRange/maxRange values in CObservationVelodyneScan (sensor-specific).
175  /** The limits of the 3D box (default=infinity) in sensor (not vehicle) local coordinates for the ROI filter \sa filterByROI */
176  float ROI_x_min, ROI_x_max, ROI_y_min, ROI_y_max, ROI_z_min, ROI_z_max;
177  /** The limits of the 3D box (default=0) in sensor (not vehicle) local coordinates for the nROI filter \sa filterBynROI */
178  float nROI_x_min, nROI_x_max, nROI_y_min, nROI_y_max, nROI_z_min, nROI_z_max;
179  float isolatedPointsFilterDistance; //!< (Default:2.0 meters) Minimum distance between a point and its two neighbors to be considered an invalid point.
180 
181  bool filterByROI; //!< Enable ROI filter (Default:false): add points inside a given 3D box
182  bool filterBynROI; //!< Enable nROI filter (Default:false): do NOT add points inside a given 3D box
183  bool filterOutIsolatedPoints; //!< (Default:false) Simple filter to remove spurious returns (e.g. Sun reflected on large water extensions)
184  bool dualKeepStrongest, dualKeepLast; //!< (Default:true) In VLP16 dual mode, keep both or just one of the returns.
185 
187  };
188 
190 
191  /** Generates the point cloud into the point cloud data fields in \a CObservationVelodyneScan::point_cloud
192  * where it is stored in local coordinates wrt the sensor (neither the vehicle nor the world).
193  * So, this method does not take into account the possible motion of the sensor through the world as it collects LIDAR scans.
194  * For high dynamics, see the more costly API generatePointCloudAlongSE3Trajectory()
195  * \note Points with ranges out of [minRange,maxRange] are discarded; as well, other filters are available in \a params.
196  * \sa generatePointCloudAlongSE3Trajectory(), TGeneratePointCloudParameters
197  */
198  void generatePointCloud(const TGeneratePointCloudParameters &params = defaultPointCloudParams );
199 
200  /** Results for generatePointCloudAlongSE3Trajectory() */
202  {
203  size_t num_points; //!< Number of points in the observation
204  size_t num_correctly_inserted_points; //!< Number of points for which a valid interpolated SE(3) pose could be determined
206  };
207 
208  /** An alternative to generatePointCloud() for cases where the motion of the sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored.
209  * \param[in] vehicle_path Timestamped sequence of known poses for the VEHICLE. Recall that the sensor has a relative pose wrt the vehicle according to CObservationVelodyneScan::getSensorPose() & CObservationVelodyneScan::setSensorPose()
210  * \param[out] out_points The generated points, in the same coordinates frame than \a vehicle_path. Points are APPENDED to the list, so prevous contents are kept.
211  * \param[out] results_stats Stats
212  * \param[in] params Filtering and other parameters
213  * \sa generatePointCloud(), TGeneratePointCloudParameters
214  */
216  const mrpt::poses::CPose3DInterpolator & vehicle_path,
217  std::vector<mrpt::math::TPointXYZIu8> & out_points,
218  TGeneratePointCloudSE3Results & results_stats,
219  const TGeneratePointCloudParameters &params = defaultPointCloudParams );
220 
221  /** @} */
222 
223  void getSensorPose( mrpt::poses::CPose3D &out_sensorPose ) const MRPT_OVERRIDE { out_sensorPose = sensorPose; } // See base class docs
224  void setSensorPose( const mrpt::poses::CPose3D &newSensorPose ) MRPT_OVERRIDE { sensorPose = newSensorPose; } // See base class docs
225  void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE; // See base class docs
226 
227  }; // End of class def.
229 
230 
231  } // End of namespace
232 
233  namespace utils { // Specialization must occur in the same namespace
234  MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservationVelodyneScan, ::mrpt::obs)
235  }
236 
237 } // End of namespace
238 
239 #endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:72
Declares a class that represents any robot's observation.
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
bool has_satellite_timestamp
If true, CObservation::timestamp has been generated from accurate satellite clock....
mrpt::system::TTimeStamp originalReceivedTimestamp
The local computer-based timestamp based on the reception of the message in the computer.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
void generatePointCloud(const TGeneratePointCloudParameters &params=defaultPointCloudParams)
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
std::vector< TVelodyneRawPacket > scan_packets
The main content of this object: raw data packet from the LIDAR.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
double maxRange
The maximum range allowed by the device, in meters (e.g. 100m). Stored here by the driver while captu...
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
void generatePointCloudAlongSE3Trajectory(const mrpt::poses::CPose3DInterpolator &vehicle_path, std::vector< mrpt::math::TPointXYZIu8 > &out_points, TGeneratePointCloudSE3Results &results_stats, const TGeneratePointCloudParameters &params=defaultPointCloudParams)
An alternative to generatePointCloud() for cases where the motion of the sensor as it grabs one scan ...
static const TGeneratePointCloudParameters defaultPointCloudParams
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor,...
mrpt::system::TTimeStamp getOriginalReceivedTimeStamp() const MRPT_OVERRIDE
By default, returns CObservation::timestamp but in sensors capable of satellite (or otherwise) accura...
mrpt::obs::VelodyneCalibration calibration
The calibration data for the LIDAR device. See mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::Velod...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
This namespace contains representation of robot actions and observations.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool filterByROI
Enable ROI filter (Default:false): add points inside a given 3D box.
double maxAzimuth_deg
Minimum azimuth, in degrees (Default=360). Points will be generated only the the area of interest [mi...
bool filterBynROI
Enable nROI filter (Default:false): do NOT add points inside a given 3D box.
float isolatedPointsFilterDistance
(Default:2.0 meters) Minimum distance between a point and its two neighbors to be considered an inval...
bool dualKeepLast
(Default:true) In VLP16 dual mode, keep both or just one of the returns.
float maxDistance
Minimum (default=1.0f) and maximum (default: Infinity) distances/ranges for a point to be considered....
double minAzimuth_deg
Minimum azimuth, in degrees (Default=0). Points will be generated only the the area of interest [minA...
bool filterOutIsolatedPoints
(Default:false) Simple filter to remove spurious returns (e.g. Sun reflected on large water extension...
size_t num_correctly_inserted_points
Number of points for which a valid interpolated SE(3) pose could be determined.
One unit of data from the scanner (the payload of one UDP DATA packet)
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
uint16_t header
Block id: UPPER_BANK or LOWER_BANK.
uint16_t rotation
0-35999, divide by 100 to get degrees
Velodyne calibration data, for usage in mrpt::obs::CObservationVelodyneScan.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Sat Jan 30 21:34:41 UTC 2021