MRPT  1.9.9
CObservationVelodyneScan.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 #include <mrpt/core/round.h>
16 #include <iostream>
17 
18 using namespace std;
19 using namespace mrpt::obs;
20 
21 // This must be added to any CSerializable class implementation file.
23 
25 
26 const float CObservationVelodyneScan::ROTATION_RESOLUTION =
27  0.01f; /**< degrees */
28 const float CObservationVelodyneScan::DISTANCE_MAX = 130.0f; /**< meters */
29 const float CObservationVelodyneScan::DISTANCE_RESOLUTION =
30  0.002f; /**< meters */
31 const float CObservationVelodyneScan::DISTANCE_MAX_UNITS =
32  (CObservationVelodyneScan::DISTANCE_MAX /
33  CObservationVelodyneScan::DISTANCE_RESOLUTION +
34  1.0f);
35 
36 const int SCANS_PER_FIRING = 16;
37 
38 const float VLP16_BLOCK_TDURATION = 110.592f; // [us]
39 const float VLP16_DSR_TOFFSET = 2.304f; // [us]
40 const float VLP16_FIRING_TOFFSET = 55.296f; // [us]
41 
42 const float HDR32_DSR_TOFFSET = 1.152f; // [us]
43 const float HDR32_FIRING_TOFFSET = 46.08f; // [us]
44 
45 mrpt::system::TTimeStamp
46  CObservationVelodyneScan::getOriginalReceivedTimeStamp() const
47 {
48  return originalReceivedTimestamp;
49 }
50 
51 uint8_t CObservationVelodyneScan::serializeGetVersion() const { return 1; }
52 void CObservationVelodyneScan::serializeTo(
54 {
55  out << timestamp << sensorLabel;
56  out << minRange << maxRange << sensorPose;
57  out.WriteAs<uint32_t>(scan_packets.size());
58  if (!scan_packets.empty())
59  out.WriteBuffer(
60  &scan_packets[0], sizeof(scan_packets[0]) * scan_packets.size());
61  out.WriteAs<uint32_t>(calibration.laser_corrections.size());
62  if (!calibration.laser_corrections.empty())
63  out.WriteBuffer(
64  &calibration.laser_corrections[0],
65  sizeof(calibration.laser_corrections[0]) *
66  calibration.laser_corrections.size());
67  out << point_cloud.x << point_cloud.y << point_cloud.z
68  << point_cloud.intensity;
69  out << has_satellite_timestamp; // v1
70 }
71 
72 void CObservationVelodyneScan::serializeFrom(
74 {
75  switch (version)
76  {
77  case 0:
78  case 1:
79  {
80  in >> timestamp >> sensorLabel;
81 
82  in >> minRange >> maxRange >> sensorPose;
83  {
84  uint32_t N;
85  in >> N;
86  scan_packets.resize(N);
87  if (N)
88  in.ReadBuffer(
89  &scan_packets[0], sizeof(scan_packets[0]) * N);
90  }
91  {
92  uint32_t N;
93  in >> N;
94  calibration.laser_corrections.resize(N);
95  if (N)
96  in.ReadBuffer(
97  &calibration.laser_corrections[0],
98  sizeof(calibration.laser_corrections[0]) * N);
99  }
100  in >> point_cloud.x >> point_cloud.y >> point_cloud.z >>
101  point_cloud.intensity;
102  if (version >= 1)
103  in >> has_satellite_timestamp;
104  else
105  has_satellite_timestamp =
106  (this->timestamp != this->originalReceivedTimestamp);
107  }
108  break;
109  default:
111  };
112 
113  // m_cachedMap.clear();
114 }
115 
116 void CObservationVelodyneScan::getDescriptionAsText(std::ostream& o) const
117 {
118  CObservation::getDescriptionAsText(o);
119  o << "Homogeneous matrix for the sensor 3D pose, relative to robot base:\n";
120  o << sensorPose.getHomogeneousMatrixVal<mrpt::math::CMatrixDouble44>()
121  << "\n"
122  << sensorPose << endl;
123  o << format("Sensor min/max range: %.02f / %.02f m\n", minRange, maxRange);
124  o << "Raw packet count: " << scan_packets.size() << "\n";
125 }
126 
127 /** [us] */
128 double HDL32AdjustTimeStamp(int firingblock, int dsr)
129 {
130  return (firingblock * HDR32_FIRING_TOFFSET) + (dsr * HDR32_DSR_TOFFSET);
131 }
132 /** [us] */
133 double VLP16AdjustTimeStamp(int firingblock, int dsr, int firingwithinblock)
134 {
135  return (firingblock * VLP16_BLOCK_TDURATION) + (dsr * VLP16_DSR_TOFFSET) +
136  (firingwithinblock * VLP16_FIRING_TOFFSET);
137 }
138 
139 /** Auxiliary class used to refactor
140  * CObservationVelodyneScan::generatePointCloud() */
142 {
143  /** Process the insertion of a new (x,y,z) point to the cloud, in
144  * sensor-centric coordinates, with the exact timestamp of that LIDAR ray */
145  virtual void add_point(
146  double pt_x, double pt_y, double pt_z, uint8_t pt_intensity,
147  const mrpt::system::TTimeStamp& tim, const float azimuth) = 0;
148 };
149 
151  const CObservationVelodyneScan& scan,
153  PointCloudStorageWrapper& out_pc)
154 {
155  // Initially based on code from ROS velodyne & from
156  // vtkVelodyneHDLReader::vtkInternal::ProcessHDLPacket().
157  using mrpt::round;
158 
159  // Access to sin/cos table:
160  mrpt::obs::T2DScanProperties scan_props;
161  scan_props.aperture = 2 * M_PI;
162  scan_props.nRays = CObservationVelodyneScan::ROTATION_MAX_UNITS;
163  scan_props.rightToLeft = true;
164  // The LUT contains sin/cos values for angles in this order: [180deg ... 0
165  // deg ... -180 deg]
168 
169  const int minAzimuth_int = round(params.minAzimuth_deg * 100);
170  const int maxAzimuth_int = round(params.maxAzimuth_deg * 100);
171  const float realMinDist =
172  std::max(static_cast<float>(scan.minRange), params.minDistance);
173  const float realMaxDist =
174  std::min(params.maxDistance, static_cast<float>(scan.maxRange));
175  const int16_t isolatedPointsFilterDistance_units =
176  params.isolatedPointsFilterDistance /
177  CObservationVelodyneScan::DISTANCE_RESOLUTION;
178 
179  // This is: 16,32,64 depending on the LIDAR model
180  const size_t num_lasers = scan.calibration.laser_corrections.size();
181 
182  for (size_t iPkt = 0; iPkt < scan.scan_packets.size(); iPkt++)
183  {
185  &scan.scan_packets[iPkt];
186 
187  mrpt::system::TTimeStamp pkt_tim; // Find out timestamp of this pkt
188  {
189  const uint32_t us_pkt0 = scan.scan_packets[0].gps_timestamp;
190  const uint32_t us_pkt_this = raw->gps_timestamp;
191  // Handle the case of time counter reset by new hour 00:00:00
192  const uint32_t us_ellapsed =
193  (us_pkt_this >= us_pkt0)
194  ? (us_pkt_this - us_pkt0)
195  : (1000000UL * 3600UL + us_pkt_this - us_pkt0);
196  pkt_tim =
197  mrpt::system::timestampAdd(scan.timestamp, us_ellapsed * 1e-6);
198  }
199 
200  // Take the median rotational speed as a good value for interpolating
201  // the missing azimuths:
202  int median_azimuth_diff;
203  {
204  // In dual return, the azimuth rate is actually twice this
205  // estimation:
206  const int nBlocksPerAzimuth =
207  (raw->laser_return_mode ==
208  CObservationVelodyneScan::RETMODE_DUAL)
209  ? 2
210  : 1;
211  std::vector<int> diffs(
212  CObservationVelodyneScan::BLOCKS_PER_PACKET -
213  nBlocksPerAzimuth);
214  for (int i = 0; i < CObservationVelodyneScan::BLOCKS_PER_PACKET -
215  nBlocksPerAzimuth;
216  ++i)
217  {
218  int localDiff = (CObservationVelodyneScan::ROTATION_MAX_UNITS +
219  raw->blocks[i + nBlocksPerAzimuth].rotation -
220  raw->blocks[i].rotation) %
221  CObservationVelodyneScan::ROTATION_MAX_UNITS;
222  diffs[i] = localDiff;
223  }
224  std::nth_element(
225  diffs.begin(),
226  diffs.begin() + CObservationVelodyneScan::BLOCKS_PER_PACKET / 2,
227  diffs.end()); // Calc median
228  median_azimuth_diff =
229  diffs[CObservationVelodyneScan::BLOCKS_PER_PACKET / 2];
230  }
231 
232  for (int block = 0; block < CObservationVelodyneScan::BLOCKS_PER_PACKET;
233  block++) // Firings per packet
234  {
235  // ignore packets with mangled or otherwise different contents
236  if ((num_lasers != 64 &&
237  CObservationVelodyneScan::UPPER_BANK !=
238  raw->blocks[block].header) ||
239  (raw->blocks[block].header !=
240  CObservationVelodyneScan::UPPER_BANK &&
241  raw->blocks[block].header !=
242  CObservationVelodyneScan::LOWER_BANK))
243  {
244  cerr << "[CObservationVelodyneScan] skipping invalid packet: "
245  "block "
246  << block << " header value is "
247  << raw->blocks[block].header;
248  continue;
249  }
250 
251  const int dsr_offset = (raw->blocks[block].header ==
252  CObservationVelodyneScan::LOWER_BANK)
253  ? 32
254  : 0;
255  const float azimuth_raw_f = (float)(raw->blocks[block].rotation);
256  const bool block_is_dual_2nd_ranges =
257  (raw->laser_return_mode ==
258  CObservationVelodyneScan::RETMODE_DUAL &&
259  ((block & 0x01) != 0));
260  const bool block_is_dual_last_ranges =
261  (raw->laser_return_mode ==
262  CObservationVelodyneScan::RETMODE_DUAL &&
263  ((block & 0x01) == 0));
264 
265  for (int dsr = 0, k = 0; dsr < SCANS_PER_FIRING; dsr++, k++)
266  {
267  if (!raw->blocks[block]
268  .laser_returns[k]
269  .distance) // Invalid return?
270  continue;
271 
272  const uint8_t rawLaserId =
273  static_cast<uint8_t>(dsr + dsr_offset);
274  uint8_t laserId = rawLaserId;
275 
276  // Detect VLP-16 data and adjust laser id if necessary
277  bool firingWithinBlock = false;
278  if (num_lasers == 16)
279  {
280  if (laserId >= 16)
281  {
282  laserId -= 16;
283  firingWithinBlock = true;
284  }
285  }
286 
287  ASSERT_BELOW_(laserId, num_lasers);
289  scan.calibration.laser_corrections[laserId];
290 
291  // In dual return, if the distance is equal in both ranges,
292  // ignore one of them:
293  if (block_is_dual_2nd_ranges)
294  {
295  if (raw->blocks[block].laser_returns[k].distance ==
296  raw->blocks[block - 1].laser_returns[k].distance)
297  continue; // duplicated point
298  if (!params.dualKeepStrongest) continue;
299  }
300  if (block_is_dual_last_ranges && !params.dualKeepLast) continue;
301 
302  // Return distance:
303  const float distance =
304  raw->blocks[block].laser_returns[k].distance *
305  CObservationVelodyneScan::DISTANCE_RESOLUTION +
306  calib.distanceCorrection;
307  if (distance < realMinDist || distance > realMaxDist) continue;
308 
309  // Isolated points filtering:
310  if (params.filterOutIsolatedPoints)
311  {
312  bool pass_filter = true;
313  const int16_t dist_this =
314  raw->blocks[block].laser_returns[k].distance;
315  if (k > 0)
316  {
317  const int16_t dist_prev =
318  raw->blocks[block].laser_returns[k - 1].distance;
319  if (!dist_prev ||
320  std::abs(dist_this - dist_prev) >
321  isolatedPointsFilterDistance_units)
322  pass_filter = false;
323  }
324  if (k < (SCANS_PER_FIRING - 1))
325  {
326  const int16_t dist_next =
327  raw->blocks[block].laser_returns[k + 1].distance;
328  if (!dist_next ||
329  std::abs(dist_this - dist_next) >
330  isolatedPointsFilterDistance_units)
331  pass_filter = false;
332  }
333  if (!pass_filter) continue; // Filter out this point
334  }
335 
336  // Azimuth correction: correct for the laser rotation as a
337  // function of timing during the firings
338  double timestampadjustment =
339  0.0; // [us] since beginning of scan
340  double blockdsr0 = 0.0;
341  double nextblockdsr0 = 1.0;
342  switch (num_lasers)
343  {
344  // VLP-16
345  case 16:
346  {
347  if (raw->laser_return_mode ==
348  CObservationVelodyneScan::RETMODE_DUAL)
349  {
350  timestampadjustment = VLP16AdjustTimeStamp(
351  block / 2, laserId, firingWithinBlock);
352  nextblockdsr0 =
353  VLP16AdjustTimeStamp(block / 2 + 1, 0, 0);
354  blockdsr0 = VLP16AdjustTimeStamp(block / 2, 0, 0);
355  }
356  else
357  {
358  timestampadjustment = VLP16AdjustTimeStamp(
359  block, laserId, firingWithinBlock);
360  nextblockdsr0 =
361  VLP16AdjustTimeStamp(block + 1, 0, 0);
362  blockdsr0 = VLP16AdjustTimeStamp(block, 0, 0);
363  }
364  }
365  break;
366  // HDL-32:
367  case 32:
368  timestampadjustment = HDL32AdjustTimeStamp(block, dsr);
369  nextblockdsr0 = HDL32AdjustTimeStamp(block + 1, 0);
370  blockdsr0 = HDL32AdjustTimeStamp(block, 0);
371  break;
372  case 64:
373  break;
374  default:
375  {
376  THROW_EXCEPTION("Error: unhandled LIDAR model!");
377  }
378  };
379 
380  const int azimuthadjustment = mrpt::round(
381  median_azimuth_diff * ((timestampadjustment - blockdsr0) /
382  (nextblockdsr0 - blockdsr0)));
383 
384  const float azimuth_corrected_f =
385  azimuth_raw_f + azimuthadjustment;
386  const int azimuth_corrected =
387  ((int)round(azimuth_corrected_f)) %
388  CObservationVelodyneScan::ROTATION_MAX_UNITS;
389 
390  // Filter by azimuth:
391  if (!((minAzimuth_int < maxAzimuth_int &&
392  azimuth_corrected >= minAzimuth_int &&
393  azimuth_corrected <= maxAzimuth_int) ||
394  (minAzimuth_int > maxAzimuth_int &&
395  (azimuth_corrected <= maxAzimuth_int ||
396  azimuth_corrected >= minAzimuth_int))))
397  continue;
398 
399  // Vertical axis mis-alignment calibration:
400  const float cos_vert_angle = calib.cosVertCorrection;
401  const float sin_vert_angle = calib.sinVertCorrection;
402  const float horz_offset = calib.horizontalOffsetCorrection;
403  const float vert_offset = calib.verticalOffsetCorrection;
404 
405  float xy_distance = distance * cos_vert_angle;
406  if (vert_offset) xy_distance += vert_offset * sin_vert_angle;
407 
408  const int azimuth_corrected_for_lut =
409  (azimuth_corrected +
410  (CObservationVelodyneScan::ROTATION_MAX_UNITS / 2)) %
411  CObservationVelodyneScan::ROTATION_MAX_UNITS;
412  const float cos_azimuth =
413  lut_sincos.ccos[azimuth_corrected_for_lut];
414  const float sin_azimuth =
415  lut_sincos.csin[azimuth_corrected_for_lut];
416 
417  // Compute raw position
418  const mrpt::math::TPoint3Df pt(
419  xy_distance * cos_azimuth +
420  horz_offset * sin_azimuth, // MRPT +X = Velodyne +Y
421  -(xy_distance * sin_azimuth -
422  horz_offset * cos_azimuth), // MRPT +Y = Velodyne -X
423  distance * sin_vert_angle + vert_offset);
424 
425  bool add_point = true;
426  if (params.filterByROI &&
427  (pt.x > params.ROI_x_max || pt.x < params.ROI_x_min ||
428  pt.y > params.ROI_y_max || pt.y < params.ROI_y_min ||
429  pt.z > params.ROI_z_max || pt.z < params.ROI_z_min))
430  add_point = false;
431 
432  if (params.filterBynROI &&
433  (pt.x <= params.nROI_x_max && pt.x >= params.nROI_x_min &&
434  pt.y <= params.nROI_y_max && pt.y >= params.nROI_y_min &&
435  pt.z <= params.nROI_z_max && pt.z >= params.nROI_z_min))
436  add_point = false;
437 
438  if (!add_point) continue;
439 
440  // Insert point:
441  out_pc.add_point(
442  pt.x, pt.y, pt.z,
443  raw->blocks[block].laser_returns[k].intensity, pkt_tim,
444  azimuth_corrected_f);
445 
446  } // end for k,dsr=[0,31]
447  } // end for each block [0,11]
448  } // end for each data packet
449 }
450 
451 void CObservationVelodyneScan::generatePointCloud(
453 {
454  struct PointCloudStorageWrapper_Inner : public PointCloudStorageWrapper
455  {
457  const TGeneratePointCloudParameters& params_;
458  PointCloudStorageWrapper_Inner(
461  : me_(me), params_(p)
462  {
463  // Reset point cloud:
464  me_.point_cloud.clear();
465  }
466  void add_point(
467  double pt_x, double pt_y, double pt_z, uint8_t pt_intensity,
468  const mrpt::system::TTimeStamp& tim, const float azimuth) override
469  {
470  me_.point_cloud.x.push_back(pt_x);
471  me_.point_cloud.y.push_back(pt_y);
472  me_.point_cloud.z.push_back(pt_z);
473  me_.point_cloud.intensity.push_back(pt_intensity);
474  if (params_.generatePerPointTimestamp)
475  {
476  me_.point_cloud.timestamp.push_back(tim);
477  }
478  if (params_.generatePerPointAzimuth)
479  {
480  const int azimuth_corrected =
481  ((int)round(azimuth)) %
482  CObservationVelodyneScan::ROTATION_MAX_UNITS;
483  me_.point_cloud.azimuth.push_back(
484  azimuth_corrected * ROTATION_RESOLUTION);
485  }
486  }
487  };
488 
489  PointCloudStorageWrapper_Inner my_pc_wrap(*this, params);
490 
491  velodyne_scan_to_pointcloud(*this, params, my_pc_wrap);
492 }
493 
494 void CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory(
495  const mrpt::poses::CPose3DInterpolator& vehicle_path,
496  std::vector<mrpt::math::TPointXYZIu8>& out_points,
497  TGeneratePointCloudSE3Results& results_stats,
499 {
500  // Pre-alloc mem:
501  out_points.reserve(
502  out_points.size() +
503  scan_packets.size() * BLOCKS_PER_PACKET * SCANS_PER_BLOCK + 16);
504 
505  struct PointCloudStorageWrapper_SE3_Interp : public PointCloudStorageWrapper
506  {
508  const mrpt::poses::CPose3DInterpolator& vehicle_path_;
509  std::vector<mrpt::math::TPointXYZIu8>& out_points_;
510  TGeneratePointCloudSE3Results& results_stats_;
511  mrpt::system::TTimeStamp last_query_tim_;
512  mrpt::poses::CPose3D last_query_;
513  bool last_query_valid_;
514 
515  PointCloudStorageWrapper_SE3_Interp(
517  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  double pt_x, double pt_y, double pt_z, uint8_t pt_intensity,
530  const mrpt::system::TTimeStamp& tim, const float azimuth) override
531  {
532  // Use a cache since it's expected that the same timestamp is
533  // queried several times in a row:
534  if (last_query_tim_ != tim)
535  {
536  last_query_tim_ = tim;
537  vehicle_path_.interpolate(tim, last_query_, last_query_valid_);
538  }
539 
540  if (last_query_valid_)
541  {
542  mrpt::poses::CPose3D global_sensor_pose(
544  global_sensor_pose.composeFrom(last_query_, me_.sensorPose);
545  double gx, gy, gz;
546  global_sensor_pose.composePoint(pt_x, pt_y, pt_z, gx, gy, gz);
547  out_points_.push_back(
548  mrpt::math::TPointXYZIu8(gx, gy, gz, pt_intensity));
549  ++results_stats_.num_correctly_inserted_points;
550  }
551  ++results_stats_.num_points;
552  }
553  };
554 
555  PointCloudStorageWrapper_SE3_Interp my_pc_wrap(
556  *this, vehicle_path, out_points, results_stats);
557  velodyne_scan_to_pointcloud(*this, params, my_pc_wrap);
558 }
559 
561 {
562  x.clear();
563  y.clear();
564  z.clear();
565  intensity.clear();
566  timestamp.clear();
567  azimuth.clear();
568 }
569 void CObservationVelodyneScan::TPointCloud::clear_deep()
570 {
571  {
572  std::vector<float> d;
573  x.swap(d);
574  }
575  {
576  std::vector<float> d;
577  y.swap(d);
578  }
579  {
580  std::vector<float> d;
581  z.swap(d);
582  }
583  {
584  std::vector<uint8_t> d;
585  intensity.swap(d);
586  }
587  {
588  std::vector<mrpt::system::TTimeStamp> d;
589  timestamp.swap(d);
590  }
591  {
592  std::vector<float> d;
593  azimuth.swap(d);
594  }
595 }
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
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:3872
#define min(a, b)
const float HDR32_FIRING_TOFFSET
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
static void velodyne_scan_to_pointcloud(const CObservationVelodyneScan &scan, const CObservationVelodyneScan::TGeneratePointCloudParameters &params, PointCloudStorageWrapper &out_pc)
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
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)
This must be inserted in all CSerializable classes implementation files.
std::vector< float > azimuth
Original azimuth of each point (if generatePerPointAzimuth=true, empty otherwise ) ...
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:156
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:48
const float VLP16_DSR_TOFFSET
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor...
double minRange
The maximum range allowed by the device, in meters (e.g.
A numeric matrix of compile-time fixed size.
Lightweight 3D point (float version).
__int16 int16_t
Definition: rptypes.h:43
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:565
const float HDR32_DSR_TOFFSET
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
std::vector< PerLaserCalib > laser_corrections
CSinCosLookUpTableFor2DScans velodyne_sincos_tables
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:52
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:86
One unit of data from the scanner (the payload of one UDP DATA packet)
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
const float VLP16_BLOCK_TDURATION
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< 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:379
GLuint in
Definition: glext.h:7274
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:24
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
const int SCANS_PER_FIRING
GLenum GLint GLint y
Definition: glext.h:3538
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:145
GLenum GLsizei GLenum format
Definition: glext.h:3531
uint16_t rotation
0-35999, divide by 100 to get degrees
std::vector< TVelodyneRawPacket > scan_packets
The main content of this object: raw data packet from the LIDAR.
GLenum GLint x
Definition: glext.h:3538
double HDL32AdjustTimeStamp(int firingblock, int dsr)
[us]
virtual void add_point(double pt_x, double pt_y, double pt_z, uint8_t pt_intensity, const mrpt::system::TTimeStamp &tim, const float azimuth)=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.
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.
unsigned __int32 uint32_t
Definition: rptypes.h:47
Auxiliary class used to refactor CObservationVelodyneScan::generatePointCloud()
XYZ point (double) + Intensity(u8)
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:186
double VLP16AdjustTimeStamp(int firingblock, int dsr, int firingwithinblock)
[us]
#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:1891
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020