MRPT  1.9.9
CObservationVelodyneScan.cpp
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 
10 #include "obs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/core/round.h>
18 #include <iostream>
19 
20 using namespace std;
21 using namespace mrpt::obs;
22 
23 // shortcut:
25 
26 // This must be added to any CSerializable class implementation file.
28 
30 
31 const float Velo::ROTATION_RESOLUTION = 0.01f; /**< degrees */
32 const float Velo::DISTANCE_MAX = 130.0f; /**< meters */
33 const float Velo::DISTANCE_RESOLUTION = 0.002f; /**< meters */
34 const float Velo::DISTANCE_MAX_UNITS =
35  (Velo::DISTANCE_MAX / Velo::DISTANCE_RESOLUTION + 1.0f);
36 
37 const int SCANS_PER_FIRING = 16;
38 
39 const float VLP16_BLOCK_TDURATION = 110.592f; // [us]
40 const float VLP16_DSR_TOFFSET = 2.304f; // [us]
41 const float VLP16_FIRING_TOFFSET = 55.296f; // [us]
42 
43 const double HDR32_DSR_TOFFSET = 1.152; // [us]
44 const double HDR32_FIRING_TOFFSET = 46.08; // [us]
45 
46 mrpt::system::TTimeStamp Velo::getOriginalReceivedTimeStamp() const
47 {
48  return originalReceivedTimestamp;
49 }
50 
51 uint8_t Velo::serializeGetVersion() const { return 2; }
53 {
54  out << timestamp << sensorLabel;
55  out << minRange << maxRange << sensorPose;
56  out.WriteAs<uint32_t>(scan_packets.size());
57  if (!scan_packets.empty())
58  out.WriteBuffer(
59  &scan_packets[0], sizeof(scan_packets[0]) * scan_packets.size());
60  out.WriteAs<uint32_t>(calibration.laser_corrections.size());
61  if (!calibration.laser_corrections.empty())
62  out.WriteBuffer(
63  &calibration.laser_corrections[0],
64  sizeof(calibration.laser_corrections[0]) *
65  calibration.laser_corrections.size());
66  out << point_cloud.x << point_cloud.y << point_cloud.z
67  << point_cloud.intensity;
68  out << has_satellite_timestamp; // v1
69  // v2:
70  out << point_cloud.timestamp << point_cloud.azimuth << point_cloud.laser_id
71  << point_cloud.pointsForLaserID;
72 }
73 
75 {
76  switch (version)
77  {
78  case 0:
79  case 1:
80  case 2:
81  {
82  in >> timestamp >> sensorLabel;
83 
84  in >> minRange >> maxRange >> sensorPose;
85  {
86  uint32_t N;
87  in >> N;
88  scan_packets.resize(N);
89  if (N)
90  in.ReadBuffer(
91  &scan_packets[0], sizeof(scan_packets[0]) * N);
92  }
93  {
94  uint32_t N;
95  in >> N;
96  calibration.laser_corrections.resize(N);
97  if (N)
98  in.ReadBuffer(
99  &calibration.laser_corrections[0],
100  sizeof(calibration.laser_corrections[0]) * N);
101  }
102  point_cloud.clear();
103  in >> point_cloud.x >> point_cloud.y >> point_cloud.z >>
104  point_cloud.intensity;
105  if (version >= 1)
106  in >> has_satellite_timestamp;
107  else
108  has_satellite_timestamp =
109  (this->timestamp != this->originalReceivedTimestamp);
110  if (version >= 2)
111  in >> point_cloud.timestamp >> point_cloud.azimuth >>
112  point_cloud.laser_id >> point_cloud.pointsForLaserID;
113  }
114  break;
115  default:
117  };
118 
119  // m_cachedMap.clear();
120 }
121 
122 void Velo::getDescriptionAsText(std::ostream& o) const
123 {
124  CObservation::getDescriptionAsText(o);
125  o << "Homogeneous matrix for the sensor 3D pose, relative to robot base:\n";
126  o << sensorPose.getHomogeneousMatrixVal<mrpt::math::CMatrixDouble44>()
127  << "\n"
128  << sensorPose << endl;
129  o << format("Sensor min/max range: %.02f / %.02f m\n", minRange, maxRange);
130  o << "Raw packet count: " << scan_packets.size() << "\n";
131 }
132 
133 /** [us] */
134 static double HDL32AdjustTimeStamp(int firingblock, int dsr)
135 {
136  return (firingblock * HDR32_FIRING_TOFFSET) + (dsr * HDR32_DSR_TOFFSET);
137 }
138 /** [us] */
139 static double VLP16AdjustTimeStamp(
140  int firingblock, int dsr, int firingwithinblock)
141 {
142  return (firingblock * VLP16_BLOCK_TDURATION) + (dsr * VLP16_DSR_TOFFSET) +
143  (firingwithinblock * VLP16_FIRING_TOFFSET);
144 }
145 
147  const Velo& scan, const Velo::TGeneratePointCloudParameters& params,
149 {
150  // Initially based on code from ROS velodyne & from
151  // vtkVelodyneHDLReader::vtkInternal::ProcessHDLPacket().
152  using mrpt::round;
153 
154  // Access to sin/cos table:
155  mrpt::obs::T2DScanProperties scan_props;
156  scan_props.aperture = 2 * M_PI;
157  scan_props.nRays = Velo::ROTATION_MAX_UNITS;
158  scan_props.rightToLeft = true;
159  // The LUT contains sin/cos values for angles in this order: [180deg ... 0
160  // deg ... -180 deg]
163 
164  const int minAzimuth_int = round(params.minAzimuth_deg * 100);
165  const int maxAzimuth_int = round(params.maxAzimuth_deg * 100);
166  const float realMinDist =
167  std::max(static_cast<float>(scan.minRange), params.minDistance);
168  const float realMaxDist =
169  std::min(params.maxDistance, static_cast<float>(scan.maxRange));
170  const auto isolatedPointsFilterDistance_units = mrpt::round(
171  params.isolatedPointsFilterDistance / Velo::DISTANCE_RESOLUTION);
172 
173  // This is: 16,32,64 depending on the LIDAR model
174  const size_t num_lasers = scan.calibration.laser_corrections.size();
175 
176  out_pc.resizeLaserCount(num_lasers);
177  out_pc.reserve(
178  Velo::SCANS_PER_BLOCK * scan.scan_packets.size() *
180  16);
181 
182  for (size_t iPkt = 0; iPkt < scan.scan_packets.size(); iPkt++)
183  {
184  const Velo::TVelodyneRawPacket* raw = &scan.scan_packets[iPkt];
185 
186  mrpt::system::TTimeStamp pkt_tim; // Find out timestamp of this pkt
187  {
188  const uint32_t us_pkt0 = scan.scan_packets[0].gps_timestamp;
189  const uint32_t us_pkt_this = raw->gps_timestamp;
190  // Handle the case of time counter reset by new hour 00:00:00
191  const uint32_t us_ellapsed =
192  (us_pkt_this >= us_pkt0)
193  ? (us_pkt_this - us_pkt0)
194  : (1000000UL * 3600UL + us_pkt_this - us_pkt0);
195  pkt_tim =
196  mrpt::system::timestampAdd(scan.timestamp, us_ellapsed * 1e-6);
197  }
198 
199  // Take the median rotational speed as a good value for interpolating
200  // the missing azimuths:
201  int median_azimuth_diff;
202  {
203  // In dual return, the azimuth rate is actually twice this
204  // estimation:
205  const unsigned int nBlocksPerAzimuth =
206  (raw->laser_return_mode == Velo::RETMODE_DUAL) ? 2 : 1;
207  const size_t nDiffs = Velo::BLOCKS_PER_PACKET - nBlocksPerAzimuth;
208  std::vector<int> diffs(nDiffs);
209  for (size_t i = 0; i < nDiffs; ++i)
210  {
211  int localDiff = (Velo::ROTATION_MAX_UNITS +
212  raw->blocks[i + nBlocksPerAzimuth].rotation -
213  raw->blocks[i].rotation) %
215  diffs[i] = localDiff;
216  }
217  std::nth_element(
218  diffs.begin(), diffs.begin() + Velo::BLOCKS_PER_PACKET / 2,
219  diffs.end()); // Calc median
220  median_azimuth_diff = diffs[Velo::BLOCKS_PER_PACKET / 2];
221  }
222 
223  // Firings per packet
224  for (int block = 0; block < Velo::BLOCKS_PER_PACKET; block++)
225  {
226  // ignore packets with mangled or otherwise different contents
227  if ((num_lasers != 64 &&
228  Velo::UPPER_BANK != raw->blocks[block].header) ||
229  (raw->blocks[block].header != Velo::UPPER_BANK &&
230  raw->blocks[block].header != Velo::LOWER_BANK))
231  {
232  cerr << "[Velo] skipping invalid packet: block " << block
233  << " header value is " << raw->blocks[block].header;
234  continue;
235  }
236 
237  const int dsr_offset =
238  (raw->blocks[block].header == Velo::LOWER_BANK) ? 32 : 0;
239  const auto azimuth_raw_f =
240  static_cast<float>(raw->blocks[block].rotation);
241  const bool block_is_dual_2nd_ranges =
243  ((block & 0x01) != 0));
244  const bool block_is_dual_last_ranges =
246  ((block & 0x01) == 0));
247 
248  for (int dsr = 0, k = 0; dsr < SCANS_PER_FIRING; dsr++, k++)
249  {
250  if (!raw->blocks[block]
251  .laser_returns[k]
252  .distance) // Invalid return?
253  continue;
254 
255  const auto rawLaserId = static_cast<uint8_t>(dsr + dsr_offset);
256  uint8_t laserId = rawLaserId;
257 
258  // Detect VLP-16 data and adjust laser id if necessary
259  bool firingWithinBlock = false;
260  if (num_lasers == 16)
261  {
262  if (laserId >= 16)
263  {
264  laserId -= 16;
265  firingWithinBlock = true;
266  }
267  }
268 
269  ASSERT_BELOW_(laserId, num_lasers);
271  scan.calibration.laser_corrections[laserId];
272 
273  // In dual return, if the distance is equal in both ranges,
274  // ignore one of them:
275  if (block_is_dual_2nd_ranges)
276  {
277  if (raw->blocks[block].laser_returns[k].distance ==
278  raw->blocks[block - 1].laser_returns[k].distance)
279  continue; // duplicated point
280  if (!params.dualKeepStrongest) continue;
281  }
282  if (block_is_dual_last_ranges && !params.dualKeepLast) continue;
283 
284  // Return distance:
285  const float distance =
286  raw->blocks[block].laser_returns[k].distance *
288  calib.distanceCorrection;
289  if (distance < realMinDist || distance > realMaxDist) continue;
290 
291  // Isolated points filtering:
292  if (params.filterOutIsolatedPoints)
293  {
294  bool pass_filter = true;
295  const int16_t dist_this =
296  raw->blocks[block].laser_returns[k].distance;
297  if (k > 0)
298  {
299  const int16_t dist_prev =
300  raw->blocks[block].laser_returns[k - 1].distance;
301  if (!dist_prev ||
302  std::abs(dist_this - dist_prev) >
303  isolatedPointsFilterDistance_units)
304  pass_filter = false;
305  }
306  if (k < (SCANS_PER_FIRING - 1))
307  {
308  const int16_t dist_next =
309  raw->blocks[block].laser_returns[k + 1].distance;
310  if (!dist_next ||
311  std::abs(dist_this - dist_next) >
312  isolatedPointsFilterDistance_units)
313  pass_filter = false;
314  }
315  if (!pass_filter) continue; // Filter out this point
316  }
317 
318  // Azimuth correction: correct for the laser rotation as a
319  // function of timing during the firings
320  double timestampadjustment =
321  0.0; // [us] since beginning of scan
322  double blockdsr0 = 0.0;
323  double nextblockdsr0 = 1.0;
324  switch (num_lasers)
325  {
326  // VLP-16
327  case 16:
328  {
330  {
331  timestampadjustment = VLP16AdjustTimeStamp(
332  block / 2, laserId, firingWithinBlock);
333  nextblockdsr0 =
334  VLP16AdjustTimeStamp(block / 2 + 1, 0, 0);
335  blockdsr0 = VLP16AdjustTimeStamp(block / 2, 0, 0);
336  }
337  else
338  {
339  timestampadjustment = VLP16AdjustTimeStamp(
340  block, laserId, firingWithinBlock);
341  nextblockdsr0 =
342  VLP16AdjustTimeStamp(block + 1, 0, 0);
343  blockdsr0 = VLP16AdjustTimeStamp(block, 0, 0);
344  }
345  }
346  break;
347  // HDL-32:
348  case 32:
349  timestampadjustment = HDL32AdjustTimeStamp(block, dsr);
350  nextblockdsr0 = HDL32AdjustTimeStamp(block + 1, 0);
351  blockdsr0 = HDL32AdjustTimeStamp(block, 0);
352  break;
353  case 64:
354  break;
355  default:
356  {
357  THROW_EXCEPTION("Error: unhandled LIDAR model!");
358  }
359  };
360 
361  const int azimuthadjustment = mrpt::round(
362  median_azimuth_diff * ((timestampadjustment - blockdsr0) /
363  (nextblockdsr0 - blockdsr0)));
364 
365  const float azimuth_corrected_f =
366  azimuth_raw_f + azimuthadjustment;
367  const int azimuth_corrected =
368  ((int)round(azimuth_corrected_f)) %
370 
371  // Filter by azimuth:
372  if (!((minAzimuth_int < maxAzimuth_int &&
373  azimuth_corrected >= minAzimuth_int &&
374  azimuth_corrected <= maxAzimuth_int) ||
375  (minAzimuth_int > maxAzimuth_int &&
376  (azimuth_corrected <= maxAzimuth_int ||
377  azimuth_corrected >= minAzimuth_int))))
378  continue;
379 
380  // Vertical axis mis-alignment calibration:
381  const float cos_vert_angle =
382  static_cast<float>(calib.cosVertCorrection);
383  const float sin_vert_angle =
384  static_cast<float>(calib.sinVertCorrection);
385  const float horz_offset =
386  static_cast<float>(calib.horizontalOffsetCorrection);
387  const float vert_offset =
388  static_cast<float>(calib.verticalOffsetCorrection);
389 
390  float xy_distance = distance * cos_vert_angle;
391  if (vert_offset != .0f)
392  xy_distance += vert_offset * sin_vert_angle;
393 
394  const int azimuth_corrected_for_lut =
395  (azimuth_corrected + (Velo::ROTATION_MAX_UNITS / 2)) %
397  const float cos_azimuth =
398  lut_sincos.ccos[azimuth_corrected_for_lut];
399  const float sin_azimuth =
400  lut_sincos.csin[azimuth_corrected_for_lut];
401 
402  // Compute raw position
403  const mrpt::math::TPoint3Df pt(
404  xy_distance * cos_azimuth +
405  horz_offset * sin_azimuth, // MRPT +X = Velodyne +Y
406  -(xy_distance * sin_azimuth -
407  horz_offset * cos_azimuth), // MRPT +Y = Velodyne -X
408  distance * sin_vert_angle + vert_offset);
409 
410  bool add_point = true;
411  if (params.filterByROI &&
412  (pt.x > params.ROI_x_max || pt.x < params.ROI_x_min ||
413  pt.y > params.ROI_y_max || pt.y < params.ROI_y_min ||
414  pt.z > params.ROI_z_max || pt.z < params.ROI_z_min))
415  add_point = false;
416 
417  if (params.filterBynROI &&
418  (pt.x <= params.nROI_x_max && pt.x >= params.nROI_x_min &&
419  pt.y <= params.nROI_y_max && pt.y >= params.nROI_y_min &&
420  pt.z <= params.nROI_z_max && pt.z >= params.nROI_z_min))
421  add_point = false;
422 
423  if (!add_point) continue;
424 
425  // Insert point:
426  out_pc.add_point(
427  pt.x, pt.y, pt.z,
428  raw->blocks[block].laser_returns[k].intensity, pkt_tim,
429  azimuth_corrected_f, laserId);
430 
431  } // end for k,dsr=[0,31]
432  } // end for each block [0,11]
433  } // end for each data packet
434 }
435 
438 {
439  velodyne_scan_to_pointcloud(*this, params, dest);
440 }
441 
443 {
444  struct PointCloudStorageWrapper_Inner : public PointCloudStorageWrapper
445  {
446  Velo& me_;
447  const TGeneratePointCloudParameters& params_;
448  PointCloudStorageWrapper_Inner(
450  : me_(me), params_(p)
451  {
452  // Reset point cloud:
453  me_.point_cloud.clear();
454  }
455 
456  void resizeLaserCount(std::size_t n) override
457  {
458  me_.point_cloud.pointsForLaserID.resize(n);
459  }
460 
461  void reserve(std::size_t n) override
462  {
463  me_.point_cloud.reserve(n);
464  if (!me_.point_cloud.pointsForLaserID.empty())
465  {
466  const std::size_t n_per_ring =
467  100 + n / (me_.point_cloud.pointsForLaserID.size());
468  for (auto& v : me_.point_cloud.pointsForLaserID)
469  v.reserve(n_per_ring);
470  }
471  }
472 
473  void add_point(
474  float pt_x, float pt_y, float pt_z, uint8_t pt_intensity,
475  const mrpt::system::TTimeStamp& tim, const float azimuth,
476  uint16_t laser_id) override
477  {
478  const auto idx = me_.point_cloud.x.size();
479  me_.point_cloud.x.push_back(pt_x);
480  me_.point_cloud.y.push_back(pt_y);
481  me_.point_cloud.z.push_back(pt_z);
482  me_.point_cloud.intensity.push_back(pt_intensity);
483  if (params_.generatePerPointTimestamp)
484  {
485  me_.point_cloud.timestamp.push_back(tim);
486  }
487  if (params_.generatePerPointAzimuth)
488  {
489  const int azimuth_corrected =
491  me_.point_cloud.azimuth.push_back(
492  azimuth_corrected * ROTATION_RESOLUTION);
493  }
494  me_.point_cloud.laser_id.push_back(laser_id);
495  if (params_.generatePointsForLaserID)
496  me_.point_cloud.pointsForLaserID[laser_id].push_back(idx);
497  }
498  };
499 
500  PointCloudStorageWrapper_Inner my_pc_wrap(*this, params);
501  point_cloud.clear();
502 
503  generatePointCloud(my_pc_wrap, params);
504 }
505 
507  const mrpt::poses::CPose3DInterpolator& vehicle_path,
508  std::vector<mrpt::math::TPointXYZIu8>& out_points,
509  TGeneratePointCloudSE3Results& results_stats,
511 {
512  struct PointCloudStorageWrapper_SE3_Interp : public PointCloudStorageWrapper
513  {
514  Velo& me_;
515  const mrpt::poses::CPose3DInterpolator& vehicle_path_;
516  std::vector<mrpt::math::TPointXYZIu8>& out_points_;
517  TGeneratePointCloudSE3Results& results_stats_;
518  mrpt::system::TTimeStamp last_query_tim_;
519  mrpt::poses::CPose3D last_query_;
520  bool last_query_valid_;
521 
522  void reserve(std::size_t n) override
523  {
524  out_points_.reserve(out_points_.size() + n);
525  }
526 
527  PointCloudStorageWrapper_SE3_Interp(
528  Velo& me, const mrpt::poses::CPose3DInterpolator& vehicle_path,
529  std::vector<mrpt::math::TPointXYZIu8>& out_points,
530  TGeneratePointCloudSE3Results& results_stats)
531  : me_(me),
532  vehicle_path_(vehicle_path),
533  out_points_(out_points),
534  results_stats_(results_stats),
535  last_query_tim_(INVALID_TIMESTAMP),
536  last_query_valid_(false)
537  {
538  }
539  void add_point(
540  float pt_x, float pt_y, float pt_z, uint8_t pt_intensity,
541  const mrpt::system::TTimeStamp& tim,
542  [[maybe_unused]] const float azimuth,
543  [[maybe_unused]] uint16_t laser_id) override
544  {
545  // Use a cache since it's expected that the same timestamp is
546  // queried several times in a row:
547  if (last_query_tim_ != tim)
548  {
549  last_query_tim_ = tim;
550  vehicle_path_.interpolate(tim, last_query_, last_query_valid_);
551  }
552 
553  if (last_query_valid_)
554  {
555  mrpt::poses::CPose3D global_sensor_pose(
557  global_sensor_pose.composeFrom(last_query_, me_.sensorPose);
558  double gx, gy, gz;
559  global_sensor_pose.composePoint(pt_x, pt_y, pt_z, gx, gy, gz);
560  out_points_.emplace_back(gx, gy, gz, pt_intensity);
561  ++results_stats_.num_correctly_inserted_points;
562  }
563  ++results_stats_.num_points;
564  }
565  };
566 
567  PointCloudStorageWrapper_SE3_Interp my_pc_wrap(
568  *this, vehicle_path, out_points, results_stats);
569  velodyne_scan_to_pointcloud(*this, params, my_pc_wrap);
570 }
571 
573 {
574  x.clear();
575  y.clear();
576  z.clear();
577  intensity.clear();
578  timestamp.clear();
579  azimuth.clear();
580  laser_id.clear();
581  pointsForLaserID.clear();
582 }
584 {
586 
587  deep_clear(x);
588  deep_clear(y);
589  deep_clear(z);
590  deep_clear(intensity);
591  deep_clear(timestamp);
593  deep_clear(laser_id);
594  deep_clear(pointsForLaserID);
595 }
596 
597 void Velo::TPointCloud::reserve(std::size_t n)
598 {
599  x.reserve(n);
600  y.reserve(n);
601  z.reserve(n);
602  intensity.reserve(n);
603  timestamp.reserve(n);
604  azimuth.reserve(n);
605  laser_id.reserve(n);
606  pointsForLaserID.reserve(64); // ring number
607 }
608 
609 // Default ctor. Do NOT move to the .h, that causes build errors.
static void velodyne_scan_to_pointcloud(const Velo &scan, const Velo::TGeneratePointCloudParameters &params, Velo::PointCloudStorageWrapper &out_pc)
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
const double HDR32_FIRING_TOFFSET
const TSinCosValues & getSinCosForScan(const CObservation2DRangeScan &scan) const
Return two vectors with the cos and the sin of the angles for each of the rays in a scan...
GLdouble GLdouble z
Definition: glext.h:3879
#define min(a, b)
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
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:368
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
static double VLP16AdjustTimeStamp(int firingblock, int dsr, int firingwithinblock)
[us]
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
std::vector< mrpt::system::TTimeStamp > timestamp
Timestamp for each point (if generatePerPointTimestamp=true in TGeneratePointCloudParameters), or empty vector if not populated (default).
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
std::vector< float > azimuth
Original azimuth of each point (if generatePerPointAzimuth=true, empty otherwise ) ...
static const uint16_t LOWER_BANK
Blocks 32-63.
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.
STL namespace.
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:157
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:49
void generatePointCloud(const TGeneratePointCloudParameters &params=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
const float VLP16_DSR_TOFFSET
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
static double HDL32AdjustTimeStamp(int firingblock, int dsr)
[us]
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
static const uint16_t ROTATION_MAX_UNITS
hundredths of degrees
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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.)
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
__int16 int16_t
Definition: rptypes.h:46
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:563
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
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.
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
std::vector< PerLaserCalib > laser_corrections
static CSinCosLookUpTableFor2DScans velodyne_sincos_tables
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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
void clear_deep()
Like clear(), but also enforcing freeing memory.
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool generatePointsForLaserID
(Default:false) If true, populate pointsForLaserID
pose_t & interpolate(const mrpt::Clock::time_point &t, pose_t &out_interp, bool &out_valid_interp) const
Returns the pose at a given time, or interpolates using splines if there is not an exact match...
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
A pair of vectors with the cos and sin values.
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 deep_clear(CONTAINER &c)
Deep clear for a std vector container.
const float VLP16_BLOCK_TDURATION
virtual void resizeLaserCount([[maybe_unused]] std::size_t n)
GLuint in
Definition: glext.h:7391
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
const int SCANS_PER_FIRING
std::vector< std::vector< uint64_t > > pointsForLaserID
The list of point indices (in x,y,z,...) generated for each laserID (ring number).
GLenum GLint GLint y
Definition: glext.h:3542
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
mrpt::system::TTimeStamp timestampAdd(const mrpt::system::TTimeStamp tim, const double num_seconds)
Shifts a timestamp the given amount of seconds (>0: forwards in time, <0: backwards) ...
Definition: datetime.h:146
GLenum GLsizei GLenum format
Definition: glext.h:3535
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
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
const double HDR32_DSR_TOFFSET
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.
unsigned __int32 uint32_t
Definition: rptypes.h:50
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1889
const float VLP16_FIRING_TOFFSET
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 582f6a819 Mon Jul 22 16:41:38 2019 -0700 at mar jul 23 01:50:13 CEST 2019