Main MRPT website > C++ reference for MRPT 1.9.9
CObservation2DRangeScan.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-2018, 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 
13 #include <mrpt/poses/CPosePDF.h>
15 #include <mrpt/math/CMatrix.h>
16 #include <mrpt/math/wrap2pi.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  : scan(m_scan), // proxy ctor
32  intensity(m_intensity), // proxy ctor
33  validRange(m_validRange) // proxy ctor
34 {
35  // rely on compiler-generated copy op + the custom = operator in proxies.
36  *this = o;
37 }
38 
39 uint8_t CObservation2DRangeScan::serializeGetVersion() const { return 7; }
40 void CObservation2DRangeScan::serializeTo(
42 {
43  // The data
44  out << aperture << rightToLeft << maxRange << sensorPose;
45  uint32_t N = scan.size();
46  out << N;
47  ASSERT_(validRange.size() == scan.size());
48  if (N)
49  {
50  out.WriteBufferFixEndianness(&m_scan[0], N);
51  out.WriteBuffer(&m_validRange[0], sizeof(m_validRange[0]) * N);
52  }
53  out << stdError;
54  out << timestamp;
55  out << beamAperture;
56 
57  out << sensorLabel;
58 
59  out << deltaPitch;
60 
61  out << hasIntensity();
62  if (hasIntensity()) out.WriteBufferFixEndianness(&m_intensity[0], N);
63 }
64 
65 void CObservation2DRangeScan::truncateByDistanceAndAngle(
66  float min_distance, float max_angle, float min_height, float max_height,
67  float h)
68 {
69  // FILTER OUT INVALID POINTS!!
70  CPose3D pose;
71  unsigned int k = 0;
72  unsigned int nPts = scan.size();
73 
74 
75  auto itValid = m_validRange.begin();
76  for (auto itScan = m_scan.begin(); itScan != m_scan.end(); itScan++, itValid++, k++)
77  {
78  float ang = fabs(k * aperture / nPts - aperture * 0.5);
79  float x = (*itScan) * cos(ang);
80 
81  if (min_height != 0 || max_height != 0)
82  {
83  ASSERT_(max_height > min_height);
84  if (*itScan < min_distance || ang > max_angle ||
85  x > h - min_height || x < h - max_height)
86  *itValid = false;
87  } // end if
88  else if (*itScan < min_distance || ang > max_angle)
89  *itValid = false;
90  }
91 }
92 
93 void CObservation2DRangeScan::serializeFrom(
95 {
96  switch (version)
97  {
98  case 0:
99  case 1:
100  case 2:
101  case 3:
102  {
103  CMatrix covSensorPose;
104  in >> aperture >> rightToLeft >> maxRange >> sensorPose >>
105  covSensorPose;
106  uint32_t N, i;
107 
108  in >> N;
109 
110  this->resizeScan(N);
111  if (N) in.ReadBufferFixEndianness(&m_scan[0], N);
112 
113  if (version >= 1)
114  { // Load validRange:
115  if (N)
116  in.ReadBuffer(
117  &m_validRange[0], sizeof(m_validRange[0]) * N);
118  }
119  else
120  {
121  // validRange: Default values: If distance is not maxRange
122  for (i = 0; i < N; i++) m_validRange[i] = scan[i] < maxRange;
123  }
124 
125  if (version >= 2)
126  {
127  in >> stdError;
128  }
129  else
130  {
131  stdError = 0.01f;
132  }
133 
134  if (version >= 3)
135  {
136  in >> timestamp;
137  }
138 
139  // Default values for newer versions:
140  beamAperture = DEG2RAD(0.25f);
141 
142  deltaPitch = 0;
143  sensorLabel = "";
144  }
145  break;
146 
147  case 4:
148  case 5:
149  case 6:
150  case 7:
151  {
152  uint32_t N;
153 
154  CMatrix covSensorPose;
155  in >> aperture >> rightToLeft >> maxRange >> sensorPose;
156 
157  if (version < 6) // covSensorPose was removed in version 6
158  in >> covSensorPose;
159 
160  in >> N;
161  this->resizeScan(N);
162  if (N)
163  {
164  in.ReadBufferFixEndianness(&m_scan[0], N);
165  in.ReadBuffer(&m_validRange[0], sizeof(m_validRange[0]) * N);
166  }
167  in >> stdError;
168  in.ReadBufferFixEndianness(&timestamp, 1);
169  in >> beamAperture;
170 
171  if (version >= 5)
172  {
173  in >> sensorLabel;
174  in >> deltaPitch;
175  }
176  else
177  {
178  sensorLabel = "";
179  deltaPitch = 0;
180  }
181  if (version >= 7)
182  {
183  bool hasIntensity;
184  in >> hasIntensity;
185  setScanHasIntensity(hasIntensity);
186  if (hasIntensity && N)
187  {
188  in.ReadBufferFixEndianness(&m_intensity[0], N);
189  }
190  }
191  }
192  break;
193  default:
195  };
196 
197  m_cachedMap.reset();
198 }
199 
200 /*---------------------------------------------------------------
201  Implements the writing to a mxArray for Matlab
202  ---------------------------------------------------------------*/
203 #if MRPT_HAS_MATLAB
204 // Add to implement mexplus::from template specialization
206 #endif
207 
208 mxArray* CObservation2DRangeScan::writeToMatlab() const
209 {
210 #if MRPT_HAS_MATLAB
211  const char* fields[] = {"class", // Data common to any MRPT class
212  "ts",
213  "sensorLabel", // Data common to any observation
214  "scan", "validRange",
215  "intensity" // Received raw data
216  "aperture",
217  "rightToLeft",
218  "maxRange", // Scan plane geometry and properties
219  "stdError", "beamAperture",
220  "deltaPitch", // Ray properties
221  "pose", // Sensor pose
222  "map"}; // Points map
223  mexplus::MxArray obs_struct(
224  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
225 
226  obs_struct.set("class", this->GetRuntimeClass()->className);
227 
228  obs_struct.set("ts", this->timestamp);
229  obs_struct.set("sensorLabel", this->sensorLabel);
230 
231  obs_struct.set("scan", this->m_scan);
232  // TODO: "validRange should be a vector<bool> for Matlab instead of
233  // vector<char>" ==> JLBC: It cannot be done like that since serializing
234  // "bool" is not cross-platform safe, while "char" is...
235  obs_struct.set("validRange", this->m_validRange);
236  obs_struct.set("intensity", this->m_intensity);
237  obs_struct.set("aperture", this->aperture);
238  obs_struct.set("rightToLeft", this->rightToLeft);
239  obs_struct.set("maxRange", this->maxRange);
240  obs_struct.set("stdError", this->stdError);
241  obs_struct.set("beamAperture", this->beamAperture);
242  obs_struct.set("deltaPitch", this->deltaPitch);
243  obs_struct.set("pose", this->sensorPose);
244  // TODO: obs_struct.set("map", ...)
245  return obs_struct.release();
246 #else
247  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
248 #endif
249 }
250 
251 /*---------------------------------------------------------------
252  isPlanarScan
253  ---------------------------------------------------------------*/
254 bool CObservation2DRangeScan::isPlanarScan(const double tolerance) const
255 {
256  return sensorPose.isHorizontal(tolerance);
257 }
258 
259 bool CObservation2DRangeScan::hasIntensity() const { return m_has_intensity; }
260 void CObservation2DRangeScan::setScanHasIntensity(bool setHasIntensityFlag)
261 {
262  m_has_intensity = setHasIntensityFlag;
263 }
264 
265 /*---------------------------------------------------------------
266  filterByExclusionAreas
267  ---------------------------------------------------------------*/
268 void CObservation2DRangeScan::filterByExclusionAreas(
269  const TListExclusionAreasWithRanges& areas)
270 {
271  if (areas.empty()) return;
272 
273  MRPT_START
274 
275  double Ang, dA;
276  size_t sizeRangeScan = scan.size();
277 
278  ASSERT_(scan.size() == validRange.size());
279 
280  if (!sizeRangeScan) return;
281 
282  if (rightToLeft)
283  {
284  Ang = -0.5 * aperture;
285  dA = aperture / (sizeRangeScan - 1);
286  }
287  else
288  {
289  Ang = +0.5 * aperture;
290  dA = -aperture / (sizeRangeScan - 1);
291  }
292 
293  auto valid_it = m_validRange.begin();
294  for (auto scan_it = m_scan.begin(); scan_it != m_scan.end(); scan_it++, valid_it++)
295  {
296  if (!*valid_it)
297  {
298  Ang += dA;
299  continue; // Already it's invalid
300  }
301 
302  // Compute point in 2D, local to the laser center:
303  const double Lx = *scan_it * cos(Ang);
304  const double Ly = *scan_it * sin(Ang);
305  Ang += dA;
306 
307  // To real 3D pose:
308  double Gx, Gy, Gz;
309  this->sensorPose.composePoint(Lx, Ly, 0, Gx, Gy, Gz);
310 
311  // Filter by X,Y:
312  for (TListExclusionAreasWithRanges::const_iterator i = areas.begin();
313  i != areas.end(); ++i)
314  {
315  if (i->first.PointIntoPolygon(Gx, Gy) &&
316  (Gz >= i->second.first && Gz <= i->second.second))
317  {
318  *valid_it = false;
319  break; // Go for next point
320  }
321  } // for each area
322  } // for each point
323 
324  MRPT_END
325 }
326 
327 /*---------------------------------------------------------------
328  filterByExclusionAreas
329  ---------------------------------------------------------------*/
330 void CObservation2DRangeScan::filterByExclusionAreas(
331  const std::vector<mrpt::math::CPolygon>& areas)
332 {
333  if (areas.empty()) return;
334 
336  for (size_t i = 0; i < areas.size(); i++)
337  {
338  TListExclusionAreasWithRanges::value_type dat;
339  dat.first = areas[i];
340  dat.second.first = -std::numeric_limits<double>::max();
341  dat.second.second = std::numeric_limits<double>::max();
342 
343  lst.push_back(dat);
344  }
345  filterByExclusionAreas(lst);
346 }
347 
348 /*---------------------------------------------------------------
349  filterByExclusionAngles
350  ---------------------------------------------------------------*/
351 void CObservation2DRangeScan::filterByExclusionAngles(
352  const std::vector<std::pair<double, double>>& angles)
353 {
354  if (angles.empty()) return;
355 
356  MRPT_START
357 
358  double Ang, dA;
359  const size_t sizeRangeScan = scan.size();
360 
361  ASSERT_(scan.size() == validRange.size());
362 
363  if (!sizeRangeScan) return;
364 
365  if (rightToLeft)
366  {
367  Ang = -0.5 * aperture;
368  dA = aperture / (sizeRangeScan - 1);
369  }
370  else
371  {
372  Ang = +0.5 * aperture;
373  dA = -aperture / (sizeRangeScan - 1);
374  }
375 
376  // For each forbiden angle range:
377  for (vector<pair<double, double>>::const_iterator itA = angles.begin();
378  itA != angles.end(); ++itA)
379  {
380  int ap_idx_ini = mrpt::math::wrapTo2Pi(itA->first - Ang) /
381  dA; // The signs are all right! ;-)
382  int ap_idx_end = mrpt::math::wrapTo2Pi(itA->second - Ang) / dA;
383 
384  if (ap_idx_ini < 0) ap_idx_ini = 0;
385  if (ap_idx_end < 0) ap_idx_end = 0;
386 
387  if (ap_idx_ini > (int)sizeRangeScan) ap_idx_ini = sizeRangeScan - 1;
388  if (ap_idx_end > (int)sizeRangeScan) ap_idx_end = sizeRangeScan - 1;
389 
390  const size_t idx_ini = ap_idx_ini;
391  const size_t idx_end = ap_idx_end;
392 
393  if (idx_end >= idx_ini)
394  {
395  for (size_t i = idx_ini; i <= idx_end; i++) m_validRange[i] = false;
396  }
397  else
398  {
399  for (size_t i = 0; i < idx_end; i++) m_validRange[i] = false;
400 
401  for (size_t i = idx_ini; i < sizeRangeScan; i++)
402  m_validRange[i] = false;
403  }
404  }
405 
406  MRPT_END
407 }
408 
409 namespace mrpt
410 {
411 namespace 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 obs
427 } // namespace mrpt
428 
429 /*---------------------------------------------------------------
430  internal_buildAuxPointsMap
431  ---------------------------------------------------------------*/
432 void CObservation2DRangeScan::internal_buildAuxPointsMap(
433  const void* options) const
434 {
436  throw std::runtime_error(
437  "[CObservation2DRangeScan::buildAuxPointsMap] ERROR: This function "
438  "needs linking against mrpt-maps.\n");
439 
440  (*ptr_internal_build_points_map_from_scan2D)(*this, m_cachedMap, options);
441 }
442 
443 /** Fill out a T2DScanProperties structure with the parameters of this scan */
444 void CObservation2DRangeScan::getScanProperties(T2DScanProperties& p) const
445 {
446  p.nRays = this->scan.size();
447  p.aperture = this->aperture;
448  p.rightToLeft = this->rightToLeft;
449 }
450 
452  const T2DScanProperties& a, const T2DScanProperties& b)
453 {
454  if (a.nRays != b.nRays) return a.nRays < b.nRays;
455  if (a.aperture != b.aperture) return a.aperture < b.aperture;
456  if (a.rightToLeft != b.rightToLeft) return a.rightToLeft;
457  return false;
458 }
459 
460 void CObservation2DRangeScan::getDescriptionAsText(std::ostream& o) const
461 {
462  CObservation::getDescriptionAsText(o);
463  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
464  "base:\n";
465  o << sensorPose.getHomogeneousMatrixVal<CMatrixDouble44>() << sensorPose
466  << endl;
467 
468  o << format(
469  "Samples direction: %s\n",
470  (rightToLeft) ? "Right->Left" : "Left->Right");
471  o << format("Points in the scan: %u\n", (unsigned)scan.size());
472  o << format("Estimated sensor 'sigma': %f\n", stdError);
473  o << format(
474  "Increment in pitch during the scan: %f deg\n", RAD2DEG(deltaPitch));
475 
476  size_t i, inval = 0;
477  for (i = 0; i < scan.size(); i++)
478  if (!validRange[i]) inval++;
479  o << format("Invalid points in the scan: %u\n", (unsigned)inval);
480 
481  o << format("Sensor maximum range: %.02f m\n", maxRange);
482  o << format(
483  "Sensor field-of-view (\"aperture\"): %.01f deg\n", RAD2DEG(aperture));
484 
485  o << "Raw scan values: [";
486  for (i = 0; i < scan.size(); i++) o << format("%.03f ", scan[i]);
487  o << "]\n";
488 
489  o << "Raw valid-scan values: [";
490  for (i = 0; i < validRange.size(); i++)
491  o << format("%u ", validRange[i] ? 1 : 0);
492  o << "]\n\n";
493 
494  if (hasIntensity())
495  {
496  o << "Raw intensity values: [";
497  for (i = 0; i < m_intensity.size(); i++)
498  o << format("%d ", m_intensity[i]);
499  o << "]\n\n";
500  }
501 }
502 
503 float CObservation2DRangeScan::getScanRange(const size_t i) const
504 {
505  ASSERT_BELOW_(i, m_scan.size());
506  return m_scan[i];
507 }
508 
509 void CObservation2DRangeScan::setScanRange(const size_t i, const float val)
510 {
511  ASSERT_BELOW_(i, m_scan.size());
512  m_scan[i] = val;
513 }
514 
515 int CObservation2DRangeScan::getScanIntensity(const size_t i) const
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 }
#define MRPT_START
Definition: exceptions.h:262
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
scan2pts_functor ptr_internal_build_points_map_from_scan2D
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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:123
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:48
GLenum GLsizei len
Definition: glext.h:4712
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A numeric matrix of compile-time fixed size.
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:41
This namespace contains representation of robot actions and observations.
int val
Definition: mrpt_jpeglib.h:955
GLubyte GLubyte b
Definition: glext.h:6279
#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:46
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:48
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
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:266
GLuint in
Definition: glext.h:7274
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:3531
GLenum GLint x
Definition: glext.h:3538
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
static size_type size()
Definition: CPose3D.h:759



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019