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. */
317  /** Scan validity matrix. */
319  /** Observation pitch range. When containing exactly two elements, they
320  * represent the bounds. */
321  std::vector<double> pitchBounds;
322  /** Actual scan set which is used to generate the mesh. */
323  std::vector<mrpt::obs::CObservation2DRangeScan> scanSet;
324 
325  public:
326  /**
327  * Basic constructor.
328  */
330  : actualMesh(0, 0), validityMatrix(0, 0), pitchBounds(), scanSet()
331  {
332  }
333  /** Empty destructor. */
334  ~CAngularObservationMesh() override = default;
335  /**
336  * Returns whether the object is configured as wireframe or solid.
337  */
338  inline bool isWireframe() const { return mWireframe; }
339  /**
340  * Sets the display mode for the object. True=wireframe, False=solid.
341  */
342  inline void setWireframe(bool enabled = true)
343  {
344  mWireframe = enabled;
346  }
347  /**
348  * Returns whether the object may be transparent or not.
349  */
350  inline bool isTransparencyEnabled() const { return mEnableTransparency; }
351  /**
352  * Enables or disables transparencies.
353  */
354  inline void enableTransparency(bool enabled = true)
355  {
356  mEnableTransparency = enabled;
358  }
359  /**
360  * Renderizes the object.
361  * \sa mrpt::opengl::CRenderizable
362  */
363  void render_dl() const override;
364  /**
365  * Traces a ray to the object, returning the distance to a given pose
366  * through its X axis.
367  * \sa mrpt::opengl::CRenderizable,trace2DSetOfRays,trace1DSetOfRays
368  */
369  bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override;
370  /**
371  * Sets the pitch bounds for this range.
372  */
373  void setPitchBounds(const double initial, const double final);
374  /**
375  * Sets the pitch bounds for this range.
376  */
377  void setPitchBounds(const std::vector<double>& bounds);
378  /**
379  * Gets the initial and final pitch bounds for this range.
380  */
381  void getPitchBounds(double& initial, double& final) const;
382  /**
383  * Gets the pitch bounds for this range.
384  */
385  void getPitchBounds(std::vector<double>& bounds) const;
386  /**
387  * Gets the scan set.
388  */
389  void getScanSet(
390  std::vector<mrpt::obs::CObservation2DRangeScan>& scans) const;
391  /**
392  * Sets the scan set.
393  */
394  bool setScanSet(
395  const std::vector<mrpt::obs::CObservation2DRangeScan>& scans);
396  /**
397  * Gets the mesh as a set of triangles, for displaying them.
398  * \sa generateSetOfTriangles(std::vector<TPolygon3D>
399  * &),mrpt::opengl::CSetOfTriangles,mrpt::opengl::CSetOfTriangles::TTriangle
400  */
402  /**
403  * Returns the scanned points as a 3D point cloud. The target pointmap must
404  * be passed as a pointer to allow the use of any derived class.
405  */
406  void generatePointCloud(mrpt::maps::CPointsMap* out_map) const;
407  /**
408  * Gets a set of lines containing the traced rays, for displaying them.
409  * \sa getUntracedRays,mrpt::opengl::CSetOfLines
410  */
411  void getTracedRays(CSetOfLines::Ptr& res) const;
412  /**
413  * Gets a set of lines containing the untraced rays, up to a specified
414  * distance, for displaying them.
415  * \sa getTracedRays,mrpt::opengl::CSetOfLines
416  */
417  void getUntracedRays(CSetOfLines::Ptr& res, double dist) const;
418  /**
419  * Gets the mesh as a set of polygons, to work with them.
420  * \sa generateSetOfTriangles(mrpt::opengl::CSetOfTriangles &)
421  */
422  void generateSetOfTriangles(std::vector<mrpt::math::TPolygon3D>& res) const;
423  /**
424  * Retrieves the full mesh, along with the validity matrix.
425  */
428  mrpt::math::CMatrixBool& validity) const
429  {
430  if (!meshUpToDate) updateMesh();
431  pts = actualMesh;
432  validity = validityMatrix;
433  }
434 
435  private:
436  /**
437  * Internal functor class to trace a ray.
438  */
439  template <class T>
440  class FTrace1D
441  {
442  protected:
444  const T& e;
445  std::vector<double>& values;
446  std::vector<char>& valid;
447 
448  public:
450  const T& s, const mrpt::poses::CPose3D& p, std::vector<double>& v,
451  std::vector<char>& v2)
452  : initial(p), e(s), values(v), valid(v2)
453  {
454  }
455  void operator()(double yaw)
456  {
457  double dist;
458  const mrpt::poses::CPose3D pNew =
459  initial + mrpt::poses::CPose3D(0.0, 0.0, 0.0, yaw, 0.0, 0.0);
460  if (e->traceRay(pNew, dist))
461  {
462  values.push_back(dist);
463  valid.push_back(1);
464  }
465  else
466  {
467  values.push_back(0);
468  valid.push_back(0);
469  }
470  }
471  };
472  /**
473  * Internal functor class to trace a set of rays.
474  */
475  template <class T>
476  class FTrace2D
477  {
478  protected:
479  const T& e;
483  std::vector<mrpt::obs::CObservation2DRangeScan>& vObs;
485 
486  public:
488  const T& s, const mrpt::poses::CPose3D& p,
491  std::vector<mrpt::obs::CObservation2DRangeScan>& obs,
492  const mrpt::poses::CPose3D& b)
493  : e(s), initial(p), caom(om), yaws(y), vObs(obs), pBase(b)
494  {
495  }
496  void operator()(double pitch)
497  {
498  std::vector<double> yValues;
499  yaws.values(yValues);
502  const mrpt::poses::CPose3D pNew =
503  initial + mrpt::poses::CPose3D(0, 0, 0, 0, pitch, 0);
504  std::vector<double> values;
505  std::vector<char> valid;
506  size_t nY = yValues.size();
507  values.reserve(nY);
508  valid.reserve(nY);
509  for_each(
510  yValues.begin(), yValues.end(),
511  FTrace1D<T>(e, pNew, values, valid));
512  o.aperture = yaws.aperture();
513  o.rightToLeft = yaws.negToPos();
514  o.maxRange = 10000;
515  o.sensorPose = pNew;
516  o.deltaPitch = 0;
517  o.resizeScan(values.size());
518  for (size_t i = 0; i < values.size(); i++)
519  {
520  o.setScanRange(i, values[i]);
521  o.setScanRangeValidity(i, valid[i] != 0);
522  }
523  vObs.push_back(o);
524  }
525  };
526 
527  public:
528  /**
529  * 2D ray tracing (will generate a 3D mesh). Given an object and two
530  * ranges, realizes a scan from the initial pose and stores it in a
531  * CAngularObservationMesh object.
532  * The objective may be a COpenGLScene, a CRenderizable or any children of
533  * its.
534  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
535  */
536  template <class T>
537  static void trace2DSetOfRays(
538  const T& e, const mrpt::poses::CPose3D& initial,
539  CAngularObservationMesh::Ptr& caom, const TDoubleRange& pitchs,
540  const TDoubleRange& yaws);
541  /**
542  * 2D ray tracing (will generate a vectorial mesh inside a plane). Given an
543  * object and a range, realizes a scan from the initial pose and stores it
544  * in a CObservation2DRangeScan object.
545  * The objective may be a COpenGLScene, a CRenderizable or any children of
546  * its.
547  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
548  */
549  template <class T>
550  static void trace1DSetOfRays(
551  const T& e, const mrpt::poses::CPose3D& initial,
553  {
554  std::vector<double> yValues;
555  yaws.values(yValues);
558  size_t nV = yaws.amount();
559  scanValues.reserve(nV);
560  valid.reserve(nV);
561  for_each(
562  yValues.begin(), yValues.end(),
563  FTrace1D<T>(e, initial, scanValues, valid));
564  obs.aperture = yaws.aperture();
565  obs.rightToLeft = yaws.negToPos();
566  obs.maxRange = 10000;
567  obs.sensorPose = initial;
568  obs.deltaPitch = 0;
569  for (size_t i = 0; i < nV; i++)
570  {
571  obs.setScanRange(i, scanValues[i]);
572  obs.setScanRangeValidity(i, valid[i]);
573  }
574  }
575 };
576 
577 template <class T>
579  const T& e, const mrpt::poses::CPose3D& initial,
580  CAngularObservationMesh::Ptr& caom, const TDoubleRange& pitchs,
581  const TDoubleRange& yaws)
582 {
583  std::vector<double> pValues;
584  pitchs.values(pValues);
585  std::vector<mrpt::obs::CObservation2DRangeScan> vObs;
586  vObs.reserve(pValues.size());
587  for_each(
588  pValues.begin(), pValues.end(),
589  FTrace2D<T>(e, initial, caom, yaws, vObs, initial));
590  caom->mWireframe = false;
591  caom->mEnableTransparency = false;
592  caom->setPitchBounds(pValues);
593  caom->setScanSet(vObs);
594 }
595 } // 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.
Trivially copiable underlying data for TPoint3D.
Definition: TPoint3D.h:20
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.
mrpt::math::CMatrixDynamic< mrpt::math::TPoint3D_data< double > > actualMesh
Mutable object with the mesh&#39;s points.
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.
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
void getActualMesh(mrpt::math::CMatrixDynamic< mrpt::math::TPoint3D_data< double >> &pts, mrpt::math::CMatrixBool &validity) const
Retrieves the full mesh, along with the validity matrix.
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.
#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
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).
This template class provides the basic functionality for a general 2D any-size, resizable container o...
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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020