Main MRPT website > C++ reference for MRPT 1.5.7
CAngularObservationMesh.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, 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 #ifndef opengl_CAngularObservationMesh_H
10 #define opengl_CAngularObservationMesh_H
11 
15 #include <mrpt/math/CMatrixB.h>
17 #include <mrpt/maps/CPointsMap.h>
19 
20 #include <mrpt/math/geometry.h>
21 
22 namespace mrpt {
23 namespace opengl {
25 
26  /**
27  * A mesh built from a set of 2D laser scan observations.
28  * Each element of this set is a single scan through the yaw, given a specific pitch.
29  * Each scan has a mrpt::poses::CPose3D identifying the origin of the scan, which ideally is the
30  * same for every one of them.
31  *
32  * <div align="center">
33  * <table border="0" cellspan="4" cellspacing="4" style="border-width: 1px; border-style: solid;">
34  * <tr> <td> mrpt::opengl::CAngularObservationMesh </td> <td> \image html preview_CAngularObservationMesh.png </td> </tr>
35  * </table>
36  * </div>
37  *
38  * \ingroup mrpt_maps_grp
39  */
42  public:
43  /**
44  * Range specification type, with several uses.
45  */
47  private:
48  /**
49  * Range type.
50  * If 0, it's specified by an initial and a final value, and an increment.
51  * If 1, it's specified by an initial and a final value, and a fixed size of samples.
52  * If 2, it's specified by an aperture, a fixed size of samples and a boolean variable controlling direction. This type is always zero-centered.
53  */
54  char rangeType;
55  /**
56  * Union type with the actual data.
57  * \sa rangeType
58  */
59  union rd {
60  struct {
61  double initial;
62  double final;
63  double increment;
64  } mode0;
65  struct {
66  double initial;
67  double final;
68  size_t amount;
69  } mode1;
70  struct {
71  double aperture;
72  size_t amount;
73  bool negToPos;
74  } mode2;
75  } rangeData;
76  /**
77  * Constructor from initial value, final value and range.
78  */
79  TDoubleRange(double a,double b,double c):rangeType(0) {
80  rangeData.mode0.initial=a;
81  rangeData.mode0.final=b;
82  rangeData.mode0.increment=c;
83  }
84  /**
85  * Constructor from initial value, final value and amount of samples.
86  */
87  TDoubleRange(double a,double b,size_t c):rangeType(1) {
88  rangeData.mode1.initial=a;
89  rangeData.mode1.final=b;
90  rangeData.mode1.amount=c;
91  }
92  /**
93  * Constructor from aperture, amount of samples and scan direction.
94  */
95  TDoubleRange(double a,size_t b,bool c):rangeType(2) {
96  rangeData.mode2.aperture=a;
97  rangeData.mode2.amount=b;
98  rangeData.mode2.negToPos=c;
99  }
100  public:
101  /**
102  * Creates a range of values from the initial value, the final value and the increment.
103  * \throw std::logic_error if the increment is zero.
104  */
105  inline static TDoubleRange CreateFromIncrement(double initial,double final,double increment) {
106  if (increment==0) throw std::logic_error("Invalid increment value.");
107  return TDoubleRange(initial,final,increment);
108  }
109  /**
110  * Creates a range of values from the initial value, the final value and a desired amount of samples.
111  */
112  inline static TDoubleRange CreateFromAmount(double initial,double final,size_t amount) {
113  return TDoubleRange(initial,final,amount);
114  }
115  /**
116  * Creates a zero-centered range of values from an aperture, an amount of samples and a direction.
117  */
118  inline static TDoubleRange CreateFromAperture(double aperture,size_t amount,bool negToPos=true) {
119  return TDoubleRange(aperture,amount,negToPos);
120  }
121  /**
122  * Returns the total aperture of the range.
123  * \throw std::logic_error on invalid range type.
124  */
125  inline double aperture() const {
126  switch (rangeType) {
127  case 0:return (mrpt::utils::sign(rangeData.mode0.increment)==mrpt::utils::sign(rangeData.mode0.final-rangeData.mode0.initial))?fabs(rangeData.mode0.final-rangeData.mode0.initial):0;
128  case 1:return rangeData.mode1.final-rangeData.mode1.initial;
129  case 2:return rangeData.mode2.aperture;
130  default:throw std::logic_error("Unknown range type.");
131  }
132  }
133  /**
134  * Returns the first value of the range.
135  * \throw std::logic_error on invalid range type.
136  */
137  inline double initialValue() const {
138  switch (rangeType) {
139  case 0:
140  case 1:return rangeData.mode0.initial;
141  case 2:return rangeData.mode2.negToPos?-rangeData.mode2.aperture/2:rangeData.mode2.aperture/2;
142  default:throw std::logic_error("Unknown range type.");
143  }
144  }
145  /**
146  * Returns the last value of the range.
147  * \throw std::logic_error on invalid range type.
148  */
149  inline double finalValue() const {
150  switch (rangeType) {
151  case 0:return (mrpt::utils::sign(rangeData.mode0.increment)==mrpt::utils::sign(rangeData.mode0.final-rangeData.mode0.initial))?rangeData.mode0.final:rangeData.mode0.initial;
152  case 1:return rangeData.mode1.final;
153  case 2:return rangeData.mode2.negToPos?rangeData.mode2.aperture/2:-rangeData.mode2.aperture/2;
154  default:throw std::logic_error("Unknown range type.");
155  }
156  }
157  /**
158  * Returns the increment between two consecutive values of the range.
159  * \throw std::logic_error on invalid range type.
160  */
161  inline double increment() const {
162  switch (rangeType) {
163  case 0:return rangeData.mode0.increment;
164  case 1:return (rangeData.mode1.final-rangeData.mode1.initial)/static_cast<double>(rangeData.mode1.amount-1);
165  case 2:return rangeData.mode2.negToPos?rangeData.mode2.aperture/static_cast<double>(rangeData.mode2.amount-1):-rangeData.mode2.aperture/static_cast<double>(rangeData.mode2.amount-1);
166  default:throw std::logic_error("Unknown range type.");
167  }
168  }
169  /**
170  * Returns the total amount of values in this range.
171  * \throw std::logic_error on invalid range type.
172  */
173  inline size_t amount() const {
174  switch (rangeType) {
175  case 0:return (mrpt::utils::sign(rangeData.mode0.increment)==mrpt::utils::sign(rangeData.mode0.final-rangeData.mode0.initial))?1+static_cast<size_t>(ceil((rangeData.mode0.final-rangeData.mode0.initial)/rangeData.mode0.increment)):1;
176  case 1:return rangeData.mode1.amount;
177  case 2:return rangeData.mode2.amount;
178  default:throw std::logic_error("Unknown range type.");
179  }
180  }
181  /**
182  * Gets a vector with every value in the range.
183  * \throw std::logic_error on invalid range type.
184  */
185  void values(std::vector<double> &vals) const;
186  /**
187  * Returns the direction of the scan. True if the increment is positive, false otherwise.
188  * \throw std::logic_error on invalid range type.
189  */
190  inline bool negToPos() const {
191  switch (rangeType) {
192  case 0:return mrpt::utils::sign(rangeData.mode0.increment)>0;
193  case 1:return mrpt::utils::sign(rangeData.mode1.final-rangeData.mode1.initial)>0;
194  case 2:return rangeData.mode2.negToPos;
195  default:throw std::logic_error("Unknown range type.");
196  }
197  }
198  };
199 
200  void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const MRPT_OVERRIDE;
201 
202  protected:
203  void updateMesh() const; //!< Updates the mesh, if needed. It's a const method, but modifies mutable content.
204  virtual ~CAngularObservationMesh() {} //!< Empty destructor.
205  mutable std::vector<CSetOfTriangles::TTriangle> triangles; //!< Actual set of triangles to be displayed.
206  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.
207  bool mWireframe; //!< Whether the mesh will be displayed wireframe or solid.
208  mutable bool meshUpToDate; //!< Mutable variable which controls if the object has suffered any change since last time the mesh was updated.
209  bool mEnableTransparency; //!< Whether the object may present transparencies or not.
210  mutable mrpt::math::CMatrixTemplate<mrpt::math::TPoint3D> actualMesh; //!< Mutable object with the mesh's points.
211  mutable mrpt::math::CMatrixB validityMatrix; //!< Scan validity matrix.
212  std::vector<double> pitchBounds; //!< Observation pitch range. When containing exactly two elements, they represent the bounds.
213  std::vector<mrpt::obs::CObservation2DRangeScan> scanSet; //!< Actual scan set which is used to generate the mesh.
214  /**
215  * Basic constructor.
216  */
217  CAngularObservationMesh():mWireframe(true),meshUpToDate(false),mEnableTransparency(true),actualMesh(0,0),validityMatrix(0,0),pitchBounds(),scanSet() {}
218  public:
219  /**
220  * Returns whether the object is configured as wireframe or solid.
221  */
222  inline bool isWireframe() const {
223  return mWireframe;
224  }
225  /**
226  * Sets the display mode for the object. True=wireframe, False=solid.
227  */
228  inline void setWireframe(bool enabled=true) {
229  mWireframe=enabled;
231  }
232  /**
233  * Returns whether the object may be transparent or not.
234  */
235  inline bool isTransparencyEnabled() const {
236  return mEnableTransparency;
237  }
238  /**
239  * Enables or disables transparencies.
240  */
241  inline void enableTransparency(bool enabled=true) {
242  mEnableTransparency=enabled;
244  }
245  /**
246  * Renderizes the object.
247  * \sa mrpt::opengl::CRenderizable
248  */
249  virtual void render_dl() const MRPT_OVERRIDE;
250  /**
251  * Traces a ray to the object, returning the distance to a given pose through its X axis.
252  * \sa mrpt::opengl::CRenderizable,trace2DSetOfRays,trace1DSetOfRays
253  */
254  bool traceRay(const mrpt::poses::CPose3D &o,double &dist) const MRPT_OVERRIDE;
255  /**
256  * Sets the pitch bounds for this range.
257  */
258  void setPitchBounds(const double initial,const double final);
259  /**
260  * Sets the pitch bounds for this range.
261  */
262  void setPitchBounds(const std::vector<double> &bounds);
263  /**
264  * Gets the initial and final pitch bounds for this range.
265  */
266  void getPitchBounds(double &initial,double &final) const;
267  /**
268  * Gets the pitch bounds for this range.
269  */
270  void getPitchBounds(std::vector<double> &bounds) const;
271  /**
272  * Gets the scan set.
273  */
274  void getScanSet(std::vector<mrpt::obs::CObservation2DRangeScan> &scans) const;
275  /**
276  * Sets the scan set.
277  */
278  bool setScanSet(const std::vector<mrpt::obs::CObservation2DRangeScan> &scans);
279  /**
280  * Gets the mesh as a set of triangles, for displaying them.
281  * \sa generateSetOfTriangles(std::vector<TPolygon3D> &),mrpt::opengl::CSetOfTriangles,mrpt::opengl::CSetOfTriangles::TTriangle
282  */
283  void generateSetOfTriangles(CSetOfTrianglesPtr &res) const;
284  /**
285  * Returns the scanned points as a 3D point cloud. The target pointmap must be passed as a pointer to allow the use of any derived class.
286  */
287  void generatePointCloud(mrpt::maps::CPointsMap *out_map) const;
288  /**
289  * Gets a set of lines containing the traced rays, for displaying them.
290  * \sa getUntracedRays,mrpt::opengl::CSetOfLines
291  */
292  void getTracedRays(CSetOfLinesPtr &res) const;
293  /**
294  * Gets a set of lines containing the untraced rays, up to a specified distance, for displaying them.
295  * \sa getTracedRays,mrpt::opengl::CSetOfLines
296  */
297  void getUntracedRays(CSetOfLinesPtr &res,double dist) const;
298  /**
299  * Gets the mesh as a set of polygons, to work with them.
300  * \sa generateSetOfTriangles(mrpt::opengl::CSetOfTriangles &)
301  */
302  void generateSetOfTriangles(std::vector<mrpt::math::TPolygon3D> &res) const;
303  /**
304  * Retrieves the full mesh, along with the validity matrix.
305  */
306  void getActualMesh(mrpt::math::CMatrixTemplate<mrpt::math::TPoint3D> &pts,mrpt::math::CMatrixBool &validity) const {
307  if (!meshUpToDate) updateMesh();
308  pts=actualMesh;
309  validity=validityMatrix;
310  }
311  private:
312  /**
313  * Internal functor class to trace a ray.
314  */
315  template<class T> class FTrace1D {
316  protected:
318  const T &e;
319  std::vector<double> &values;
320  std::vector<char> &valid;
321  public:
322  FTrace1D(const T &s,const mrpt::poses::CPose3D &p,std::vector<double> &v,std::vector<char> &v2):initial(p),e(s),values(v),valid(v2) {}
323  void operator()(double yaw) {
324  double dist;
325  const mrpt::poses::CPose3D pNew=initial+mrpt::poses::CPose3D(0.0,0.0,0.0,yaw,0.0,0.0);
326  if (e->traceRay(pNew,dist)) {
327  values.push_back(dist);
328  valid.push_back(1);
329  } else {
330  values.push_back(0);
331  valid.push_back(0);
332  }
333  }
334  };
335  /**
336  * Internal functor class to trace a set of rays.
337  */
338  template<class T> class FTrace2D {
339  protected:
340  const T &e;
342  CAngularObservationMeshPtr &caom;
344  std::vector<mrpt::obs::CObservation2DRangeScan> &vObs;
346  public:
347  FTrace2D(const T &s,const mrpt::poses::CPose3D &p,CAngularObservationMeshPtr &om,const CAngularObservationMesh::TDoubleRange &y,std::vector<mrpt::obs::CObservation2DRangeScan> &obs,const mrpt::poses::CPose3D &b):e(s),initial(p),caom(om),yaws(y),vObs(obs),pBase(b) {}
348  void operator()(double pitch) {
349  std::vector<double> yValues;
350  yaws.values(yValues);
352  const mrpt::poses::CPose3D pNew=initial+mrpt::poses::CPose3D(0,0,0,0,pitch,0);
353  std::vector<double> values;
354  std::vector<char> valid;
355  size_t nY=yValues.size();
356  values.reserve(nY);
357  valid.reserve(nY);
358  for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,pNew,values,valid));
359  o.aperture=yaws.aperture();
360  o.rightToLeft=yaws.negToPos();
361  o.maxRange=10000;
362  o.sensorPose=pNew;
363  o.deltaPitch=0;
364  o.resizeScan(values.size());
365  for (size_t i=0;i<values.size();i++) {
366  o.setScanRange(i, values[i]);
367  o.setScanRangeValidity(i, valid[i]!=0);
368  }
369  vObs.push_back(o);
370  }
371  };
372  public:
373  /**
374  * 2D ray tracing (will generate a 3D mesh). Given an object and two ranges, realizes a scan from the initial pose and stores it in a CAngularObservationMesh object.
375  * The objective may be a COpenGLScene, a CRenderizable or any children of its.
376  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
377  */
378  template<class T> static void trace2DSetOfRays(const T &e,const mrpt::poses::CPose3D &initial,CAngularObservationMeshPtr &caom,const TDoubleRange &pitchs,const TDoubleRange &yaws);
379  /**
380  * 2D ray tracing (will generate a vectorial mesh inside a plane). Given an object and a range, realizes a scan from the initial pose and stores it in a CObservation2DRangeScan object.
381  * The objective may be a COpenGLScene, a CRenderizable or any children of its.
382  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
383  */
384  template<class T> static void trace1DSetOfRays(const T &e,const mrpt::poses::CPose3D &initial,mrpt::obs::CObservation2DRangeScan &obs,const TDoubleRange &yaws) {
385  std::vector<double> yValues;
386  yaws.values(yValues);
387  std::vector<float> scanValues;
388  std::vector<char> valid;
389  size_t nV=yaws.amount();
390  scanValues.reserve(nV);
391  valid.reserve(nV);
392  for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,initial,scanValues,valid));
393  obs.aperture=yaws.aperture();
394  obs.rightToLeft=yaws.negToPos();
395  obs.maxRange=10000;
396  obs.sensorPose=initial;
397  obs.deltaPitch=0;
398  obs.scan=scanValues;
399  obs.validRange=valid;
400  }
401  };
402  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(CAngularObservationMesh,CRenderizableDisplayList, MAPS_IMPEXP)
403 
404  template<class T>
405  void CAngularObservationMesh::trace2DSetOfRays(const T &e,const mrpt::poses::CPose3D &initial,CAngularObservationMeshPtr &caom,const TDoubleRange &pitchs,const TDoubleRange &yaws) {
406  std::vector<double> pValues;
407  pitchs.values(pValues);
408  std::vector<mrpt::obs::CObservation2DRangeScan> vObs;
409  vObs.reserve(pValues.size());
410  for_each(pValues.begin(),pValues.end(),FTrace2D<T>(e,initial,caom,yaws,vObs,initial));
411  caom->mWireframe=false;
412  caom->mEnableTransparency=false;
413  caom->setPitchBounds(pValues);
414  caom->setScanSet(vObs);
415  }
416 }
417 }
418 #endif
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.
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.
static void trace2DSetOfRays(const T &e, const mrpt::poses::CPose3D &initial, CAngularObservationMeshPtr &caom, const TDoubleRange &pitchs, const TDoubleRange &yaws)
2D ray tracing (will generate a 3D mesh).
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3535
TDoubleRange(double a, size_t b, bool c)
Constructor from aperture, amount of samples and scan direction.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
virtual ~CAngularObservationMesh()
Empty destructor.
bool BASE_IMPEXP traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:1996
void setScanRange(const size_t i, const float val)
mrpt::math::CMatrixTemplate< mrpt::math::TPoint3D > actualMesh
Mutable object with the mesh&#39;s points.
int sign(T x)
Returns the sign of X as "1" or "-1".
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
STL namespace.
std::vector< mrpt::obs::CObservation2DRangeScan > & vObs
GLdouble s
Definition: glext.h:3602
bool mEnableTransparency
Whether the object may present transparencies or not.
void setWireframe(bool enabled=true)
Sets the display mode for the object.
bool isTransparencyEnabled() const
Returns whether the object may be transparent or not.
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. 80m, 50m,...)
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
double finalValue() const
Returns the last value of the range.
mrpt::math::CMatrixB validityMatrix
Scan validity matrix.
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
const GLubyte * c
Definition: glext.h:5590
double initialValue() const
Returns the first value of the range.
size_t amount() const
Returns the total amount of values in this range.
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
FTrace1D(const T &s, const mrpt::poses::CPose3D &p, std::vector< double > &v, std::vector< char > &v2)
GLubyte GLubyte b
Definition: glext.h:5575
class BASE_IMPEXP TPolygon3D
double increment() const
Returns the increment between two consecutive values of the range.
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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).
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:72
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.
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...
FTrace2D(const T &s, const mrpt::poses::CPose3D &p, CAngularObservationMeshPtr &om, const CAngularObservationMesh::TDoubleRange &y, std::vector< mrpt::obs::CObservation2DRangeScan > &obs, const mrpt::poses::CPose3D &b)
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
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.
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:26
GLenum GLint GLint y
Definition: glext.h:3516
TDoubleRange(double a, double b, double c)
Constructor from initial value, final value and range.
GLfloat GLfloat GLfloat v2
Definition: glext.h:3923
GLuint res
Definition: glext.h:6298
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
Lightweight 3D point.
void enableTransparency(bool enabled=true)
Enables or disables transparencies.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
std::vector< CSetOfTriangles::TTriangle > triangles
Actual set of triangles to be displayed.
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
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. When containing exactly two elements, they represent the bounds...
bool isWireframe() const
Returns whether the object is configured as wireframe or solid.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019