Main MRPT website > C++ reference for MRPT 1.5.7
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-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 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 #include <mrpt/utils/round.h>
15 #include <mrpt/utils/CStream.h>
16 
17 using namespace std;
18 using namespace mrpt::obs;
19 
20 
21 // This must be added to any CSerializable class implementation file.
23 
25 
26 const float CObservationVelodyneScan::ROTATION_RESOLUTION = 0.01f; /**< degrees */
27 const float CObservationVelodyneScan::DISTANCE_MAX = 130.0f; /**< meters */
28 const float CObservationVelodyneScan::DISTANCE_RESOLUTION = 0.002f; /**< meters */
29 const float CObservationVelodyneScan::DISTANCE_MAX_UNITS = (CObservationVelodyneScan::DISTANCE_MAX / CObservationVelodyneScan::DISTANCE_RESOLUTION + 1.0f);
30 
31 const int SCANS_PER_FIRING = 16;
32 
33 const float VLP16_BLOCK_TDURATION = 110.592f; // [us]
34 const float VLP16_DSR_TOFFSET = 2.304f; // [us]
35 const float VLP16_FIRING_TOFFSET = 55.296f; // [us]
36 
37 const float HDR32_DSR_TOFFSET = 1.152f; // [us]
38 const float HDR32_FIRING_TOFFSET = 46.08f; // [us]
39 
40 
41 const CObservationVelodyneScan::TGeneratePointCloudParameters CObservationVelodyneScan::defaultPointCloudParams;
42 
43 
45  minAzimuth_deg(0.0),
46  maxAzimuth_deg(360.0),
47  minDistance(1.0f),
48  maxDistance(std::numeric_limits<float>::max()),
49  ROI_x_min(-std::numeric_limits<float>::max()),
50  ROI_x_max(+std::numeric_limits<float>::max()),
51  ROI_y_min(-std::numeric_limits<float>::max()),
52  ROI_y_max(+std::numeric_limits<float>::max()),
53  ROI_z_min(-std::numeric_limits<float>::max()),
54  ROI_z_max(+std::numeric_limits<float>::max()),
55  nROI_x_min(.0f),
56  nROI_x_max(.0f),
57  nROI_y_min(.0f),
58  nROI_y_max(.0f),
59  nROI_z_min(.0f),
60  nROI_z_max(.0f),
61  isolatedPointsFilterDistance(2.0f),
62  filterByROI(false),
63  filterBynROI(false),
64  filterOutIsolatedPoints(false),
65  dualKeepStrongest(true),
66  dualKeepLast(true),
67  generatePerPointTimestamp(false),
68  generatePerPointAzimuth(false)
69 {
70 }
71 
72 CObservationVelodyneScan::TGeneratePointCloudSE3Results::TGeneratePointCloudSE3Results() :
73  num_points (0),
74  num_correctly_inserted_points(0)
75 {
76 }
77 
79  minRange(1.0),
80  maxRange(130.0),
81  sensorPose(),
84 {
85 }
86 
88 {
89 }
90 
93 }
94 
95 /*---------------------------------------------------------------
96  Implements the writing to a CStream capability of CSerializable objects
97  ---------------------------------------------------------------*/
99 {
100  if (version)
101  *version = 1;
102  else
103  {
104  out << timestamp << sensorLabel;
105 
106  out << minRange << maxRange << sensorPose;
107  {
108  uint32_t N = scan_packets.size();
109  out << N;
110  if (N) out.WriteBuffer(&scan_packets[0],sizeof(scan_packets[0])*N);
111  }
112  {
114  out << N;
116  }
118  out << has_satellite_timestamp; // v1
119  }
120 }
121 
122 /*---------------------------------------------------------------
123  Implements the reading from a CStream capability of CSerializable objects
124  ---------------------------------------------------------------*/
126 {
127  switch(version)
128  {
129  case 0:
130  case 1:
131  {
132  in >> timestamp >> sensorLabel;
133 
134  in >> minRange >> maxRange >> sensorPose;
135  {
136  uint32_t N;
137  in >> N;
138  scan_packets.resize(N);
139  if (N) in.ReadBuffer(&scan_packets[0],sizeof(scan_packets[0])*N);
140  }
141  {
142  uint32_t N;
143  in >> N;
144  calibration.laser_corrections.resize(N);
146  }
148  if (version>=1)
150  else has_satellite_timestamp = (this->timestamp!=this->originalReceivedTimestamp);
151 
152  } break;
153  default:
155  };
156 
157  //m_cachedMap.clear();
158 }
159 
161 {
163  o << "Homogeneous matrix for the sensor 3D pose, relative to robot base:\n";
164  o << sensorPose.getHomogeneousMatrixVal() << "\n" << sensorPose << endl;
165 
166  o << format("Sensor min/max range: %.02f / %.02f m\n", minRange, maxRange );
167 
168  o << "Raw packet count: " << scan_packets.size() << "\n";
169 }
170 
171 double HDL32AdjustTimeStamp(int firingblock, int dsr) //!< [us]
172 {
173  return
174  (firingblock * HDR32_FIRING_TOFFSET) +
175  (dsr * HDR32_DSR_TOFFSET);
176 }
177 double VLP16AdjustTimeStamp(int firingblock,int dsr,int firingwithinblock) //!< [us]
178 {
179  return
180  (firingblock * VLP16_BLOCK_TDURATION) +
181  (dsr * VLP16_DSR_TOFFSET) +
182  (firingwithinblock * VLP16_FIRING_TOFFSET);
183 }
184 
185 /** Auxiliary class used to refactor CObservationVelodyneScan::generatePointCloud() */
187  /** 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 */
188  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;
189 };
190 
192  const CObservationVelodyneScan & scan,
194  PointCloudStorageWrapper & out_pc)
195 {
196  // Initially based on code from ROS velodyne & from vtkVelodyneHDLReader::vtkInternal::ProcessHDLPacket().
197  using mrpt::utils::round;
198 
199  // Access to sin/cos table:
200  mrpt::obs::T2DScanProperties scan_props;
201  scan_props.aperture = 2*M_PI;
203  scan_props.rightToLeft = true;
204  // The LUT contains sin/cos values for angles in this order: [180deg ... 0 deg ... -180 deg]
206 
207  const int minAzimuth_int = round( params.minAzimuth_deg * 100 );
208  const int maxAzimuth_int = round( params.maxAzimuth_deg * 100 );
209  const float realMinDist = std::max(static_cast<float>(scan.minRange),params.minDistance);
210  const float realMaxDist = std::min(params.maxDistance,static_cast<float>(scan.maxRange));
211  const int16_t isolatedPointsFilterDistance_units = params.isolatedPointsFilterDistance/CObservationVelodyneScan::DISTANCE_RESOLUTION;
212 
213  // This is: 16,32,64 depending on the LIDAR model
214  const size_t num_lasers = scan.calibration.laser_corrections.size();
215 
216  for (size_t iPkt = 0; iPkt<scan.scan_packets.size();iPkt++)
217  {
219 
220  mrpt::system::TTimeStamp pkt_tim; // Find out timestamp of this pkt
221  {
222  const uint32_t us_pkt0 = scan.scan_packets[0].gps_timestamp;
223  const uint32_t us_pkt_this = raw->gps_timestamp;
224  // Handle the case of time counter reset by new hour 00:00:00
225  const uint32_t us_ellapsed = (us_pkt_this>=us_pkt0) ? (us_pkt_this-us_pkt0) : (1000000UL*3600UL + us_pkt_this-us_pkt0);
226  pkt_tim = mrpt::system::timestampAdd(scan.timestamp,us_ellapsed*1e-6);
227  }
228 
229  // Take the median rotational speed as a good value for interpolating the missing azimuths:
230  int median_azimuth_diff;
231  {
232  // In dual return, the azimuth rate is actually twice this estimation:
233  const int nBlocksPerAzimuth = (raw->laser_return_mode==CObservationVelodyneScan::RETMODE_DUAL) ? 2 : 1;
234  std::vector<int> diffs(CObservationVelodyneScan::BLOCKS_PER_PACKET - nBlocksPerAzimuth);
235  for(int i = 0; i < CObservationVelodyneScan::BLOCKS_PER_PACKET-nBlocksPerAzimuth; ++i) {
237  diffs[i] = localDiff;
238  }
239  std::nth_element(diffs.begin(), diffs.begin() + CObservationVelodyneScan::BLOCKS_PER_PACKET/2, diffs.end()); // Calc median
240  median_azimuth_diff = diffs[CObservationVelodyneScan::BLOCKS_PER_PACKET/2];
241  }
242 
243  for (int block = 0; block < CObservationVelodyneScan::BLOCKS_PER_PACKET; block++) // Firings per packet
244  {
245  // ignore packets with mangled or otherwise different contents
246  if ((num_lasers!=64 && CObservationVelodyneScan::UPPER_BANK != raw->blocks[block].header) ||
248  {
249  cerr << "[CObservationVelodyneScan] skipping invalid packet: block " << block << " header value is " << raw->blocks[block].header;
250  continue;
251  }
252 
253  const int dsr_offset = (raw->blocks[block].header==CObservationVelodyneScan::LOWER_BANK) ? 32:0;
254  const float azimuth_raw_f = (float)(raw->blocks[block].rotation);
255  const bool block_is_dual_2nd_ranges = (raw->laser_return_mode==CObservationVelodyneScan::RETMODE_DUAL && ((block & 0x01)!=0));
256  const bool block_is_dual_last_ranges = (raw->laser_return_mode==CObservationVelodyneScan::RETMODE_DUAL && ((block & 0x01)==0));
257 
258  for (int dsr=0,k=0; dsr < SCANS_PER_FIRING; dsr++, k++)
259  {
260  if (!raw->blocks[block].laser_returns[k].distance) // Invalid return?
261  continue;
262 
263  const uint8_t rawLaserId = static_cast<uint8_t>(dsr + dsr_offset);
264  uint8_t laserId = rawLaserId;
265 
266  // Detect VLP-16 data and adjust laser id if necessary
267  bool firingWithinBlock = false;
268  if(num_lasers==16) {
269  if(laserId >= 16) {
270  laserId -= 16;
271  firingWithinBlock = true;
272  }
273  }
274 
275  ASSERT_BELOW_(laserId,num_lasers)
277 
278  // In dual return, if the distance is equal in both ranges, ignore one of them:
279  if (block_is_dual_2nd_ranges) {
280  if (raw->blocks[block].laser_returns[k].distance == raw->blocks[block-1].laser_returns[k].distance)
281  continue; // duplicated point
282  if (!params.dualKeepStrongest)
283  continue;
284  }
285  if (block_is_dual_last_ranges && !params.dualKeepLast)
286  continue;
287 
288  // Return distance:
290  if (distance<realMinDist || distance>realMaxDist)
291  continue;
292 
293  // Isolated points filtering:
294  if (params.filterOutIsolatedPoints) {
295  bool pass_filter = true;
296  const int16_t dist_this = raw->blocks[block].laser_returns[k].distance;
297  if (k>0) {
298  const int16_t dist_prev = raw->blocks[block].laser_returns[k-1].distance;
299  if (!dist_prev || std::abs(dist_this-dist_prev)>isolatedPointsFilterDistance_units)
300  pass_filter=false;
301  }
302  if (k<(SCANS_PER_FIRING-1)) {
303  const int16_t dist_next = raw->blocks[block].laser_returns[k+1].distance;
304  if (!dist_next || std::abs(dist_this-dist_next)>isolatedPointsFilterDistance_units)
305  pass_filter=false;
306  }
307  if (!pass_filter) continue; // Filter out this point
308  }
309 
310  // Azimuth correction: correct for the laser rotation as a function of timing during the firings
311  double timestampadjustment = 0.0; // [us] since beginning of scan
312  double blockdsr0 = 0.0;
313  double nextblockdsr0 = 1.0;
314  switch (num_lasers)
315  {
316  // VLP-16
317  case 16:
318  {
320  timestampadjustment = VLP16AdjustTimeStamp(block/2, laserId, firingWithinBlock);
321  nextblockdsr0 = VLP16AdjustTimeStamp(block/2+1,0,0);
322  blockdsr0 = VLP16AdjustTimeStamp(block/2,0,0);
323  }
324  else {
325  timestampadjustment = VLP16AdjustTimeStamp(block, laserId, firingWithinBlock);
326  nextblockdsr0 = VLP16AdjustTimeStamp(block+1,0,0);
327  blockdsr0 = VLP16AdjustTimeStamp(block,0,0);
328  }
329  }
330  break;
331  // HDL-32:
332  case 32:
333  timestampadjustment = HDL32AdjustTimeStamp(block, dsr);
334  nextblockdsr0 = HDL32AdjustTimeStamp(block+1,0);
335  blockdsr0 = HDL32AdjustTimeStamp(block,0);
336  break;
337  case 64:
338  break;
339  default: {
340  THROW_EXCEPTION("Error: unhandled LIDAR model!")
341  }
342  };
343 
344  const int azimuthadjustment = mrpt::utils::round( median_azimuth_diff * ((timestampadjustment - blockdsr0) / (nextblockdsr0 - blockdsr0)));
345 
346  const float azimuth_corrected_f = azimuth_raw_f + azimuthadjustment;
347  const int azimuth_corrected = ((int)round(azimuth_corrected_f)) % CObservationVelodyneScan::ROTATION_MAX_UNITS;
348 
349  // Filter by azimuth:
350  if (!((minAzimuth_int < maxAzimuth_int && azimuth_corrected >= minAzimuth_int && azimuth_corrected <= maxAzimuth_int )
351  ||(minAzimuth_int > maxAzimuth_int && (azimuth_corrected <= maxAzimuth_int || azimuth_corrected >= minAzimuth_int))))
352  continue;
353 
354  // Vertical axis mis-alignment calibration:
355  const float cos_vert_angle = calib.cosVertCorrection;
356  const float sin_vert_angle = calib.sinVertCorrection;
357  const float horz_offset = calib.horizontalOffsetCorrection;
358  const float vert_offset = calib.verticalOffsetCorrection;
359 
360  float xy_distance = distance * cos_vert_angle;
361  if (vert_offset) xy_distance+= vert_offset * sin_vert_angle;
362 
363  const int azimuth_corrected_for_lut = (azimuth_corrected + (CObservationVelodyneScan::ROTATION_MAX_UNITS/2))%CObservationVelodyneScan::ROTATION_MAX_UNITS;
364  const float cos_azimuth = lut_sincos.ccos[azimuth_corrected_for_lut];
365  const float sin_azimuth = lut_sincos.csin[azimuth_corrected_for_lut];
366 
367  // Compute raw position
368  const mrpt::math::TPoint3Df pt(
369  xy_distance * cos_azimuth + horz_offset * sin_azimuth, // MRPT +X = Velodyne +Y
370  -(xy_distance * sin_azimuth - horz_offset * cos_azimuth), // MRPT +Y = Velodyne -X
371  distance * sin_vert_angle + vert_offset
372  );
373 
374  bool add_point = true;
375  if (params.filterByROI && (
376  pt.x>params.ROI_x_max || pt.x<params.ROI_x_min ||
377  pt.y>params.ROI_y_max || pt.y<params.ROI_y_min ||
378  pt.z>params.ROI_z_max || pt.z<params.ROI_z_min))
379  add_point=false;
380 
381  if (params.filterBynROI && (
382  pt.x<=params.nROI_x_max && pt.x>=params.nROI_x_min &&
383  pt.y<=params.nROI_y_max && pt.y>=params.nROI_y_min &&
384  pt.z<=params.nROI_z_max && pt.z>=params.nROI_z_min))
385  add_point=false;
386 
387  if (!add_point)
388  continue;
389 
390  // Insert point:
391  out_pc.add_point(pt.x,pt.y,pt.z, raw->blocks[block].laser_returns[k].intensity,pkt_tim, azimuth_corrected_f);
392 
393  } // end for k,dsr=[0,31]
394  } // end for each block [0,11]
395  } // end for each data packet
396 }
397 
398 
400 {
401  struct PointCloudStorageWrapper_Inner : public PointCloudStorageWrapper {
403  const TGeneratePointCloudParameters &params_;
404  PointCloudStorageWrapper_Inner(CObservationVelodyneScan &me, const TGeneratePointCloudParameters &p) : me_(me), params_(p) {
405  // Reset point cloud:
406  me_.point_cloud.clear();
407  }
408  void add_point(double pt_x,double pt_y, double pt_z,uint8_t pt_intensity, const mrpt::system::TTimeStamp &tim, const float azimuth) MRPT_OVERRIDE {
409  me_.point_cloud.x.push_back( pt_x );
410  me_.point_cloud.y.push_back( pt_y );
411  me_.point_cloud.z.push_back( pt_z );
412  me_.point_cloud.intensity.push_back( pt_intensity );
413  if (params_.generatePerPointTimestamp) {
414  me_.point_cloud.timestamp.push_back(tim);
415  }
416  if (params_.generatePerPointAzimuth) {
417  const int azimuth_corrected = ((int)round(azimuth)) % CObservationVelodyneScan::ROTATION_MAX_UNITS;
418  me_.point_cloud.azimuth.push_back(azimuth_corrected * ROTATION_RESOLUTION);
419  }
420  }
421  };
422 
423  PointCloudStorageWrapper_Inner my_pc_wrap(*this, params);
424 
425  velodyne_scan_to_pointcloud(*this,params, my_pc_wrap);
426 
427 }
428 
430  const mrpt::poses::CPose3DInterpolator & vehicle_path,
431  std::vector<mrpt::math::TPointXYZIu8> & out_points,
432  TGeneratePointCloudSE3Results & results_stats,
434 {
435  // Pre-alloc mem:
436  out_points.reserve( out_points.size() + scan_packets.size() * BLOCKS_PER_PACKET * SCANS_PER_BLOCK + 16);
437 
438  struct PointCloudStorageWrapper_SE3_Interp : public PointCloudStorageWrapper {
440  const mrpt::poses::CPose3DInterpolator & vehicle_path_;
441  std::vector<mrpt::math::TPointXYZIu8> & out_points_;
442  TGeneratePointCloudSE3Results &results_stats_;
443  mrpt::system::TTimeStamp last_query_tim_;
444  mrpt::poses::CPose3D last_query_;
445  bool last_query_valid_;
446 
447  PointCloudStorageWrapper_SE3_Interp(CObservationVelodyneScan &me,const mrpt::poses::CPose3DInterpolator & vehicle_path,std::vector<mrpt::math::TPointXYZIu8> & out_points,TGeneratePointCloudSE3Results &results_stats) :
448  me_(me),vehicle_path_(vehicle_path),out_points_(out_points),results_stats_(results_stats),last_query_tim_(INVALID_TIMESTAMP),last_query_valid_(false)
449  {
450  }
451  void add_point(double pt_x,double pt_y, double pt_z,uint8_t pt_intensity, const mrpt::system::TTimeStamp &tim, const float azimuth) MRPT_OVERRIDE
452  {
453  // Use a cache since it's expected that the same timestamp is queried several times in a row:
454  if (last_query_tim_!=tim) {
455  last_query_tim_ = tim;
456  vehicle_path_.interpolate(tim,last_query_,last_query_valid_);
457  }
458 
459  if (last_query_valid_) {
461  global_sensor_pose.composeFrom(last_query_, me_.sensorPose);
462  double gx,gy,gz;
463  global_sensor_pose.composePoint(pt_x,pt_y,pt_z, gx,gy,gz);
464  out_points_.push_back( mrpt::math::TPointXYZIu8(gx,gy,gz,pt_intensity) );
465  ++results_stats_.num_correctly_inserted_points;
466  }
467  ++results_stats_.num_points;
468  }
469  };
470 
471  PointCloudStorageWrapper_SE3_Interp my_pc_wrap(*this,vehicle_path,out_points,results_stats);
472  velodyne_scan_to_pointcloud(*this,params, my_pc_wrap);
473 }
474 
476 {
477  x.clear();
478  y.clear();
479  z.clear();
480  intensity.clear();
481  timestamp.clear();
482  azimuth.clear();
483 }
485 {
486  { std::vector<float> d; x.swap(d); }
487  { std::vector<float> d; y.swap(d); }
488  { std::vector<float> d; z.swap(d); }
489  { std::vector<uint8_t> d; intensity.swap(d); }
490  { std::vector<mrpt::system::TTimeStamp> d; timestamp.swap(d); }
491  { std::vector<float> d; azimuth.swap(d); }
492 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer On any error, or if ZERO bytes are read...
Definition: CStream.cpp:45
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
float ROI_x_min
The limits of the 3D box (default=infinity) in sensor (not vehicle) local coordinates for the ROI fil...
float maxDistance
Minimum (default=1.0f) and maximum (default: Infinity) distances/ranges for a point to be considered...
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
#define min(a, b)
const float HDR32_FIRING_TOFFSET
bool filterOutIsolatedPoints
(Default:false) Simple filter to remove spurious returns (e.g. Sun reflected on large water extension...
static void velodyne_scan_to_pointcloud(const CObservationVelodyneScan &scan, const CObservationVelodyneScan::TGeneratePointCloudParameters &params, PointCloudStorageWrapper &out_pc)
#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).
#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 ) ...
static const uint16_t LOWER_BANK
Blocks 32-63.
mrpt::system::TTimeStamp originalReceivedTimestamp
The local computer-based timestamp based on the reception of the message in the computer.
#define THROW_EXCEPTION(msg)
mrpt::system::TTimeStamp getOriginalReceivedTimeStamp() const MRPT_OVERRIDE
By default, returns CObservation::timestamp but in sensors capable of satellite (or otherwise) accura...
#define ASSERT_BELOW_(__A, __B)
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
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...
void clear_deep()
Like clear(), but also enforcing freeing memory.
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.
STL namespace.
#define M_PI
Definition: bits.h:78
bool filterByROI
Enable ROI filter (Default:false): add points inside a given 3D box.
const float VLP16_DSR_TOFFSET
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
GLuint in
Definition: glew.h:7146
static const uint16_t ROTATION_MAX_UNITS
hundredths of degrees
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
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...
void generatePointCloudAlongSE3Trajectory(const mrpt::poses::CPose3DInterpolator &vehicle_path, std::vector< mrpt::math::TPointXYZIu8 > &out_points, TGeneratePointCloudSE3Results &results_stats, const TGeneratePointCloudParameters &params=defaultPointCloudParams)
An alternative to generatePointCloud() for cases where the motion of the sensor as it grabs one scan ...
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
pose_t & interpolate(mrpt::system::TTimeStamp 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...
float isolatedPointsFilterDistance
(Default:2.0 meters) Minimum distance between a point and its two neighbors to be considered an inval...
Lightweight 3D point (float version).
__int16 int16_t
Definition: rptypes.h:45
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:595
const float HDR32_DSR_TOFFSET
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
std::vector< PerLaserCalib > laser_corrections
CSinCosLookUpTableFor2DScans velodyne_sincos_tables
void generatePointCloud(const TGeneratePointCloudParameters &params=defaultPointCloudParams)
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
size_t num_correctly_inserted_points
Number of points for which a valid interpolated SE(3) pose could be determined.
int version
Definition: mrpt_jpeglib.h:898
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
GLfloat GLfloat p
Definition: glew.h:10113
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
uint16_t header
Block id: UPPER_BANK or LOWER_BANK.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
bool generatePerPointTimestamp
(Default:false) If true, populate the vector timestamp
bool dualKeepLast
(Default:true) In VLP16 dual mode, keep both or just one of the returns.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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:427
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
GLdouble GLdouble z
Definition: glew.h:1464
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&#39;s observation.
static const uint16_t UPPER_BANK
Blocks 0-31.
const float VLP16_BLOCK_TDURATION
GLfloat * params
Definition: glew.h:1436
uint8_t laser_return_mode
0x37: strongest, 0x38: last, 0x39: dual return
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
static const float DISTANCE_RESOLUTION
meters
const int SCANS_PER_FIRING
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
mrpt::system::TTimeStamp BASE_IMPEXP 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.cpp:197
uint16_t rotation
0-35999, divide by 100 to get degrees
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
std::vector< TVelodyneRawPacket > scan_packets
The main content of this object: raw data packet from the LIDAR.
float nROI_x_min
The limits of the 3D box (default=0) in sensor (not vehicle) local coordinates for the nROI filter...
double HDL32AdjustTimeStamp(int firingblock, int dsr)
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:49
Auxiliary class used to refactor CObservationVelodyneScan::generatePointCloud()
XYZ point (double) + Intensity(u8)
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
static const float ROTATION_RESOLUTION
degrees
double VLP16AdjustTimeStamp(int firingblock, int dsr, int firingwithinblock)
double maxRange
The maximum range allowed by the device, in meters (e.g. 100m). Stored here by the driver while captu...
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...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
const float VLP16_FIRING_TOFFSET
bool filterBynROI
Enable nROI filter (Default:false): do NOT add points inside a given 3D box.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018