Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 {
23  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CObservationVelodyneScan, CObservation, OBS_IMPEXP)
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:
63  virtual ~CObservationVelodyneScan( );
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)
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];
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 
137  mrpt::system::TTimeStamp getOriginalReceivedTimeStamp() const MRPT_OVERRIDE; // See base class docs
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  std::vector<mrpt::system::TTimeStamp> timestamp; //!< Timestamp for each point (if `generatePerPointTimestamp`=true in `TGeneratePointCloudParameters`), or empty vector if not populated (default).
145  std::vector<float> azimuth; //!< Original azimuth of each point (if `generatePerPointAzimuth`=true, empty otherwise )
146 
147  inline size_t size() const {
148  return x.size();
149  }
150  void clear(); //!< Sets all vectors to zero length
151  void clear_deep(); //!< Like clear(), but also enforcing freeing memory
152  };
153 
154  /** Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor, not the vehicle)
155  * with intensity (graylevel) information. See axes convention in mrpt::obs::CObservationVelodyneScan \sa generatePointCloud()
156  */
158  /** @} */
159 
160  /** @name Related to conversion to 3D point cloud
161  * @{ */
163  {
164  double minAzimuth_deg; //!< Minimum azimuth, in degrees (Default=0). Points will be generated only the the area of interest [minAzimuth, maxAzimuth]
165  double maxAzimuth_deg; //!< Minimum azimuth, in degrees (Default=360). Points will be generated only the the area of interest [minAzimuth, maxAzimuth]
166  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).
167  /** The limits of the 3D box (default=infinity) in sensor (not vehicle) local coordinates for the ROI filter \sa filterByROI */
168  float ROI_x_min, ROI_x_max, ROI_y_min, ROI_y_max, ROI_z_min, ROI_z_max;
169  /** The limits of the 3D box (default=0) in sensor (not vehicle) local coordinates for the nROI filter \sa filterBynROI */
170  float nROI_x_min, nROI_x_max, nROI_y_min, nROI_y_max, nROI_z_min, nROI_z_max;
171  float isolatedPointsFilterDistance; //!< (Default:2.0 meters) Minimum distance between a point and its two neighbors to be considered an invalid point.
172 
173  bool filterByROI; //!< Enable ROI filter (Default:false): add points inside a given 3D box
174  bool filterBynROI; //!< Enable nROI filter (Default:false): do NOT add points inside a given 3D box
175  bool filterOutIsolatedPoints; //!< (Default:false) Simple filter to remove spurious returns (e.g. Sun reflected on large water extensions)
176  bool dualKeepStrongest, dualKeepLast; //!< (Default:true) In VLP16 dual mode, keep both or just one of the returns.
177  bool generatePerPointTimestamp; //!< (Default:false) If `true`, populate the vector timestamp
178  bool generatePerPointAzimuth; //!< (Default:false) If `true`, populate the vector azimuth
179 
181  };
182 
184 
185  /** Generates the point cloud into the point cloud data fields in \a CObservationVelodyneScan::point_cloud
186  * where it is stored in local coordinates wrt the sensor (neither the vehicle nor the world).
187  * So, this method does not take into account the possible motion of the sensor through the world as it collects LIDAR scans.
188  * For high dynamics, see the more costly API generatePointCloudAlongSE3Trajectory()
189  * \note Points with ranges out of [minRange,maxRange] are discarded; as well, other filters are available in \a params.
190  * \sa generatePointCloudAlongSE3Trajectory(), TGeneratePointCloudParameters
191  */
192  void generatePointCloud(const TGeneratePointCloudParameters &params = defaultPointCloudParams );
193 
194  /** Results for generatePointCloudAlongSE3Trajectory() */
196  {
197  size_t num_points; //!< Number of points in the observation
198  size_t num_correctly_inserted_points; //!< Number of points for which a valid interpolated SE(3) pose could be determined
200  };
201 
202  /** An alternative to generatePointCloud() for cases where the motion of the sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored.
203  * \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()
204  * \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.
205  * \param[out] results_stats Stats
206  * \param[in] params Filtering and other parameters
207  * \sa generatePointCloud(), TGeneratePointCloudParameters
208  */
209  void generatePointCloudAlongSE3Trajectory(
210  const mrpt::poses::CPose3DInterpolator & vehicle_path,
211  std::vector<mrpt::math::TPointXYZIu8> & out_points,
212  TGeneratePointCloudSE3Results & results_stats,
213  const TGeneratePointCloudParameters &params = defaultPointCloudParams );
214 
215  /** @} */
216 
217  void getSensorPose( mrpt::poses::CPose3D &out_sensorPose ) const MRPT_OVERRIDE { out_sensorPose = sensorPose; } // See base class docs
218  void setSensorPose( const mrpt::poses::CPose3D &newSensorPose ) MRPT_OVERRIDE { sensorPose = newSensorPose; } // See base class docs
219  void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE; // See base class docs
220 
221  }; // End of class def.
222  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CObservationVelodyneScan, CObservation, OBS_IMPEXP)
223 
224 
225  } // End of namespace
226 
227  namespace utils { // Specialization must occur in the same namespace
228  MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservationVelodyneScan, ::mrpt::obs)
229  }
230 
231 } // End of namespace
232 
233 #endif
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
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:46
bool filterOutIsolatedPoints
(Default:false) Simple filter to remove spurious returns (e.g. Sun reflected on large water extension...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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 ) ...
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. See mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::Velod...
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.
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.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
double minAzimuth_deg
Minimum azimuth, in degrees (Default=0). Points will be generated only the the area of interest [minA...
unsigned char uint8_t
Definition: rptypes.h:43
double maxAzimuth_deg
Minimum azimuth, in degrees (Default=360). Points will be generated only the the area of interest [mi...
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor...
#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...
float isolatedPointsFilterDistance
(Default:2.0 meters) Minimum distance between a point and its two neighbors to be considered an inval...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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:72
#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:72
One unit of data from the scanner (the payload of one UDP DATA packet)
Declares a class that represents any robot's observation.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
GLfloat * params
Definition: glew.h:1436
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
static const TGeneratePointCloudParameters defaultPointCloudParams
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.
unsigned __int32 uint32_t
Definition: rptypes.h:49
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
bool filterBynROI
Enable nROI filter (Default:false): do NOT add points inside a given 3D box.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018