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