Main MRPT website > C++ reference
MRPT logo
CAngularObservationMesh.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef opengl_CAngularObservationMesh_H
29 #define opengl_CAngularObservationMesh_H
30 
31 #include <mrpt/opengl/CRenderizableDisplayList.h>
32 #include <mrpt/opengl/CSetOfTriangles.h>
33 #include <mrpt/math/CMatrixTemplate.h>
34 #include <mrpt/math/CMatrixB.h>
35 #include <mrpt/utils/stl_extensions.h>
36 #include <mrpt/slam/CObservation2DRangeScan.h>
37 #include <mrpt/slam/CPointsMap.h>
38 #include <mrpt/opengl/CSetOfLines.h>
39 
40 #include <mrpt/math/geometry.h>
41 
42 namespace mrpt {
43 namespace opengl {
44  using namespace mrpt::utils;
45  using namespace mrpt::slam;
46  using namespace mrpt::poses;
47 
48  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(CAngularObservationMesh,CRenderizableDisplayList, MAPS_IMPEXP)
49 
50  /**
51  * A mesh built from a set of 2D laser scan observations.
52  * Each element of this set is a single scan through the yaw, given a specific pitch.
53  * Each scan has a CPose3D identifying the origin of the scan, which ideally is the
54  * same for every one of them.
55  * \ingroup mrpt_maps_grp
56  */
57  class MAPS_IMPEXP CAngularObservationMesh:public CRenderizableDisplayList {
58  DEFINE_SERIALIZABLE(CAngularObservationMesh)
59  public:
60  /**
61  * Range specification type, with several uses.
62  */
63  struct MAPS_IMPEXP TDoubleRange {
64  private:
65  /**
66  * Range type.
67  * If 0, it's specified by an initial and a final value, and an increment.
68  * If 1, it's specified by an initial and a final value, and a fixed size of samples.
69  * 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.
70  */
71  char rangeType;
72  /**
73  * Union type with the actual data.
74  * \sa rangeType
75  */
76  union rd {
77  struct {
78  double initial;
79  double final;
80  double increment;
81  } mode0;
82  struct {
83  double initial;
84  double final;
85  size_t amount;
86  } mode1;
87  struct {
88  double aperture;
89  size_t amount;
90  bool negToPos;
91  } mode2;
92  } rangeData;
93  /**
94  * Constructor from initial value, final value and range.
95  */
96  TDoubleRange(double a,double b,double c):rangeType(0) {
97  rangeData.mode0.initial=a;
98  rangeData.mode0.final=b;
99  rangeData.mode0.increment=c;
100  }
101  /**
102  * Constructor from initial value, final value and amount of samples.
103  */
104  TDoubleRange(double a,double b,size_t c):rangeType(1) {
105  rangeData.mode1.initial=a;
106  rangeData.mode1.final=b;
107  rangeData.mode1.amount=c;
108  }
109  /**
110  * Constructor from aperture, amount of samples and scan direction.
111  */
112  TDoubleRange(double a,size_t b,bool c):rangeType(2) {
113  rangeData.mode2.aperture=a;
114  rangeData.mode2.amount=b;
115  rangeData.mode2.negToPos=c;
116  }
117  public:
118  /**
119  * Creates a range of values from the initial value, the final value and the increment.
120  * \throw std::logic_error if the increment is zero.
121  */
122  inline static TDoubleRange CreateFromIncrement(double initial,double final,double increment) {
123  if (increment==0) throw std::logic_error("Invalid increment value.");
124  return TDoubleRange(initial,final,increment);
125  }
126  /**
127  * Creates a range of values from the initial value, the final value and a desired amount of samples.
128  */
129  inline static TDoubleRange CreateFromAmount(double initial,double final,size_t amount) {
130  return TDoubleRange(initial,final,amount);
131  }
132  /**
133  * Creates a zero-centered range of values from an aperture, an amount of samples and a direction.
134  */
135  inline static TDoubleRange CreateFromAperture(double aperture,size_t amount,bool negToPos=true) {
136  return TDoubleRange(aperture,amount,negToPos);
137  }
138  /**
139  * Returns the total aperture of the range.
140  * \throw std::logic_error on invalid range type.
141  */
142  inline double aperture() const {
143  switch (rangeType) {
144  case 0:return (sign(rangeData.mode0.increment)==sign(rangeData.mode0.final-rangeData.mode0.initial))?fabs(rangeData.mode0.final-rangeData.mode0.initial):0;
145  case 1:return rangeData.mode1.final-rangeData.mode1.initial;
146  case 2:return rangeData.mode2.aperture;
147  default:throw std::logic_error("Unknown range type.");
148  }
149  }
150  /**
151  * Returns the first value of the range.
152  * \throw std::logic_error on invalid range type.
153  */
154  inline double initialValue() const {
155  switch (rangeType) {
156  case 0:
157  case 1:return rangeData.mode0.initial;
158  case 2:return rangeData.mode2.negToPos?-rangeData.mode2.aperture/2:rangeData.mode2.aperture/2;
159  default:throw std::logic_error("Unknown range type.");
160  }
161  }
162  /**
163  * Returns the last value of the range.
164  * \throw std::logic_error on invalid range type.
165  */
166  inline double finalValue() const {
167  switch (rangeType) {
168  case 0:return (sign(rangeData.mode0.increment)==sign(rangeData.mode0.final-rangeData.mode0.initial))?rangeData.mode0.final:rangeData.mode0.initial;
169  case 1:return rangeData.mode1.final;
170  case 2:return rangeData.mode2.negToPos?rangeData.mode2.aperture/2:-rangeData.mode2.aperture/2;
171  default:throw std::logic_error("Unknown range type.");
172  }
173  }
174  /**
175  * Returns the increment between two consecutive values of the range.
176  * \throw std::logic_error on invalid range type.
177  */
178  inline double increment() const {
179  switch (rangeType) {
180  case 0:return rangeData.mode0.increment;
181  case 1:return (rangeData.mode1.final-rangeData.mode1.initial)/static_cast<double>(rangeData.mode1.amount-1);
182  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);
183  default:throw std::logic_error("Unknown range type.");
184  }
185  }
186  /**
187  * Returns the total amount of values in this range.
188  * \throw std::logic_error on invalid range type.
189  */
190  inline size_t amount() const {
191  switch (rangeType) {
192  case 0:return (sign(rangeData.mode0.increment)==sign(rangeData.mode0.final-rangeData.mode0.initial))?1+static_cast<size_t>(ceil((rangeData.mode0.final-rangeData.mode0.initial)/rangeData.mode0.increment)):1;
193  case 1:return rangeData.mode1.amount;
194  case 2:return rangeData.mode2.amount;
195  default:throw std::logic_error("Unknown range type.");
196  }
197  }
198  /**
199  * Gets a vector with every value in the range.
200  * \throw std::logic_error on invalid range type.
201  */
202  void values(std::vector<double> &vals) const;
203  /**
204  * Returns the direction of the scan. True if the increment is positive, false otherwise.
205  * \throw std::logic_error on invalid range type.
206  */
207  inline bool negToPos() const {
208  switch (rangeType) {
209  case 0:return sign(rangeData.mode0.increment)>0;
210  case 1:return sign(rangeData.mode1.final-rangeData.mode1.initial)>0;
211  case 2:return rangeData.mode2.negToPos;
212  default:throw std::logic_error("Unknown range type.");
213  }
214  }
215  };
216  protected:
217  /**
218  * Updates the mesh, if needed. It's a const method, but modifies mutable content.
219  */
220  void updateMesh() const;
221  /**
222  * Empty destructor.
223  */
224  virtual ~CAngularObservationMesh() {}
225  /**
226  * Actual set of triangles to be displayed.
227  */
228  mutable std::vector<CSetOfTriangles::TTriangle> triangles;
229  /**
230  * Internal method to add a triangle to the mutable mesh.
231  */
232  void addTriangle(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) const;
233  /**
234  * Whether the mesh will be displayed wireframe or solid.
235  */
236  bool mWireframe;
237  /**
238  * Mutable variable which controls if the object has suffered any change since last time the mesh was updated.
239  */
240  mutable bool meshUpToDate;
241  /**
242  * Whether the object may present transparencies or not.
243  */
244  bool mEnableTransparency;
245  /**
246  * Mutable object with the mesh's points.
247  */
248  mutable mrpt::math::CMatrixTemplate<TPoint3D> actualMesh;
249  /**
250  * Scan validity matrix.
251  */
252  mutable mrpt::math::CMatrixB validityMatrix;
253  /**
254  * Observation pitch range. When containing exactly two elements, they represent the bounds.
255  */
256  std::vector<double> pitchBounds;
257  /**
258  * Actual scan set which is used to generate the mesh.
259  */
260  std::vector<CObservation2DRangeScan> scanSet;
261  /**
262  * Basic constructor.
263  */
264  CAngularObservationMesh():mWireframe(true),meshUpToDate(false),mEnableTransparency(true),actualMesh(0,0),validityMatrix(0,0),pitchBounds(),scanSet() {}
265  public:
266  /**
267  * Returns whether the object is configured as wireframe or solid.
268  */
269  inline bool isWireframe() const {
270  return mWireframe;
271  }
272  /**
273  * Sets the display mode for the object. True=wireframe, False=solid.
274  */
275  inline void setWireframe(bool enabled=true) {
276  mWireframe=enabled;
277  CRenderizableDisplayList::notifyChange();
278  }
279  /**
280  * Returns whether the object may be transparent or not.
281  */
282  inline bool isTransparencyEnabled() const {
283  return mEnableTransparency;
284  }
285  /**
286  * Enables or disables transparencies.
287  */
288  inline void enableTransparency(bool enabled=true) {
289  mEnableTransparency=enabled;
290  CRenderizableDisplayList::notifyChange();
291  }
292  /**
293  * Renderizes the object.
294  * \sa mrpt::opengl::CRenderizable
295  */
296  virtual void render_dl() const;
297  /**
298  * Traces a ray to the object, returning the distance to a given pose through its X axis.
299  * \sa mrpt::opengl::CRenderizable,trace2DSetOfRays,trace1DSetOfRays
300  */
301  virtual bool traceRay(const mrpt::poses::CPose3D &o,double &dist) const;
302  /**
303  * Sets the pitch bounds for this range.
304  */
305  void setPitchBounds(const double initial,const double final);
306  /**
307  * Sets the pitch bounds for this range.
308  */
309  void setPitchBounds(const std::vector<double> bounds);
310  /**
311  * Gets the initial and final pitch bounds for this range.
312  */
313  void getPitchBounds(double &initial,double &final) const;
314  /**
315  * Gets the pitch bounds for this range.
316  */
317  void getPitchBounds(std::vector<double> &bounds) const;
318  /**
319  * Gets the scan set.
320  */
321  void getScanSet(std::vector<CObservation2DRangeScan> &scans) const;
322  /**
323  * Sets the scan set.
324  */
325  bool setScanSet(const std::vector<CObservation2DRangeScan> &scans);
326  /**
327  * Gets the mesh as a set of triangles, for displaying them.
328  * \sa generateSetOfTriangles(std::vector<TPolygon3D> &),mrpt::opengl::CSetOfTriangles,mrpt::opengl::CSetOfTriangles::TTriangle
329  */
330  void generateSetOfTriangles(CSetOfTrianglesPtr &res) const;
331  /**
332  * 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.
333  */
334  void generatePointCloud(CPointsMap *out_map) const;
335  /**
336  * Gets a set of lines containing the traced rays, for displaying them.
337  * \sa getUntracedRays,mrpt::opengl::CSetOfLines
338  */
339  void getTracedRays(CSetOfLinesPtr &res) const;
340  /**
341  * Gets a set of lines containing the untraced rays, up to a specified distance, for displaying them.
342  * \sa getTracedRays,mrpt::opengl::CSetOfLines
343  */
344  void getUntracedRays(CSetOfLinesPtr &res,double dist) const;
345  /**
346  * Gets the mesh as a set of polygons, to work with them.
347  * \sa generateSetOfTriangles(mrpt::opengl::CSetOfTriangles &)
348  */
349  void generateSetOfTriangles(std::vector<TPolygon3D> &res) const;
350  /**
351  * Retrieves the full mesh, along with the validity matrix.
352  */
353  void getActualMesh(mrpt::math::CMatrixTemplate<mrpt::math::TPoint3D> &pts,mrpt::math::CMatrixBool &validity) const {
354  if (!meshUpToDate) updateMesh();
355  pts=actualMesh;
356  validity=validityMatrix;
357  }
358  private:
359  /**
360  * Internal functor class to trace a ray.
361  */
362  template<class T> class FTrace1D {
363  protected:
364  const CPose3D &initial;
365  const T &e;
366  std::vector<double> &values;
367  std::vector<char> &valid;
368  public:
369  FTrace1D(const T &s,const CPose3D &p,std::vector<double> &v,std::vector<char> &v2):initial(p),e(s),values(v),valid(v2) {}
370  void operator()(double yaw) {
371  double dist;
372  const CPose3D pNew=initial+CPose3D(0.0,0.0,0.0,yaw,0.0,0.0);
373  if (e->traceRay(pNew,dist)) {
374  values.push_back(dist);
375  valid.push_back(1);
376  } else {
377  values.push_back(0);
378  valid.push_back(0);
379  }
380  }
381  };
382  /**
383  * Internal functor class to trace a set of rays.
384  */
385  template<class T> class FTrace2D {
386  protected:
387  const T &e;
388  const CPose3D &initial;
389  CAngularObservationMeshPtr &caom;
390  const CAngularObservationMesh::TDoubleRange &yaws;
391  std::vector<CObservation2DRangeScan> &vObs;
392  const CPose3D &pBase;
393  public:
394  FTrace2D(const T &s,const CPose3D &p,CAngularObservationMeshPtr &om,const CAngularObservationMesh::TDoubleRange &y,std::vector<CObservation2DRangeScan> &obs,const CPose3D &b):e(s),initial(p),caom(om),yaws(y),vObs(obs),pBase(b) {}
395  void operator()(double pitch) {
396  std::vector<double> yValues;
397  yaws.values(yValues);
398  CObservation2DRangeScan o=CObservation2DRangeScan();
399  const CPose3D pNew=initial+CPose3D(0,0,0,0,pitch,0);
400  std::vector<double> values;
401  std::vector<char> valid;
402  size_t nY=yValues.size();
403  values.reserve(nY);
404  valid.reserve(nY);
405  for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,pNew,values,valid));
406  o.aperture=yaws.aperture();
407  o.rightToLeft=yaws.negToPos();
408  o.maxRange=10000;
409  o.sensorPose=pNew;
410  o.deltaPitch=0;
411  mrpt::utils::copy_container_typecasting(values,o.scan);
412  o.validRange=valid;
413  vObs.push_back(o);
414  }
415  };
416  public:
417  /**
418  * 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.
419  * The objective may be a COpenGLScene, a CRenderizable or any children of its.
420  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
421  */
422  template<class T> static void trace2DSetOfRays(const T &e,const CPose3D &initial,CAngularObservationMeshPtr &caom,const TDoubleRange &pitchs,const TDoubleRange &yaws) {
423  std::vector<double> pValues;
424  pitchs.values(pValues);
425  std::vector<CObservation2DRangeScan> vObs;
426  vObs.reserve(pValues.size());
427  for_each(pValues.begin(),pValues.end(),FTrace2D<T>(e,initial,caom,yaws,vObs,initial));
428  caom->mWireframe=false;
429  caom->mEnableTransparency=false;
430  caom->setPitchBounds(pValues);
431  caom->setScanSet(vObs);
432  }
433  /**
434  * 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.
435  * The objective may be a COpenGLScene, a CRenderizable or any children of its.
436  * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
437  */
438  template<class T> static void trace1DSetOfRays(const T &e,const CPose3D &initial,CObservation2DRangeScan &obs,const TDoubleRange &yaws) {
439  std::vector<double> yValues;
440  yaws.values(yValues);
441  std::vector<double> scanValues;
442  std::vector<char> valid;
443  size_t nV=yaws.amount();
444  scanValues.reserve(nV);
445  valid.reserve(nV);
446  for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,initial,scanValues,valid));
447  obs.aperture=yaws.aperture();
448  obs.rightToLeft=yaws.negToPos();
449  obs.maxRange=10000;
450  obs.sensorPose=initial;
451  obs.deltaPitch=0;
452  obs.scan=scanValues;
453  obs.validRange=valid;
454  }
455  };
456 }
457 }
458 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013