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



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020