Go to the documentation of this file.
9 #ifndef opengl_CAngularObservationMesh_H
10 #define opengl_CAngularObservationMesh_H
80 rangeData.mode0.initial=a;
81 rangeData.mode0.final=b;
82 rangeData.mode0.increment=c;
88 rangeData.mode1.initial=a;
89 rangeData.mode1.final=b;
90 rangeData.mode1.amount=c;
96 rangeData.mode2.aperture=a;
97 rangeData.mode2.amount=b;
98 rangeData.mode2.negToPos=c;
106 if (increment==0)
throw std::logic_error(
"Invalid increment value.");
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.");
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.");
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.");
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.");
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.");
185 void values(std::vector<double> &vals)
const;
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.");
203 void updateMesh()
const;
205 mutable std::vector<CSetOfTriangles::TTriangle>
triangles;
213 std::vector<mrpt::obs::CObservation2DRangeScan>
scanSet;
217 CAngularObservationMesh():mWireframe(true),meshUpToDate(false),mEnableTransparency(true),actualMesh(0,0),validityMatrix(0,0),pitchBounds(),scanSet() {}
236 return mEnableTransparency;
242 mEnableTransparency=enabled;
258 void setPitchBounds(const
double initial,const
double final);
262 void setPitchBounds(const std::vector<
double> &bounds);
266 void getPitchBounds(
double &initial,
double &final) const;
270 void getPitchBounds(std::vector<
double> &bounds) const;
274 void getScanSet(std::vector<
mrpt::obs::CObservation2DRangeScan> &scans) const;
278 bool setScanSet(const std::vector<
mrpt::obs::CObservation2DRangeScan> &scans);
283 void generateSetOfTriangles(CSetOfTrianglesPtr &res) const;
287 void generatePointCloud(
mrpt::maps::CPointsMap *out_map) const;
292 void getTracedRays(CSetOfLinesPtr &res) const;
297 void getUntracedRays(CSetOfLinesPtr &res,
double dist) const;
302 void generateSetOfTriangles(std::vector<
mrpt::math::
TPolygon3D> &res) const;
306 void getActualMesh(
mrpt::math::CMatrixTemplate<
mrpt::math::TPoint3D> &pts,
mrpt::math::CMatrixBool &validity)
const {
307 if (!meshUpToDate) updateMesh();
309 validity=validityMatrix;
326 if (e->traceRay(pNew,dist)) {
327 values.push_back(dist);
342 CAngularObservationMeshPtr &
caom;
344 std::vector<mrpt::obs::CObservation2DRangeScan> &
vObs;
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) {}
349 std::vector<double> yValues;
353 std::vector<double> values;
354 std::vector<char> valid;
355 size_t nY=yValues.size();
358 for_each(yValues.begin(),yValues.end(),
FTrace1D<T>(e,pNew,values,valid));
364 o.
scan.resize(values.size());
365 for (
size_t i=0;i<values.size();i++) o.
scan[i]=values[i];
376 template<
class T>
static void trace2DSetOfRays(
const T &e,
const mrpt::poses::CPose3D &initial,CAngularObservationMeshPtr &caom,
const TDoubleRange &pitchs,
const TDoubleRange &yaws);
383 std::vector<double> yValues;
385 std::vector<float> scanValues;
386 std::vector<char> valid;
388 scanValues.reserve(nV);
390 for_each(yValues.begin(),yValues.end(),
FTrace1D<T>(e,initial,scanValues,valid));
404 std::vector<double> 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);
std::vector< double > pitchBounds
Observation pitch range. When containing exactly two elements, they represent the bounds.
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
size_t amount() const
Returns the total amount of values in this range.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
std::vector< mrpt::obs::CObservation2DRangeScan > scanSet
Actual scan set which is used to generate the mesh.
bool isTransparencyEnabled() const
Returns whether the object may be transparent or not.
double initialValue() const
Returns the first value of the range.
std::vector< mrpt::obs::CObservation2DRangeScan > & vObs
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void operator()(double pitch)
const mrpt::poses::CPose3D & initial
double finalValue() const
Returns the last value of the range.
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).
Union type with the actual data.
A renderizable object suitable for rendering with OpenGL's display lists.
std::vector< char > & valid
static TDoubleRange CreateFromIncrement(double initial, double final, double increment)
Creates a range of values from the initial value, the final value and the increment.
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated)
CAngularObservationMesh()
Basic constructor.
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).
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.
TDoubleRange(double a, double b, double c)
Constructor from initial value, final value and range.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double increment() const
Returns the increment between two consecutive values of the range.
const CAngularObservationMesh::TDoubleRange & yaws
FTrace1D(const T &s, const mrpt::poses::CPose3D &p, std::vector< double > &v, std::vector< char > &v2)
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
double aperture() const
Returns the total aperture of the range.
const mrpt::poses::CPose3D & initial
Range specification type, with several uses.
std::vector< CSetOfTriangles::TTriangle > triangles
Actual set of triangles to be displayed.
std::vector< double > & values
int sign(T x)
Returns the sign of X as "1" or "-1".
void enableTransparency(bool enabled=true)
Enables or disables transparencies.
CAngularObservationMeshPtr & caom
bool negToPos() const
Returns the direction of the scan.
char rangeType
Range type.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
void values(std::vector< double > &vals) const
Gets a vector with every value in the range.
std::vector< char > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
virtual ~CAngularObservationMesh()
Empty destructor.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
bool mEnableTransparency
Whether the object may present transparencies or not.
void setWireframe(bool enabled=true)
Sets the display mode for the object.
A mesh built from a set of 2D laser scan observations.
This class is a "CSerializable" wrapper for "CMatrixBool".
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
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.
bool meshUpToDate
Mutable variable which controls if the object has suffered any change since last time the mesh was up...
const mrpt::poses::CPose3D & pBase
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)
Internal functor class to trace a ray.
Internal functor class to trace a set of rays.
#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...
bool mWireframe
Whether the mesh will be displayed wireframe or solid.
TDoubleRange(double a, size_t b, bool c)
Constructor from aperture, amount of samples and scan direction.
bool isWireframe() const
Returns whether the object is configured as wireframe or solid.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
class BASE_IMPEXP TPolygon3D
bool BASE_IMPEXP traceRay(const vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
mrpt::math::CMatrixB validityMatrix
Scan validity matrix.
void operator()(double yaw)
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
std::vector< float > scan
The range values of the scan, in meters. Must have same length than validRange.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
mrpt::math::CMatrixTemplate< mrpt::math::TPoint3D > actualMesh
Mutable object with the mesh's points.
Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 22:32:58 UTC 2019 | | |