MRPT  1.9.9
CObservationRotatingScan.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 "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/math/wrap2pi.h>
13 #include <mrpt/obs/CObservation.h>
20 
21 using namespace std;
22 using namespace mrpt::obs;
23 
24 // This must be added to any CSerializable class implementation file.
26 
27 // static CSinCosLookUpTableFor2DScans velodyne_sincos_tables;
28 
30 
31 mrpt::system::TTimeStamp RotScan::getOriginalReceivedTimeStamp() const
32 {
33  return originalReceivedTimestamp;
34 }
35 
36 uint8_t RotScan::serializeGetVersion() const { return 0; }
38 {
39  out << timestamp << sensorLabel << rowCount << columnCount
40  << rangeResolution << startAzimuth << azimuthSpan << sweepDuration
41  << lidarModel << minRange << maxRange << sensorPose
42  << originalReceivedTimestamp << has_satellite_timestamp;
43 
44  out.WriteAs<uint16_t>(rangeImage.cols());
45  out.WriteAs<uint16_t>(rangeImage.rows());
46  if (!rangeImage.empty())
47  out.WriteBufferFixEndianness(&rangeImage(0, 0), rangeImage.size());
48 
49  out.WriteAs<uint16_t>(intensityImage.cols());
50  out.WriteAs<uint16_t>(intensityImage.rows());
51  if (!intensityImage.empty())
52  out.WriteBufferFixEndianness(
53  &intensityImage(0, 0), intensityImage.size());
54 
55  out.WriteAs<uint16_t>(rangeOtherLayers.size());
56  for (const auto& ly : rangeOtherLayers)
57  {
58  out << ly.first;
59  ASSERT_EQUAL_(ly.second.cols(), columnCount);
60  ASSERT_EQUAL_(ly.second.rows(), rowCount);
61  out.WriteBufferFixEndianness(&ly.second(0, 0), ly.second.size());
62  }
63 }
64 
66 {
67  switch (version)
68  {
69  case 0:
70  {
71  in >> timestamp >> sensorLabel >> rowCount >> columnCount >>
72  rangeResolution >> startAzimuth >> azimuthSpan >>
73  sweepDuration >> lidarModel >> minRange >> maxRange >>
74  sensorPose >> originalReceivedTimestamp >>
75  has_satellite_timestamp;
76 
77  const auto nCols = in.ReadAs<uint16_t>(),
78  nRows = in.ReadAs<uint16_t>();
79  rangeImage.resize(nRows, nCols);
80  if (!rangeImage.empty())
81  in.ReadBufferFixEndianness(
82  &rangeImage(0, 0), rangeImage.size());
83 
84  {
85  const auto nIntCols = in.ReadAs<uint16_t>(),
86  nIntRows = in.ReadAs<uint16_t>();
87  intensityImage.resize(nIntRows, nIntCols);
88  if (!intensityImage.empty())
89  in.ReadBufferFixEndianness(
90  &intensityImage(0, 0), intensityImage.size());
91  }
92 
93  const auto nOtherLayers = in.ReadAs<uint16_t>();
94  rangeOtherLayers.clear();
95  for (size_t i = 0; i < nOtherLayers; i++)
96  {
98  in >> name;
99  auto& im = rangeOtherLayers[name];
100  im.resize(nRows, nCols);
101  in.ReadBufferFixEndianness(&im(0, 0), im.size());
102  }
103  }
104  break;
105  default:
107  };
108 }
109 
110 void RotScan::getDescriptionAsText(std::ostream& o) const
111 {
112  CObservation::getDescriptionAsText(o);
113  o << "Homogeneous matrix for the sensor 3D pose, relative to "
114  "robot base:\n";
115  o << sensorPose.getHomogeneousMatrixVal<mrpt::math::CMatrixDouble44>()
116  << "\n"
117  << sensorPose << endl;
118 
119  o << "lidarModel: " << lidarModel << "\n";
120  o << "Range rows=" << rowCount << " cols=" << columnCount << "\n";
121  o << "Range resolution=" << rangeResolution << " [meter]\n";
122  o << "Scan azimuth: start=" << mrpt::RAD2DEG(startAzimuth)
123  << " span=" << mrpt::RAD2DEG(azimuthSpan) << "\n";
124  o << "Sweep duration: " << sweepDuration << " [s]\n";
125  o << mrpt::format(
126  "Sensor min/max range: %.02f / %.02f m\n", minRange, maxRange);
127  o << "has_satellite_timestamp: " << (has_satellite_timestamp ? "YES" : "NO")
128  << "\n";
129  o << "originalReceivedTimestamp: "
130  << mrpt::system::dateTimeToString(originalReceivedTimestamp)
131  << " (UTC)\n";
132 }
133 
134 MRPT_TODO("toPointCloud / calibration");
135 
137 {
139  using degree_cents = uint16_t;
140  using gps_microsecs = uint32_t;
141 
142  MRPT_START
143 
144  // Reset:
145  *this = CObservationRotatingScan();
146 
147  // Copy properties:
148  has_satellite_timestamp = o.has_satellite_timestamp;
149  originalReceivedTimestamp = o.originalReceivedTimestamp;
150  timestamp = o.timestamp;
151  sensorPose = o.sensorPose;
152  sensorLabel = o.sensorLabel;
153  minRange = o.minRange;
154  maxRange = o.maxRange;
155 
156  // Convert ranges to range images:
157 
158  uint8_t model = 0;
159  gps_microsecs last_pkt_tim = std::numeric_limits<gps_microsecs>::max();
160  degree_cents last_pkt_az = 0; // last azimuth
161 
162  // Azimuth (wrt sensor) at the beginning of one packet, to its timestamp:
163  std::map<degree_cents, gps_microsecs> azimuth2timestamp;
164  std::multiset<double> rotspeed; // per packet estimated rot speed (deg/sec)
165 
166  // column count:
167  const size_t num_lasers = o.calibration.laser_corrections.size();
168  ASSERT_ABOVE_(num_lasers, 2);
169  rowCount = num_lasers;
170 
171  // row count:
172  columnCount =
174 
175  const double timeBetweenLastTwoBlocks =
176  1e-6 * (o.scan_packets.rbegin()->gps_timestamp -
177  (o.scan_packets.rbegin() + 1)->gps_timestamp);
178 
179  rangeImage.setZero(rowCount, columnCount);
180  intensityImage.setZero(rowCount, columnCount);
181  rangeOtherLayers.clear();
182  rangeResolution = Velo::DISTANCE_RESOLUTION;
183  azimuthSpan = 0;
184 
185  ASSERT_ABOVEEQ_(o.scan_packets.size(), 1);
186 
187  for (size_t pktIdx = 0; pktIdx < o.scan_packets.size(); pktIdx++)
188  {
189  const auto& pkt = o.scan_packets[pktIdx];
190 
191  model = pkt.velodyne_model_ID;
192 
193  const degree_cents pkt_azimuth = pkt.blocks[0].rotation;
194 
195  if (last_pkt_tim != std::numeric_limits<uint32_t>::max())
196  {
197  // Estimate rot speed:
198  ASSERT_ABOVE_(pkt.gps_timestamp, last_pkt_tim);
199  const double dT = 1e-6 * (pkt.gps_timestamp - last_pkt_tim);
200  const auto dAzimuth = 1e-2 * (pkt_azimuth - last_pkt_az);
201  const auto estRotVel = dAzimuth / dT;
202  rotspeed.insert(estRotVel);
203  }
204  last_pkt_tim = pkt.gps_timestamp;
205  last_pkt_az = pkt_azimuth;
206 
207  azimuth2timestamp[pkt_azimuth] = pkt.gps_timestamp;
208 
209  // Accum azimuth span:
210  if (pktIdx + 1 == o.scan_packets.size())
211  {
212  // last packet:
213  // sanity checks: rot speed should be fairly stable:
214  const double maxRotSpeed = *rotspeed.rbegin(),
215  minRotSpeed = *rotspeed.begin();
216  ASSERT_ABOVE_(maxRotSpeed, 0);
217  ASSERT_BELOW_((maxRotSpeed - minRotSpeed) / maxRotSpeed, 0.01);
218 
219  // Median speed:
220  const double rotVel_degps = [&]() {
221  auto it = rotspeed.begin();
222  std::advance(it, rotspeed.size() / 2);
223  return *it;
224  }();
225 
226  azimuthSpan +=
227  mrpt::DEG2RAD(rotVel_degps * timeBetweenLastTwoBlocks);
228  }
229  else
230  {
231  // non-last packet:
232  const double curAng =
233  0.01 * o.scan_packets[pktIdx].blocks[0].rotation;
234  const double nextAng =
235  0.01 * o.scan_packets[pktIdx + 1].blocks[0].rotation;
236 
237  const double incrAng = mrpt::math::angDistance(
238  mrpt::DEG2RAD(curAng), mrpt::DEG2RAD(nextAng));
239  azimuthSpan += incrAng;
240  }
241 
242  // Process each block in this packet:
243  for (int block = 0; block < Velo::BLOCKS_PER_PACKET; block++)
244  {
245  const int dsr_offset =
246  (pkt.blocks[block].header == Velo::LOWER_BANK) ? 32 : 0;
247  const bool block_is_dual_strongest_range =
248  (pkt.laser_return_mode == Velo::RETMODE_DUAL &&
249  ((block & 0x01) != 0));
250  const bool block_is_dual_last_range =
251  (pkt.laser_return_mode == Velo::RETMODE_DUAL &&
252  ((block & 0x01) == 0));
253 
254  for (int dsr = 0, k = 0; dsr < Velo::SCANS_PER_FIRING; dsr++, k++)
255  {
256  if (!pkt.blocks[block]
257  .laser_returns[k]
258  .distance) // Invalid return?
259  continue;
260 
261  const auto rawLaserId = static_cast<uint8_t>(dsr + dsr_offset);
262  uint8_t laserId = rawLaserId;
263 
264  // Detect VLP-16 data and adjust laser id if necessary
265  // bool firingWithinBlock = false;
266  if (num_lasers == 16)
267  {
268  if (laserId >= 16)
269  {
270  laserId -= 16;
271  // firingWithinBlock = true;
272  }
273  }
274 
275  ASSERT_BELOW_(laserId, num_lasers);
276  const auto& calib = o.calibration.laser_corrections[laserId];
277 
278  // In dual return, if the distance is equal in both ranges,
279  // ignore one of them:
280 
281  const auto distance =
282  pkt.blocks[block].laser_returns[k].distance +
283  static_cast<uint16_t>(
284  calib.distanceCorrection / Velo::DISTANCE_RESOLUTION);
285 
286  const auto columnIdx = [&]() {
287  switch (num_lasers)
288  {
289  case 16:
290  case 32:
291  case 64:
292  {
293  int c = (dsr + block * Velo::SCANS_PER_BLOCK +
294  pktIdx * Velo::SCANS_PER_PACKET) /
295  num_lasers;
296  if (pkt.laser_return_mode == Velo::RETMODE_DUAL)
297  c /= 2;
298  return c;
299  }
300  default:
301  {
302  THROW_EXCEPTION("Error: unhandled LIDAR model!");
303  }
304  };
305  }();
306 
307  ASSERT_BELOW_(columnIdx, columnCount);
308  if (pkt.laser_return_mode != Velo::RETMODE_DUAL ||
309  block_is_dual_strongest_range)
310  {
311  // Regular range, or strongest in multi return mode:
312  rangeImage(laserId, columnIdx) = distance;
313 
314  // Intensity:
315  intensityImage(laserId, columnIdx) =
316  pkt.blocks[block].laser_returns[k].intensity;
317  }
318  else if (block_is_dual_last_range)
319  {
320  // Regular range, or strongest in multi return mode:
321  auto& r = rangeOtherLayers["STRONGEST"];
322  // 1st time init:
323  if (static_cast<size_t>(r.rows()) != num_lasers)
324  r.setZero(rowCount, columnCount);
325 
326  r(laserId, columnIdx) = distance;
327  }
328 
329  } // end for k,dsr=[0,31]
330  } // end for each block [0,11]
331  }
332 
333  // Start and end azimuth:
334  startAzimuth =
335  mrpt::DEG2RAD(o.scan_packets.begin()->blocks[0].rotation * 1e-2);
336 
337  const auto microsecs_1st_pkt = o.scan_packets.begin()->gps_timestamp;
338  const auto microsecs_last_pkt = o.scan_packets.rbegin()->gps_timestamp;
339  sweepDuration = 1e-6 * (microsecs_last_pkt - microsecs_1st_pkt) +
340  timeBetweenLastTwoBlocks;
341 
342  // Decode model byte:
343  switch (model)
344  {
345  case 0x21:
346  lidarModel = "HDL-32E";
347  break;
348  case 0x22:
349  lidarModel = "VLP-16";
350  break;
351  default:
352  lidarModel = "Unknown";
353  break;
354  };
355 
356  MRPT_END
357 }
358 
360 {
361  MRPT_START
362 
363  // Reset:
364  *this = CObservationRotatingScan();
365 
366  // Copy properties:
367  this->has_satellite_timestamp = false;
368  this->timestamp = o.timestamp;
369  this->sensorPose = o.sensorPose;
370  this->sensorLabel = o.sensorLabel;
371  this->maxRange = o.maxRange;
372 
373  // Convert ranges to range images:
374  this->rowCount = 1;
375  this->columnCount = o.getScanSize();
376 
377  this->rangeImage.setZero(rowCount, columnCount);
378  this->intensityImage.setZero(rowCount, columnCount);
379  this->rangeOtherLayers.clear();
380  this->rangeResolution = 0.01;
381  this->azimuthSpan = o.aperture * (o.rightToLeft ? +1.0 : -1.0);
382  this->startAzimuth = o.aperture * (o.rightToLeft ? -0.5 : +0.5);
383 
384  for (size_t i = 0; i < o.getScanSize(); i++)
385  {
386  uint16_t& range_out = rangeImage(0, i);
387  uint8_t& intensity_out = intensityImage(0, i);
388  range_out = 0;
389  intensity_out = 0;
390 
391  // Convert range into discrete units:
392  const float r = o.getScanRange(i);
393  const uint16_t r_discr = static_cast<uint16_t>(
394  (r / static_cast<float>(rangeResolution)) + 0.5f);
395 
396  if (!o.getScanRangeValidity(i) || r <= 0 || r >= o.maxRange) continue;
397 
398  range_out = r_discr;
399 
400  if (o.hasIntensity()) intensity_out = o.getScanIntensity(i);
401  }
402 
403  this->lidarModel = std::string("2D_SCAN_") + this->sensorLabel;
404 
405  MRPT_END
406 }
407 
409 {
410  MRPT_START
411  MRPT_TODO("fromPointCloud");
412  THROW_EXCEPTION("fromPointCloud() not implemented yet");
413  MRPT_END
414 }
415 
417 {
418  MRPT_START
419 
420  if (auto oVel = dynamic_cast<const CObservationVelodyneScan*>(&o); oVel)
421  {
422  fromVelodyne(*oVel);
423  return true;
424  }
425  if (auto o2D = dynamic_cast<const CObservation2DRangeScan*>(&o); o2D)
426  {
427  fromScan2D(*o2D);
428  return true;
429  }
430  if (auto oPc = dynamic_cast<const CObservationPointCloud*>(&o); oPc)
431  {
432  fromPointCloud(*oPc);
433  return true;
434  }
435  return false;
436 
437  MRPT_END
438 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
void fromPointCloud(const mrpt::obs::CObservationPointCloud &o)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
An observation from any sensor that can be summarized as a pointcloud.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:95
mrpt::obs::VelodyneCalibration calibration
The calibration data for the LIDAR device.
bool has_satellite_timestamp
If true, CObservation::timestamp has been generated from accurate satellite clock.
STL namespace.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
size_t getScanSize() const
Get number of scan rays.
void fromVelodyne(const mrpt::obs::CObservationVelodyneScan &o)
float maxRange
The maximum range allowed by the device, in meters (e.g.
#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
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
double minRange
The maximum range allowed by the device, in meters (e.g.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
constexpr double DEG2RAD(const double x)
Degrees to radians.
const GLubyte * c
Definition: glext.h:6406
std::vector< PerLaserCalib > laser_corrections
This namespace contains representation of robot actions and observations.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
GLsizei const GLchar ** string
Definition: glext.h:4116
const int32_t & getScanIntensity(const size_t i) const
The intensity values of the scan.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
bool fromGeneric(const mrpt::obs::CObservation &o)
Will convert from another observation if it&#39;s any of the supported source types (see fromVelodyne()...
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
std::string dateTimeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC time): YEAR/MONTH/DAY,HH:MM:SS.MMM.
Definition: datetime.cpp:154
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
GLuint const GLchar * name
Definition: glext.h:4068
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
void fromScan2D(const mrpt::obs::CObservation2DRangeScan &o)
MRPT_TODO("toPointCloud / calibration")
bool hasIntensity() const
Return true if scan has intensity.
std::vector< TVelodyneRawPacket > scan_packets
The main content of this object: raw data packet from the LIDAR.
A CObservation-derived class for raw range data from a 2D or 3D rotating scanner. ...
static constexpr float DISTANCE_RESOLUTION
meters
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
bool getScanRangeValidity(const size_t i) const
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019