Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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>
19
20#include <mrpt/math/geometry.h>
21
22namespace mrpt {
23namespace 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 */
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;
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
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;
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.scan.resize(values.size());
365 for (size_t i=0;i<values.size();i++) o.scan[i]=values[i];
366 o.validRange=valid;
367 vObs.push_back(o);
368 }
369 };
370 public:
371 /**
372 * 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.
373 * The objective may be a COpenGLScene, a CRenderizable or any children of its.
374 * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
375 */
376 template<class T> static void trace2DSetOfRays(const T &e,const mrpt::poses::CPose3D &initial,CAngularObservationMeshPtr &caom,const TDoubleRange &pitchs,const TDoubleRange &yaws);
377 /**
378 * 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.
379 * The objective may be a COpenGLScene, a CRenderizable or any children of its.
380 * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
381 */
382 template<class T> static void trace1DSetOfRays(const T &e,const mrpt::poses::CPose3D &initial,mrpt::obs::CObservation2DRangeScan &obs,const TDoubleRange &yaws) {
383 std::vector<double> yValues;
384 yaws.values(yValues);
385 std::vector<float> scanValues;
386 std::vector<char> valid;
387 size_t nV=yaws.amount();
388 scanValues.reserve(nV);
389 valid.reserve(nV);
390 for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,initial,scanValues,valid));
391 obs.aperture=yaws.aperture();
392 obs.rightToLeft=yaws.negToPos();
393 obs.maxRange=10000;
394 obs.sensorPose=initial;
395 obs.deltaPitch=0;
396 obs.scan=scanValues;
397 obs.validRange=valid;
398 }
399 };
400 DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(CAngularObservationMesh,CRenderizableDisplayList, MAPS_IMPEXP)
401
402 template<class T>
404 std::vector<double> pValues;
405 pitchs.values(pValues);
406 std::vector<mrpt::obs::CObservation2DRangeScan> vObs;
407 vObs.reserve(pValues.size());
408 for_each(pValues.begin(),pValues.end(),FTrace2D<T>(e,initial,caom,yaws,vObs,initial));
409 caom->mWireframe=false;
410 caom->mEnableTransparency=false;
411 caom->setPitchBounds(pValues);
412 caom->setScanSet(vObs);
413 }
414}
415}
416#endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#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...
This class is a "CSerializable" wrapper for "CMatrixBool".
Definition CMatrixB.h:27
This template class provides the basic functionality for a general 2D any-size, resizable container o...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
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< float > scan
The range values of the scan, in meters. Must have same length than validRange.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
std::vector< char > validRange
It'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)
Internal functor class to trace a set of rays.
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)
const CAngularObservationMesh::TDoubleRange & yaws
std::vector< mrpt::obs::CObservation2DRangeScan > & vObs
A mesh built from a set of 2D laser scan observations.
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.
mrpt::math::CMatrixB validityMatrix
Scan validity matrix.
std::vector< CSetOfTriangles::TTriangle > triangles
Actual set of triangles to be displayed.
bool mWireframe
Whether the mesh will be displayed wireframe or solid.
mrpt::math::CMatrixTemplate< mrpt::math::TPoint3D > actualMesh
Mutable object with the mesh's points.
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).
virtual void render_dl() const MRPT_OVERRIDE
Renderizes the object.
void setWireframe(bool enabled=true)
Sets the display mode for the object.
bool meshUpToDate
Mutable variable which controls if the object has suffered any change since last time the mesh was up...
virtual ~CAngularObservationMesh()
Empty destructor.
bool mEnableTransparency
Whether the object may present transparencies or not.
void updateMesh() const
Updates the mesh, if needed. It's a const method, but modifies mutable content.
bool isWireframe() const
Returns whether the object is configured as wireframe or solid.
std::vector< double > pitchBounds
Observation pitch range. When containing exactly two elements, they represent the bounds.
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const MRPT_OVERRIDE
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
std::vector< mrpt::obs::CObservation2DRangeScan > scanSet
Actual scan set which is used to generate the mesh.
void enableTransparency(bool enabled=true)
Enables or disables transparencies.
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).
bool isTransparencyEnabled() const
Returns whether the object may be transparent or not.
A renderizable object suitable for rendering with OpenGL's display lists.
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition CPose3D.h:73
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
struct OPENGL_IMPEXP CSetOfLinesPtr
Definition CSetOfLines.h:21
struct OPENGL_IMPEXP CSetOfTrianglesPtr
struct MAPS_IMPEXP CAngularObservationMeshPtr
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition bits.h:91
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
Lightweight 3D point.
Range specification type, with several uses.
TDoubleRange(double a, size_t b, bool c)
Constructor from aperture, amount of samples and scan direction.
double increment() const
Returns the increment between two consecutive values of the range.
double aperture() const
Returns the total aperture of the range.
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.
double initialValue() const
Returns the first value of the range.
TDoubleRange(double a, double b, double c)
Constructor from initial value, final value and range.
size_t amount() const
Returns the total amount of values in this range.
void values(std::vector< double > &vals) const
Gets a vector with every value in the range.
bool negToPos() const
Returns the direction of the scan.
double finalValue() const
Returns the last value 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.
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.
TDoubleRange(double a, double b, size_t c)
Constructor from initial value, final value and amount of samples.



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 27 15:23:24 UTC 2023