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.");
107 return TDoubleRange(initial,
final,increment);
113 return TDoubleRange(initial,
final,amount);
119 return TDoubleRange(aperture,amount,negToPos);
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;
249 virtual void render_dl()
const;
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;
292 void getTracedRays(CSetOfLinesPtr &res)
const;
297 void getUntracedRays(CSetOfLinesPtr &res,
double dist)
const;
302 void generateSetOfTriangles(std::vector<mrpt::math::TPolygon3D> &res)
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;
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);
bool meshUpToDate
Mutable variable which controls if the object has suffered any change since last time the mesh was up...
std::vector< char > & valid
A mesh built from a set of 2D laser scan observations.
double initialValue() const
Returns the first value of the range.
CAngularObservationMeshPtr & caom
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).
const mrpt::poses::CPose3D & pBase
TDoubleRange(double a, size_t b, bool c)
Constructor from aperture, amount of samples and scan direction.
virtual ~CAngularObservationMesh()
Empty destructor.
const mrpt::poses::CPose3D & initial
char rangeType
Range type.
mrpt::math::CMatrixTemplate< mrpt::math::TPoint3D > actualMesh
Mutable object with the mesh's points.
void values(std::vector< double > &vals) const
Gets a vector with every value in the range.
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.
CAngularObservationMesh()
Basic constructor.
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
A renderizable object suitable for rendering with OpenGL's display lists.
void operator()(double pitch)
bool negToPos() const
Returns the direction of the scan.
double aperture() const
Returns the total aperture of the range.
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.
std::vector< float > scan
The range values of the scan, in meters. Must have same length than validRange.
#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.
FTrace1D(const T &s, const mrpt::poses::CPose3D &p, std::vector< double > &v, std::vector< char > &v2)
double finalValue() const
Returns the last value of the range.
bool BASE_IMPEXP traceRay(const vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
std::vector< double > & values
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).
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).
bool mWireframe
Whether the mesh will be displayed wireframe or solid.
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".
TDoubleRange(double a, double b, double c)
Constructor from initial value, final value and range.
void enableTransparency(bool enabled=true)
Enables or disables transparencies.
Union type with the actual data.
void operator()(double yaw)
const mrpt::poses::CPose3D & initial
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
std::vector< char > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
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.