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::obs
410 {
411 // Tricky way to call to a library that depends on us, a sort of "run-time"
412 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
413 // "mrpt-obs", set by "mrpt-maps" at its startup.
414 using scan2pts_functor = void (*)(
416  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
417 
419 
421 {
423 }
424 }
425 
426 /*---------------------------------------------------------------
427  internal_buildAuxPointsMap
428  ---------------------------------------------------------------*/
429 void CObservation2DRangeScan::internal_buildAuxPointsMap(
430  const void* options) const
431 {
433  throw std::runtime_error(
434  "[CObservation2DRangeScan::buildAuxPointsMap] ERROR: This function "
435  "needs linking against mrpt-maps.\n");
436 
437  (*ptr_internal_build_points_map_from_scan2D)(*this, m_cachedMap, options);
438 }
439 
440 /** Fill out a T2DScanProperties structure with the parameters of this scan */
441 void CObservation2DRangeScan::getScanProperties(T2DScanProperties& p) const
442 {
443  p.nRays = this->scan.size();
444  p.aperture = this->aperture;
445  p.rightToLeft = this->rightToLeft;
446 }
447 
449  const T2DScanProperties& a, const T2DScanProperties& b)
450 {
451  if (a.nRays != b.nRays) return a.nRays < b.nRays;
452  if (a.aperture != b.aperture) return a.aperture < b.aperture;
453  if (a.rightToLeft != b.rightToLeft) return a.rightToLeft;
454  return false;
455 }
456 
457 void CObservation2DRangeScan::getDescriptionAsText(std::ostream& o) const
458 {
459  CObservation::getDescriptionAsText(o);
460  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
461  "base:\n";
462  o << sensorPose.getHomogeneousMatrixVal<CMatrixDouble44>() << sensorPose
463  << endl;
464 
465  o << format(
466  "Samples direction: %s\n",
467  (rightToLeft) ? "Right->Left" : "Left->Right");
468  o << format("Points in the scan: %u\n", (unsigned)scan.size());
469  o << format("Estimated sensor 'sigma': %f\n", stdError);
470  o << format(
471  "Increment in pitch during the scan: %f deg\n", RAD2DEG(deltaPitch));
472 
473  size_t i, inval = 0;
474  for (i = 0; i < scan.size(); i++)
475  if (!validRange[i]) inval++;
476  o << format("Invalid points in the scan: %u\n", (unsigned)inval);
477 
478  o << format("Sensor maximum range: %.02f m\n", maxRange);
479  o << format(
480  "Sensor field-of-view (\"aperture\"): %.01f deg\n", RAD2DEG(aperture));
481 
482  o << "Raw scan values: [";
483  for (i = 0; i < scan.size(); i++) o << format("%.03f ", scan[i]);
484  o << "]\n";
485 
486  o << "Raw valid-scan values: [";
487  for (i = 0; i < validRange.size(); i++)
488  o << format("%u ", validRange[i] ? 1 : 0);
489  o << "]\n\n";
490 
491  if (hasIntensity())
492  {
493  o << "Raw intensity values: [";
494  for (i = 0; i < m_intensity.size(); i++)
495  o << format("%d ", m_intensity[i]);
496  o << "]\n\n";
497  }
498 }
499 
500 float CObservation2DRangeScan::getScanRange(const size_t i) const
501 {
502  ASSERT_BELOW_(i, m_scan.size());
503  return m_scan[i];
504 }
505 
506 void CObservation2DRangeScan::setScanRange(const size_t i, const float val)
507 {
508  ASSERT_BELOW_(i, m_scan.size());
509  m_scan[i] = val;
510 }
511 
512 int CObservation2DRangeScan::getScanIntensity(const size_t i) const
513 {
514  ASSERT_BELOW_(i, m_intensity.size());
515  return m_intensity[i];
516 }
517 void CObservation2DRangeScan::setScanIntensity(const size_t i, const int val)
518 {
519  ASSERT_BELOW_(i, m_intensity.size());
520  m_intensity[i] = val;
521 }
522 
523 bool CObservation2DRangeScan::getScanRangeValidity(const size_t i) const
524 {
525  ASSERT_BELOW_(i, m_validRange.size());
526  return m_validRange[i] != 0;
527 }
528 void CObservation2DRangeScan::setScanRangeValidity(
529  const size_t i, const bool val)
530 {
531  ASSERT_BELOW_(i, m_validRange.size());
532  m_validRange[i] = val ? 1 : 0;
533 }
534 
535 void CObservation2DRangeScan::resizeScan(const size_t len)
536 {
537  m_scan.resize(len);
538  m_intensity.resize(len);
539  m_validRange.resize(len);
540 }
541 
542 void CObservation2DRangeScan::resizeScanAndAssign(
543  const size_t len, const float rangeVal, const bool rangeValidity,
544  const int32_t rangeIntensity)
545 {
546  m_scan.assign(len, rangeVal);
547  m_validRange.assign(len, rangeValidity);
548  m_intensity.assign(len, rangeIntensity);
549 }
550 
551 size_t CObservation2DRangeScan::getScanSize() const { return m_scan.size(); }
552 void CObservation2DRangeScan::loadFromVectors(
553  size_t nRays, const float* scanRanges, const char* scanValidity)
554 {
555  ASSERT_(scanRanges);
556  ASSERT_(scanValidity);
557  resizeScan(nRays);
558  for (size_t i = 0; i < nRays; i++)
559  {
560  m_scan[i] = scanRanges[i];
561  m_validRange[i] = scanValidity[i];
562  }
563 }
#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.
static constexpr size_type size()
Definition: CPose3D.h:757
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:127
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:39
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
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:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
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:22
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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020