MRPT  1.9.9
CAngularObservationMesh.h
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 #pragma once
10 
11 #include <mrpt/maps/CPointsMap.h>
12 #include <mrpt/math/CMatrixB.h>
18 
19 #include <mrpt/math/geometry.h>
20 
21 namespace mrpt::opengl
22 {
23 /**
24  * A mesh built from a set of 2D laser scan observations.
25  * Each element of this set is a single scan through the yaw, given a specific
26  * pitch.
27  * Each scan has a mrpt::poses::CPose3D identifying the origin of the scan,
28  * which ideally is the
29  * same for every one of them.
30  *
31  * <div align="center">
32  * <table border="0" cellspan="4" cellspacing="4" style="border-width: 1px;
33  * border-style: solid;">
34  * <tr> <td> mrpt::opengl::CAngularObservationMesh </td> <td> \image html
35  * preview_CAngularObservationMesh.png </td> </tr>
36  * </table>
37  * </div>
38  *
39  * \ingroup mrpt_maps_grp
40  */
42 {
44  public:
45  /**
46  * Range specification type, with several uses.
47  */
48  struct TDoubleRange
49  {
50  private:
51  /**
52  * Range type.
53  * If 0, it's specified by an initial and a final value, and an
54  * increment.
55  * If 1, it's specified by an initial and a final value, and a fixed
56  * size of samples.
57  * If 2, it's specified by an aperture, a fixed size of samples and a
58  * boolean variable controlling direction. This type is always
59  * zero-centered.
60  */
61  char rangeType;
62  /**
63  * Union type with the actual data.
64  * \sa rangeType
65  */
66  union rd {
67  struct
68  {
69  double initial;
70  double final;
71  double increment;
72  } mode0;
73  struct
74  {
75  double initial;
76  double final;
77  size_t amount;
78  } mode1;
79  struct
80  {
81  double aperture;
82  size_t amount;
83  bool negToPos;
84  } mode2;
85  } rangeData;
86 
87  public:
88  /**
89  * Constructor from initial value, final value and range.
90  */
91  TDoubleRange(double a, double b, double c) : rangeType(0)
92  {
94  rangeData.mode0.final = b;
96  }
97  /**
98  * Constructor from initial value, final value and amount of samples.
99  */
100  TDoubleRange(double a, double b, size_t c) : rangeType(1)
101  {
102  rangeData.mode1.initial = a;
103  rangeData.mode1.final = b;
104  rangeData.mode1.amount = c;
105  }
106  /**
107  * Constructor from aperture, amount of samples and scan direction.
108  */
109  TDoubleRange(double a, size_t b, bool c) : rangeType(2)
110  {
112  rangeData.mode2.amount = b;
114  }
115  /**
116  * Creates a range of values from the initial value, the final value
117  * and the increment.
118  * \throw std::logic_error if the increment is zero.
119  */
121  double initial, double final, double increment)
122  {
123  if (increment == 0)
124  throw std::logic_error("Invalid increment value.");
125  return TDoubleRange(initial, final, increment);
126  }
127  /**
128  * Creates a range of values from the initial value, the final value
129  * and a desired amount of samples.
130  */
132  double initial, double final, size_t amount)
133  {
134  return TDoubleRange(initial, final, amount);
135  }
136  /**
137  * Creates a zero-centered range of values from an aperture, an amount
138  * of samples and a direction.
139  */
141  double aperture, size_t amount, bool negToPos = true)
142  {
144  }
145  /**
146  * Returns the total aperture of the range.
147  * \throw std::logic_error on invalid range type.
148  */
149  inline double aperture() const
150  {
151  switch (rangeType)
152  {
153  case 0:
154  return (mrpt::sign(rangeData.mode0.increment) ==
155  mrpt::sign(
158  ? fabs(
161  : 0;
162  case 1:
164  case 2:
165  return rangeData.mode2.aperture;
166  default:
167  throw std::logic_error("Unknown range type.");
168  }
169  }
170  /**
171  * Returns the first value of the range.
172  * \throw std::logic_error on invalid range type.
173  */
174  inline double initialValue() const
175  {
176  switch (rangeType)
177  {
178  case 0:
179  case 1:
180  return rangeData.mode0.initial;
181  case 2:
182  return rangeData.mode2.negToPos
183  ? -rangeData.mode2.aperture / 2
184  : rangeData.mode2.aperture / 2;
185  default:
186  throw std::logic_error("Unknown range type.");
187  }
188  }
189  /**
190  * Returns the last value of the range.
191  * \throw std::logic_error on invalid range type.
192  */
193  inline double finalValue() const
194  {
195  switch (rangeType)
196  {
197  case 0:
198  return (mrpt::sign(rangeData.mode0.increment) ==
199  mrpt::sign(
204  case 1:
205  return rangeData.mode1.final;
206  case 2:
207  return rangeData.mode2.negToPos
208  ? rangeData.mode2.aperture / 2
209  : -rangeData.mode2.aperture / 2;
210  default:
211  throw std::logic_error("Unknown range type.");
212  }
213  }
214  /**
215  * Returns the increment between two consecutive values of the range.
216  * \throw std::logic_error on invalid range type.
217  */
218  inline double increment() const
219  {
220  switch (rangeType)
221  {
222  case 0:
223  return rangeData.mode0.increment;
224  case 1:
226  static_cast<double>(rangeData.mode1.amount - 1);
227  case 2:
228  return rangeData.mode2.negToPos
230  static_cast<double>(
231  rangeData.mode2.amount - 1)
233  static_cast<double>(
234  rangeData.mode2.amount - 1);
235  default:
236  throw std::logic_error("Unknown range type.");
237  }
238  }
239  /**
240  * Returns the total amount of values in this range.
241  * \throw std::logic_error on invalid range type.
242  */
243  inline size_t amount() const
244  {
245  switch (rangeType)
246  {
247  case 0:
248  return (mrpt::sign(rangeData.mode0.increment) ==
249  mrpt::sign(
252  ? 1 + static_cast<size_t>(ceil(
256  : 1;
257  case 1:
258  return rangeData.mode1.amount;
259  case 2:
260  return rangeData.mode2.amount;
261  default:
262  throw std::logic_error("Unknown range type.");
263  }
264  }
265  /**
266  * Gets a vector with every value in the range.
267  * \throw std::logic_error on invalid range type.
268  */
269  void values(std::vector<double>& vals) const;
270  /**
271  * Returns the direction of the scan. True if the increment is
272  * positive, false otherwise.
273  * \throw std::logic_error on invalid range type.
274  */
275  inline bool negToPos() const
276  {
277  switch (rangeType)
278  {
279  case 0:
280  return mrpt::sign(rangeData.mode0.increment) > 0;
281  case 1:
282  return mrpt::sign(
284  rangeData.mode1.initial) > 0;
285  case 2:
286  return rangeData.mode2.negToPos;
287  default:
288  throw std::logic_error("Unknown range type.");
289  }
290  }
291  };
292 
293  void getBoundingBox(
295  mrpt::math::TPoint3D& bb_max) const override;
296 
297  protected:
298  /** Updates the mesh, if needed. It's a const method, but modifies mutable
299  * content. */
300  void updateMesh() const;
301  /** Actual set of triangles to be displayed. */
302  mutable std::vector<CSetOfTriangles::TTriangle> triangles;
303  /** Internal method to add a triangle to the mutable mesh. */
304  void addTriangle(
305  const mrpt::math::TPoint3D& p1, const mrpt::math::TPoint3D& p2,
306  const mrpt::math::TPoint3D& p3) const;
307  /** Whether the mesh will be displayed wireframe or solid. */
308  bool mWireframe{true};
309  /** Mutable variable which controls if the object has suffered any change
310  * since last time the mesh was updated. */
311  mutable bool meshUpToDate{false};
312  /** Whether the object may present transparencies or not. */
314  /** Mutable object with the mesh's points. */
316  /** Scan validity matrix. */
318  /** Observation pitch range. When containing exactly two elements, they
319  * represent the bounds. */
320  std::vector<double> pitchBounds;
321  /** Actual scan set which is used to generate the mesh. */
322  std::vector<mrpt::obs::CObservation2DRangeScan> scanSet;
323 
324  public:
325  /**
326  * Basic constructor.
327  */
329  : actualMesh(0, 0), validityMatrix(0, 0), pitchBounds(), scanSet()
330  {
331  }
332  /** Empty destructor. */
333  ~CAngularObservationMesh() override = default;
334  /**
335  * Returns whether the object is configured as wireframe or solid.
336  */
337  inline bool isWireframe() const { return mWireframe; }
338  /**
339  * Sets the display mode for the object. True=wireframe, False=solid.
340  */
341  inline void setWireframe(bool enabled = true)
342  {
343  mWireframe = enabled;
345  }
346  /**
347  * Returns whether the object may be transparent or not.
348  */
349  inline bool isTransparencyEnabled() const { return mEnableTransparency; }
350  /**
351  * Enables or disables transparencies.
352  */
353  inline void enableTransparency(bool enabled = true)
354  {
355  mEnableTransparency = enabled;
357  }
358  /**
359  * Renderizes the object.
360  * \sa mrpt::opengl::CRenderizable
361  */
362  void render_dl() const override;
363  /**
364  * Traces a ray to the object, returning the distance to a given pose
365  * through its X axis.
366  * \sa mrpt::opengl::CRenderizable,trace2DSetOfRays,trace1DSetOfRays
367  */
368  bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override;
369  /**
370  * Sets the pitch bounds for this range.
371  */
372  void setPitchBounds(const double initial, const double final);
373  /**
374  * Sets the pitch bounds for this range.
375  */
376  void setPitchBounds(const std::vector<double>& bounds);
377  /**
378  * Gets the initial and final pitch bounds for this range.
379  */
380  void getPitchBounds(double& initial, double& final) const;
381  /**
382  * Gets the pitch bounds for this range.
383  */
384  void getPitchBounds(std::vector<double>& bounds) const;
385  /**
386  * Gets the scan set.
387  */
388  void getScanSet(
389  std::vector<mrpt::obs::CObservation2DRangeScan>& scans) const;
390  /**
391  * Sets the scan set.
392  */
393  bool setScanSet(
394  const std::vector<mrpt::obs::CObservation2DRangeScan>& scans);
395  /**
396  * Gets the mesh as a set of triangles, for displaying them.
397  * \sa generateSetOfTriangles(std::vector<TPolygon3D>
398  * &),mrpt::opengl::CSetOfTriangles,mrpt::opengl::CSetOfTriangles::TTriangle
399  */
401  /**
402  * Returns the scanned points as a 3D point cloud. The target pointmap must
403  * be passed as a pointer to allow the use of any derived class.
404  */
405  void generatePointCloud(mrpt::maps::CPointsMap* out_map) const;
406  /**
407  * Gets a set of lines containing the traced rays, for displaying them.
408  * \sa getUntracedRays,mrpt::opengl::CSetOfLines
409  */
410  void getTracedRays(CSetOfLines::Ptr& res) const;
411  /**
412  * Gets a set of lines containing the untraced rays, up to a specified
413  * distance, for displaying them.
414  * \sa getTracedRays,mrpt::opengl::CSetOfLines
415  */
416  void getUntracedRays(CSetOfLines::Ptr& res, double dist) const;
417  /**
418  * Gets the mesh as a set of polygons, to work with them.
419  * \sa generateSetOfTriangles(mrpt::opengl::CSetOfTriangles &)
420  */
421  void generateSetOfTriangles(std::vector<mrpt::math::TPolygon3D>& res) const;
422  /**
423  * Retrieves the full mesh, along with the validity matrix.
424  */
427  mrpt::math::CMatrixBool& validity) const
428  {
429  if (!meshUpToDate) updateMesh();
430  pts = actualMesh;
431  validity = validityMatrix;
432  }
433 
434  private:
435  /**
436  * Internal functor class to trace a ray.
437  */
438  template <class T>
439  class FTrace1D
440  {
441  protected:
443  const T& e;
444  std::vector<double>& values;
445  std::vector<char>& valid;
446 
447  public:
449  const T& s, const mrpt::poses::CPose3D& p, std::vector<double>& v,
450  std::vector<char>& v2)
451  : initial(p), e(s), values(v), valid(v2)
452  {
453  }
454  void operator()(double yaw)
455  {
456  double dist;
457  const mrpt::poses::CPose3D pNew =
458  initial + mrpt::poses::CPose3D(0.0, 0.0, 0.0, yaw, 0.0, 0.0);
459  if (e->traceRay(pNew, dist))
460  {
461  values.push_back(dist);
462  valid.push_back(1);
463  }
464  else
465  {
466  values.push_back(0);
467  valid.push_back(0);
468  }
469  }
470  };
471  /**
472  * Internal functor class to trace a set of rays.
473  */
474  template <class T>
475  class FTrace2D
476  {
477  protected:
478  const T& e;
482  std::vector<mrpt::obs::CObservation2DRangeScan>& vObs;
484 
485  public:
487  const T& s, const mrpt::poses::CPose3D& p,
490  std::vector<mrpt::obs::CObservation2DRangeScan>& obs,
491  const mrpt::poses::CPose3D& b)
492  : e(s), initial(p), caom(om), yaws(y), vObs(obs), pBase(b)
493  {
494  }
495  void operator()(double pitch)
496  {
497  std::vector<double> yValues;
498  yaws.values(yValues);
501  const mrpt::poses::CPose3D pNew =
502  initial + mrpt::poses::CPose3D(0, 0, 0, 0, pitch, 0);
503  std::vector<double> values;
504  std::vector<char> valid;
505  size_t nY = yValues.size();
506  values.reserve(nY);
507  valid.reserve(nY);
508  for_each(
509  yValues.begin(), yValues.end(),
510  FTrace1D<T>(e, pNew, values, valid));
511  o.aperture = yaws.aperture();
512  o.rightToLeft = yaws.negToPos();
513  o.maxRange = 10000;
514  o.sensorPose = pNew;
515  o.deltaPitch = 0;
516  o.resizeScan(values.size());
517  for (size_t i = 0; i < values.size(); i++)
518  {
519  o.setScanRange(i, values[i]);
520  o.setScanRangeValidity(i, valid[i] != 0);
521  }
522  vObs.push_back(o);
523  }
524  };
525 
526  public:
527  /**
528  * 2D ray tracing (will generate a 3D mesh). Given an object and two
529  * ranges, realizes a scan from the initial pose and stores it in a
530  * CAngularObservationMesh object.
531  * The objective may be a COpenGLScene, a CRenderizable or any children of
532  * its.
533  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
534  */
535  template <class T>
536  static void trace2DSetOfRays(
537  const T& e, const mrpt::poses::CPose3D& initial,
538  CAngularObservationMesh::Ptr& caom, const TDoubleRange& pitchs,
539  const TDoubleRange& yaws);
540  /**
541  * 2D ray tracing (will generate a vectorial mesh inside a plane). Given an
542  * object and a range, realizes a scan from the initial pose and stores it
543  * in a CObservation2DRangeScan object.
544  * The objective may be a COpenGLScene, a CRenderizable or any children of
545  * its.
546  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
547  */
548  template <class T>
549  static void trace1DSetOfRays(
550  const T& e, const mrpt::poses::CPose3D& initial,
552  {
553  std::vector<double> yValues;
554  yaws.values(yValues);
557  size_t nV = yaws.amount();
558  scanValues.reserve(nV);
559  valid.reserve(nV);
560  for_each(
561  yValues.begin(), yValues.end(),
562  FTrace1D<T>(e, initial, scanValues, valid));
563  obs.aperture = yaws.aperture();
564  obs.rightToLeft = yaws.negToPos();
565  obs.maxRange = 10000;
566  obs.sensorPose = initial;
567  obs.deltaPitch = 0;
568  for (size_t i = 0; i < nV; i++)
569  {
570  obs.setScanRange(i, scanValues[i]);
571  obs.setScanRangeValidity(i, valid[i]);
572  }
573  }
574 };
575 
576 template <class T>
578  const T& e, const mrpt::poses::CPose3D& initial,
579  CAngularObservationMesh::Ptr& caom, const TDoubleRange& pitchs,
580  const TDoubleRange& yaws)
581 {
582  std::vector<double> pValues;
583  pitchs.values(pValues);
584  std::vector<mrpt::obs::CObservation2DRangeScan> vObs;
585  vObs.reserve(pValues.size());
586  for_each(
587  pValues.begin(), pValues.end(),
588  FTrace2D<T>(e, initial, caom, yaws, vObs, initial));
589  caom->mWireframe = false;
590  caom->mEnableTransparency = false;
591  caom->setPitchBounds(pValues);
592  caom->setScanSet(vObs);
593 }
594 } // namespace mrpt::opengl
void getUntracedRays(CSetOfLines::Ptr &res, double dist) const
Gets a set of lines containing the untraced rays, up to a specified distance, for displaying them...
bool meshUpToDate
Mutable variable which controls if the object has suffered any change since last time the mesh was up...
bool negToPos() const
Returns the direction of the scan.
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
A mesh built from a set of 2D laser scan observations.
Internal functor class to trace a set of rays.
Internal functor class to trace a ray.
FTrace2D(const T &s, const mrpt::poses::CPose3D &p, CAngularObservationMesh::Ptr &om, const CAngularObservationMesh::TDoubleRange &y, std::vector< mrpt::obs::CObservation2DRangeScan > &obs, const mrpt::poses::CPose3D &b)
TDoubleRange(double a, size_t b, bool c)
Constructor from aperture, amount of samples and scan direction.
void setScanRange(const size_t i, const float val)
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Traces a ray to the object, returning the distance to a given pose through its X axis.
struct mrpt::opengl::CAngularObservationMesh::TDoubleRange::rd::@11 mode1
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
std::vector< mrpt::obs::CObservation2DRangeScan > & vObs
bool mEnableTransparency
Whether the object may present transparencies or not.
void setWireframe(bool enabled=true)
Sets the display mode for the object.
void getTracedRays(CSetOfLines::Ptr &res) const
Gets a set of lines containing the traced rays, for displaying them.
bool isTransparencyEnabled() const
Returns whether the object may be transparent or not.
void getScanSet(std::vector< mrpt::obs::CObservation2DRangeScan > &scans) const
Gets the scan set.
void render_dl() const override
Renderizes the object.
void values(std::vector< double > &vals) const
Gets a vector with every value in the range.
float maxRange
The maximum range allowed by the device, in meters (e.g.
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
double finalValue() const
Returns the last value of the range.
void getPitchBounds(double &initial, double &final) const
Gets the initial and final pitch bounds for this range.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
mrpt::math::CMatrixB validityMatrix
Scan validity matrix.
void generateSetOfTriangles(CSetOfTriangles::Ptr &res) const
Gets the mesh as a set of triangles, for displaying them.
void getActualMesh(mrpt::math::CMatrixDynamic< mrpt::math::TPoint3D_data > &pts, mrpt::math::CMatrixBool &validity) const
Retrieves the full mesh, along with the validity matrix.
double initialValue() const
Returns the first value of the range.
size_t amount() const
Returns the total amount of values in this range.
FTrace1D(const T &s, const mrpt::poses::CPose3D &p, std::vector< double > &v, std::vector< char > &v2)
int sign(T x)
Returns the sign of X as "1" or "-1".
struct mrpt::opengl::CAngularObservationMesh::TDoubleRange::rd::@10 mode0
double increment() const
Returns the increment between two consecutive values of the range.
static TDoubleRange CreateFromIncrement(double initial, double final, double increment)
Creates a range of values from the initial value, the final value and the increment.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
static void trace1DSetOfRays(const T &e, const mrpt::poses::CPose3D &initial, mrpt::obs::CObservation2DRangeScan &obs, const TDoubleRange &yaws)
2D ray tracing (will generate a vectorial mesh inside a plane).
~CAngularObservationMesh() override=default
Empty destructor.
static TDoubleRange CreateFromAmount(double initial, double final, size_t amount)
Creates a range of values from the initial value, the final value and a desired amount of samples...
const CAngularObservationMesh::TDoubleRange & yaws
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
bool mWireframe
Whether the mesh will be displayed wireframe or solid.
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
void generatePointCloud(mrpt::maps::CPointsMap *out_map) const
Returns the scanned points as a 3D point cloud.
static TDoubleRange CreateFromAperture(double aperture, size_t amount, bool negToPos=true)
Creates a zero-centered range of values from an aperture, an amount of samples and a direction...
void addTriangle(const mrpt::math::TPoint3D &p1, const mrpt::math::TPoint3D &p2, const mrpt::math::TPoint3D &p3) const
Internal method to add a triangle to the mutable mesh.
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
union mrpt::opengl::CAngularObservationMesh::TDoubleRange::rd rangeData
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
double aperture() const
Returns the total aperture of the range.
const auto bb_max
std::vector< mrpt::obs::CObservation2DRangeScan > scanSet
Actual scan set which is used to generate the mesh.
Range specification type, with several uses.
TDoubleRange(double a, double b, size_t c)
Constructor from initial value, final value and amount of samples.
This class is a "CSerializable" wrapper for "CMatrixBool".
Definition: CMatrixB.h:21
TDoubleRange(double a, double b, double c)
Constructor from initial value, final value and range.
mrpt::math::CMatrixDynamic< mrpt::math::TPoint3D_data > actualMesh
Mutable object with the mesh&#39;s points.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void updateMesh() const
Updates the mesh, if needed.
bool setScanSet(const std::vector< mrpt::obs::CObservation2DRangeScan > &scans)
Sets the scan set.
const auto bb_min
struct mrpt::opengl::CAngularObservationMesh::TDoubleRange::rd::@12 mode2
Lightweight 3D point.
Definition: TPoint3D.h:90
void enableTransparency(bool enabled=true)
Enables or disables transparencies.
void setPitchBounds(const double initial, const double final)
Sets the pitch bounds for this range.
static void trace2DSetOfRays(const T &e, const mrpt::poses::CPose3D &initial, CAngularObservationMesh::Ptr &caom, const TDoubleRange &pitchs, const TDoubleRange &yaws)
2D ray tracing (will generate a 3D mesh).
std::vector< CSetOfTriangles::TTriangle > triangles
Actual set of triangles to be displayed.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
std::vector< double > pitchBounds
Observation pitch range.
bool isWireframe() const
Returns whether the object is configured as wireframe or solid.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019