MRPT  2.0.1
CObservation2DRangeScan.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 
12 #include <mrpt/core/round.h>
13 #include <mrpt/math/CMatrixF.h>
14 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/poses/CPosePDF.h>
18 #if MRPT_HAS_MATLAB
19 #include <mexplus.h>
20 #endif
21 
22 using namespace std;
23 using namespace mrpt::obs;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 
27 // This must be added to any CSerializable class implementation file.
29 
30 uint8_t CObservation2DRangeScan::serializeGetVersion() const { return 7; }
31 void CObservation2DRangeScan::serializeTo(
33 {
34  // The data
35  out << aperture << rightToLeft << maxRange << sensorPose;
36  uint32_t N = m_scan.size();
37  out << N;
38  ASSERT_EQUAL_(m_validRange.size(), m_scan.size());
39  if (N)
40  {
41  out.WriteBufferFixEndianness(&m_scan[0], N);
42  out.WriteBuffer(&m_validRange[0], sizeof(m_validRange[0]) * N);
43  }
44  out << stdError;
45  out << timestamp;
46  out << beamAperture;
47 
48  out << sensorLabel;
49 
50  out << deltaPitch;
51 
52  out << hasIntensity();
53  if (hasIntensity()) out.WriteBufferFixEndianness(&m_intensity[0], N);
54 }
55 
56 void CObservation2DRangeScan::truncateByDistanceAndAngle(
57  float min_distance, float max_angle, float min_height, float max_height,
58  float h)
59 {
60  // FILTER OUT INVALID POINTS!!
61  CPose3D pose;
62  unsigned int k = 0;
63  const auto nPts = m_scan.size();
64 
65  auto itValid = m_validRange.begin();
66  for (auto itScan = m_scan.begin(); itScan != m_scan.end();
67  itScan++, itValid++, k++)
68  {
69  const auto ang = std::abs(k * aperture / nPts - aperture * 0.5f);
70  float x = (*itScan) * cos(ang);
71 
72  if (min_height != 0 || max_height != 0)
73  {
74  ASSERT_(max_height > min_height);
75  if (*itScan < min_distance || ang > max_angle ||
76  x > h - min_height || x < h - max_height)
77  *itValid = false;
78  } // end if
79  else if (*itScan < min_distance || ang > max_angle)
80  *itValid = false;
81  }
82 }
83 
84 void CObservation2DRangeScan::serializeFrom(
85  mrpt::serialization::CArchive& in, uint8_t version)
86 {
87  switch (version)
88  {
89  case 0:
90  case 1:
91  case 2:
92  case 3:
93  {
94  CMatrixF covSensorPose;
95  in >> aperture >> rightToLeft >> maxRange >> sensorPose >>
96  covSensorPose;
97  uint32_t N, i;
98 
99  in >> N;
100 
101  this->resizeScan(N);
102  if (N) in.ReadBufferFixEndianness(&m_scan[0], N);
103 
104  if (version >= 1)
105  { // Load validRange:
106  if (N)
107  in.ReadBuffer(
108  &m_validRange[0], sizeof(m_validRange[0]) * N);
109  }
110  else
111  {
112  // validRange: Default values: If distance is not maxRange
113  for (i = 0; i < N; i++) m_validRange[i] = m_scan[i] < maxRange;
114  }
115 
116  if (version >= 2)
117  {
118  in >> stdError;
119  }
120  else
121  {
122  stdError = 0.01f;
123  }
124 
125  if (version >= 3)
126  {
127  in >> timestamp;
128  }
129 
130  // Default values for newer versions:
131  beamAperture = DEG2RAD(0.25f);
132 
133  deltaPitch = 0;
134  sensorLabel = "";
135  }
136  break;
137 
138  case 4:
139  case 5:
140  case 6:
141  case 7:
142  {
143  uint32_t N;
144 
145  CMatrixF covSensorPose;
146  in >> aperture >> rightToLeft >> maxRange >> sensorPose;
147 
148  if (version < 6) // covSensorPose was removed in version 6
149  in >> covSensorPose;
150 
151  in >> N;
152  this->resizeScan(N);
153  if (N)
154  {
155  in.ReadBufferFixEndianness(&m_scan[0], N);
156  in.ReadBuffer(&m_validRange[0], sizeof(m_validRange[0]) * N);
157  }
158  in >> stdError;
159  in.ReadBufferFixEndianness(&timestamp, 1);
160  in >> beamAperture;
161 
162  if (version >= 5)
163  {
164  in >> sensorLabel;
165  in >> deltaPitch;
166  }
167  else
168  {
169  sensorLabel = "";
170  deltaPitch = 0;
171  }
172  if (version >= 7)
173  {
174  bool hasIntensity;
175  in >> hasIntensity;
176  setScanHasIntensity(hasIntensity);
177  if (hasIntensity && N)
178  {
179  in.ReadBufferFixEndianness(&m_intensity[0], N);
180  }
181  }
182  }
183  break;
184  default:
186  };
187 
188  m_cachedMap.reset();
189 }
190 
191 /*---------------------------------------------------------------
192  Implements the writing to a mxArray for Matlab
193  ---------------------------------------------------------------*/
194 #if MRPT_HAS_MATLAB
195 // Add to implement mexplus::from template specialization
197 #endif
198 
199 mxArray* CObservation2DRangeScan::writeToMatlab() const
200 {
201 #if MRPT_HAS_MATLAB
202  const char* fields[] = {"class", // Data common to any MRPT class
203  "ts",
204  "sensorLabel", // Data common to any observation
205  "scan",
206  "validRange",
207  "intensity" // Received raw data
208  "aperture",
209  "rightToLeft",
210  "maxRange", // Scan plane geometry and properties
211  "stdError",
212  "beamAperture",
213  "deltaPitch", // Ray properties
214  "pose", // Sensor pose
215  "map"}; // Points map
216  mexplus::MxArray obs_struct(
217  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
218 
219  obs_struct.set("class", this->GetRuntimeClass()->className);
220 
221  obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
222  obs_struct.set("sensorLabel", this->sensorLabel);
223 
224  obs_struct.set("scan", this->m_scan);
225  // TODO: "validRange should be a vector<bool> for Matlab instead of
226  // vector<char>" ==> JLBC: It cannot be done like that since serializing
227  // "bool" is not cross-platform safe, while "char" is...
228  obs_struct.set("validRange", this->m_validRange);
229  obs_struct.set("intensity", this->m_intensity);
230  obs_struct.set("aperture", this->aperture);
231  obs_struct.set("rightToLeft", this->rightToLeft);
232  obs_struct.set("maxRange", this->maxRange);
233  obs_struct.set("stdError", this->stdError);
234  obs_struct.set("beamAperture", this->beamAperture);
235  obs_struct.set("deltaPitch", this->deltaPitch);
236  obs_struct.set("pose", this->sensorPose);
237  // TODO: obs_struct.set("map", ...)
238  return obs_struct.release();
239 #else
240  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
241 #endif
242 }
243 
244 /*---------------------------------------------------------------
245  isPlanarScan
246  ---------------------------------------------------------------*/
247 bool CObservation2DRangeScan::isPlanarScan(const double tolerance) const
248 {
249  return sensorPose.isHorizontal(tolerance);
250 }
251 
252 bool CObservation2DRangeScan::hasIntensity() const { return m_has_intensity; }
253 void CObservation2DRangeScan::setScanHasIntensity(bool setHasIntensityFlag)
254 {
255  m_has_intensity = setHasIntensityFlag;
256 }
257 
258 /*---------------------------------------------------------------
259  filterByExclusionAreas
260  ---------------------------------------------------------------*/
261 void CObservation2DRangeScan::filterByExclusionAreas(
262  const TListExclusionAreasWithRanges& areas)
263 {
264  if (areas.empty()) return;
265 
266  MRPT_START
267 
268  float Ang, dA;
269  const size_t sizeRangeScan = m_scan.size();
270 
271  ASSERT_EQUAL_(m_scan.size(), m_validRange.size());
272 
273  if (!sizeRangeScan) return;
274 
275  if (rightToLeft)
276  {
277  Ang = -0.5f * aperture;
278  dA = aperture / (sizeRangeScan - 1);
279  }
280  else
281  {
282  Ang = +0.5f * aperture;
283  dA = -aperture / (sizeRangeScan - 1);
284  }
285 
286  auto valid_it = m_validRange.begin();
287  for (auto scan_it = m_scan.begin(); scan_it != m_scan.end();
288  scan_it++, valid_it++)
289  {
290  if (!*valid_it)
291  {
292  Ang += dA;
293  continue; // Already it's invalid
294  }
295 
296  // Compute point in 2D, local to the laser center:
297  const auto Lx = *scan_it * cos(Ang);
298  const auto Ly = *scan_it * sin(Ang);
299  Ang += dA;
300 
301  // To real 3D pose:
302  double Gx, Gy, Gz;
303  this->sensorPose.composePoint(Lx, Ly, 0, Gx, Gy, Gz);
304 
305  // Filter by X,Y:
306  for (const auto& area : areas)
307  {
308  if (area.first.PointIntoPolygon(Gx, Gy) &&
309  (Gz >= area.second.first && Gz <= area.second.second))
310  {
311  *valid_it = false;
312  break; // Go for next point
313  }
314  } // for each area
315  } // for each point
316 
317  MRPT_END
318 }
319 
320 /*---------------------------------------------------------------
321  filterByExclusionAreas
322  ---------------------------------------------------------------*/
323 void CObservation2DRangeScan::filterByExclusionAreas(
324  const std::vector<mrpt::math::CPolygon>& areas)
325 {
326  if (areas.empty()) return;
327 
329  for (const auto& area : areas)
330  {
331  TListExclusionAreasWithRanges::value_type dat;
332  dat.first = area;
333  dat.second.first = -std::numeric_limits<double>::max();
334  dat.second.second = std::numeric_limits<double>::max();
335 
336  lst.push_back(dat);
337  }
338  filterByExclusionAreas(lst);
339 }
340 
341 /*---------------------------------------------------------------
342  filterByExclusionAngles
343  ---------------------------------------------------------------*/
344 void CObservation2DRangeScan::filterByExclusionAngles(
345  const std::vector<std::pair<double, double>>& angles)
346 {
347  if (angles.empty()) return;
348 
349  MRPT_START
350 
351  double Ang, dA;
352  const size_t sizeRangeScan = m_scan.size();
353 
354  ASSERT_EQUAL_(m_scan.size(), m_validRange.size());
355 
356  if (!sizeRangeScan) return;
357 
358  if (rightToLeft)
359  {
360  Ang = -0.5 * aperture;
361  dA = aperture / (sizeRangeScan - 1);
362  }
363  else
364  {
365  Ang = +0.5 * aperture;
366  dA = -aperture / (sizeRangeScan - 1);
367  }
368 
369  // For each forbiden angle range:
370  for (const auto& angle : angles)
371  {
372  int ap_idx_ini =
373  mrpt::round(mrpt::math::wrapTo2Pi(angle.first - Ang) / dA);
374  int ap_idx_end =
375  mrpt::round(mrpt::math::wrapTo2Pi(angle.second - Ang) / dA);
376 
377  if (ap_idx_ini < 0) ap_idx_ini = 0;
378  if (ap_idx_end < 0) ap_idx_end = 0;
379 
380  if (ap_idx_ini > static_cast<int>(sizeRangeScan))
381  ap_idx_ini = sizeRangeScan - 1;
382  if (ap_idx_end > static_cast<int>(sizeRangeScan))
383  ap_idx_end = sizeRangeScan - 1;
384 
385  const size_t idx_ini = ap_idx_ini;
386  const size_t idx_end = ap_idx_end;
387 
388  if (idx_end >= idx_ini)
389  {
390  for (size_t i = idx_ini; i <= idx_end; i++) m_validRange[i] = false;
391  }
392  else
393  {
394  for (size_t i = 0; i < idx_end; i++) m_validRange[i] = false;
395 
396  for (size_t i = idx_ini; i < sizeRangeScan; i++)
397  m_validRange[i] = false;
398  }
399  }
400 
401  MRPT_END
402 }
403 
404 namespace mrpt::obs
405 {
406 // Tricky way to call to a library that depends on us, a sort of "run-time"
407 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
408 // "mrpt-obs", set by "mrpt-maps" at its startup.
409 using scan2pts_functor = void (*)(
411  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
412 
415 
417 {
419 }
420 } // namespace mrpt::obs
421 
422 /*---------------------------------------------------------------
423  internal_buildAuxPointsMap
424  ---------------------------------------------------------------*/
425 void CObservation2DRangeScan::internal_buildAuxPointsMap(
426  const void* options) const
427 {
429  throw std::runtime_error(
430  "[CObservation2DRangeScan::buildAuxPointsMap] ERROR: This function "
431  "needs linking against mrpt-maps.\n");
432 
433  (*ptr_internal_build_points_map_from_scan2D)(*this, m_cachedMap, options);
434 }
435 
436 /** Fill out a T2DScanProperties structure with the parameters of this scan */
437 void CObservation2DRangeScan::getScanProperties(T2DScanProperties& p) const
438 {
439  p.nRays = m_scan.size();
440  p.aperture = this->aperture;
441  p.rightToLeft = this->rightToLeft;
442 }
443 
445  const T2DScanProperties& a, const T2DScanProperties& b)
446 {
447  if (a.nRays != b.nRays) return a.nRays < b.nRays;
448  if (a.aperture != b.aperture) return a.aperture < b.aperture;
449  if (a.rightToLeft != b.rightToLeft) return a.rightToLeft;
450  return false;
451 }
452 
453 void CObservation2DRangeScan::getDescriptionAsText(std::ostream& o) const
454 {
455  CObservation::getDescriptionAsText(o);
456  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
457  "base:\n";
458  o << sensorPose.getHomogeneousMatrixVal<CMatrixDouble44>() << sensorPose
459  << endl;
460 
461  o << format(
462  "Samples direction: %s\n",
463  (rightToLeft) ? "Right->Left" : "Left->Right");
464  o << format("Points in the scan: %u\n", m_scan.size());
465  o << format("Estimated sensor 'sigma': %f\n", stdError);
466  o << format(
467  "Increment in pitch during the scan: %f deg\n", RAD2DEG(deltaPitch));
468 
469  size_t i, inval = 0;
470  for (i = 0; i < m_scan.size(); i++)
471  if (!m_validRange[i]) inval++;
472  o << format("Invalid points in the scan: %u\n", (unsigned)inval);
473 
474  o << format("Sensor maximum range: %.02f m\n", maxRange);
475  o << format(
476  "Sensor field-of-view (\"aperture\"): %.01f deg\n", RAD2DEG(aperture));
477 
478  o << "Raw scan values: [";
479  for (i = 0; i < m_scan.size(); i++) o << format("%.03f ", m_scan[i]);
480  o << "]\n";
481 
482  o << "Raw valid-scan values: [";
483  for (i = 0; i < m_validRange.size(); i++)
484  o << format("%u ", m_validRange[i] ? 1 : 0);
485  o << "]\n\n";
486 
487  if (hasIntensity())
488  {
489  o << "Raw intensity values: [";
490  for (i = 0; i < m_intensity.size(); i++)
491  o << format("%d ", m_intensity[i]);
492  o << "]\n\n";
493  }
494 }
495 
496 const float& CObservation2DRangeScan::getScanRange(const size_t i) const
497 {
498  ASSERT_BELOW_(i, m_scan.size());
499  return m_scan[i];
500 }
501 float& CObservation2DRangeScan::getScanRange(const size_t i)
502 {
503  ASSERT_BELOW_(i, m_scan.size());
504  return m_scan[i];
505 }
506 
507 void CObservation2DRangeScan::setScanRange(const size_t i, const float val)
508 {
509  ASSERT_BELOW_(i, m_scan.size());
510  m_scan[i] = val;
511 }
512 
513 const int32_t& CObservation2DRangeScan::getScanIntensity(const size_t i) const
514 {
515  ASSERT_BELOW_(i, m_intensity.size());
516  return m_intensity[i];
517 }
518 int32_t& CObservation2DRangeScan::getScanIntensity(const size_t i)
519 {
520  ASSERT_BELOW_(i, m_intensity.size());
521  return m_intensity[i];
522 }
523 void CObservation2DRangeScan::setScanIntensity(const size_t i, const int val)
524 {
525  ASSERT_BELOW_(i, m_intensity.size());
526  m_intensity[i] = val;
527 }
528 
529 bool CObservation2DRangeScan::getScanRangeValidity(const size_t i) const
530 {
531  ASSERT_BELOW_(i, m_validRange.size());
532  return m_validRange[i] != 0;
533 }
534 void CObservation2DRangeScan::setScanRangeValidity(
535  const size_t i, const bool val)
536 {
537  ASSERT_BELOW_(i, m_validRange.size());
538  m_validRange[i] = val ? 1 : 0;
539 }
540 
541 void CObservation2DRangeScan::resizeScan(const size_t len)
542 {
543  m_scan.resize(len);
544  m_intensity.resize(len);
545  m_validRange.resize(len);
546 }
547 
548 void CObservation2DRangeScan::resizeScanAndAssign(
549  const size_t len, const float rangeVal, const bool rangeValidity,
550  const int32_t rangeIntensity)
551 {
552  m_scan.assign(len, rangeVal);
553  m_validRange.assign(len, rangeValidity);
554  m_intensity.assign(len, rangeIntensity);
555 }
556 
557 size_t CObservation2DRangeScan::getScanSize() const { return m_scan.size(); }
558 void CObservation2DRangeScan::loadFromVectors(
559  size_t nRays, const float* scanRanges, const char* scanValidity)
560 {
561  ASSERT_(scanRanges);
562  ASSERT_(scanValidity);
563  resizeScan(nRays);
564  for (size_t i = 0; i < nRays; i++)
565  {
566  m_scan[i] = scanRanges[i];
567  m_validRange[i] = scanValidity[i];
568  }
569 }
static double toDouble(const time_point t) noexcept
Converts a timestamp to a UNIX time_t-like number, with fractional part.
Definition: Clock.cpp:106
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
scan2pts_functor ptr_internal_build_points_map_from_scan2D
#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 constexpr size_type size()
Definition: CPose3D.h:697
bool operator<(const T2DScanProperties &a, const T2DScanProperties &b)
Order operator, so T2DScanProperties can appear in associative STL containers.
void internal_set_build_points_map_from_scan2D(scan2pts_functor fn)
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:38
This namespace contains representation of robot actions and observations.
int val
Definition: mrpt_jpeglib.h:957
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:18
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
std::vector< std::pair< mrpt::math::CPolygon, std::pair< double, double > >> TListExclusionAreasWithRanges
Used in filterByExclusionAreas.
#define MRPT_END
Definition: exceptions.h:245
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
void(*)(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps) scan2pts_functor
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020