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  */
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  };
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
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
bool meshUpToDate
Mutable variable which controls if the object has suffered any change since last time the mesh was up...
const GLdouble * v
Definition: glew.h:1296
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
A mesh built from a set of 2D laser scan observations.
double initialValue() const
Returns the first value of the range.
Internal functor class to trace a set of rays.
double increment() const
Returns the increment between two consecutive values of the range.
Internal functor class to trace a ray.
Declares a matrix of booleans (non serializable).
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).
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)
const GLfloat * c
Definition: glew.h:10088
GLboolean GLenum GLenum GLvoid * values
Definition: glew.h:2897
mrpt::math::CMatrixTemplate< mrpt::math::TPoint3D > actualMesh
Mutable object with the mesh&#39;s points.
GLfloat GLfloat GLfloat v2
Definition: glew.h:1763
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) ...
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.
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.
bool negToPos() const
Returns the direction of the scan.
double aperture() const
Returns the total aperture of the range.
GLdouble s
Definition: glew.h:1295
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
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...
void getActualMesh(mrpt::math::CMatrixTemplate< mrpt::math::TPoint3D > &pts, mrpt::math::CMatrixBool &validity) const
Retrieves the full mesh, along with the validity matrix.
bool isWireframe() const
Returns whether the object is configured as wireframe or solid.
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)
GLfloat GLfloat p
Definition: glew.h:10113
double finalValue() const
Returns the last value of the range.
GLuint res
Definition: glew.h:7143
bool isTransparencyEnabled() const
Returns whether the object may be transparent or not.
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).
void values(std::vector< double > &vals) const
Gets a vector with every value in the range.
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)
size_t amount() const
Returns the total amount of values in this range.
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)...
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
TDoubleRange(double a, double b, double c)
Constructor from initial value, final value and range.
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
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.
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 rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018