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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019