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.");
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;
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);
#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".
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.
Internal functor class to trace a ray.
void operator()(double yaw)
std::vector< double > & values
const mrpt::poses::CPose3D & initial
std::vector< char > & valid
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.
CAngularObservationMeshPtr & caom
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)
void operator()(double pitch)
const mrpt::poses::CPose3D & pBase
const CAngularObservationMesh::TDoubleRange & yaws
std::vector< mrpt::obs::CObservation2DRangeScan > & vObs
const mrpt::poses::CPose3D & initial
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.
CAngularObservationMesh()
Basic constructor.
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).
bool BASE_IMPEXP traceRay(const vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
class BASE_IMPEXP TPolygon3D
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
int sign(T x)
Returns the sign of X as "1" or "-1".
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
char rangeType
Range type.
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.
Union type with the actual data.