00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef opengl_CAngularObservationMesh_H
00029 #define opengl_CAngularObservationMesh_H
00030 #include <mrpt/opengl/CRenderizable.h>
00031 #include <mrpt/opengl/CSetOfTriangles.h>
00032 #include <mrpt/math/CMatrixTemplate.h>
00033 #include <mrpt/math/CMatrixB.h>
00034 #include <mrpt/utils/stl_extensions.h>
00035 #include <mrpt/slam/CObservation2DRangeScan.h>
00036 #include <mrpt/slam/CPointsMap.h>
00037 #include <mrpt/core.h>
00038 #include <mrpt/math/geometry.h>
00039 namespace mrpt {
00040 namespace opengl {
00041 using namespace mrpt::utils;
00042 using namespace mrpt::slam;
00043 using namespace mrpt::poses;
00044 class MRPTDLLIMPEXP CAngularObservationMesh;
00045 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(CAngularObservationMesh,CRenderizable)
00046
00047
00048
00049
00050
00051
00052 class MRPTDLLIMPEXP CAngularObservationMesh:public CRenderizable {
00053 DEFINE_SERIALIZABLE(CAngularObservationMesh)
00054 public:
00055
00056
00057
00058 struct MRPTDLLIMPEXP TDoubleRange {
00059 private:
00060
00061
00062
00063
00064
00065
00066 char rangeType;
00067
00068
00069
00070
00071 union rd {
00072 struct {
00073 double initial;
00074 double final;
00075 double increment;
00076 } mode0;
00077 struct {
00078 double initial;
00079 double final;
00080 size_t amount;
00081 } mode1;
00082 struct {
00083 double aperture;
00084 size_t amount;
00085 bool negToPos;
00086 } mode2;
00087 } rangeData;
00088
00089
00090
00091 TDoubleRange(double a,double b,double c):rangeType(0) {
00092 rangeData.mode0.initial=a;
00093 rangeData.mode0.final=b;
00094 rangeData.mode0.increment=c;
00095 }
00096
00097
00098
00099 TDoubleRange(double a,double b,size_t c):rangeType(1) {
00100 rangeData.mode1.initial=a;
00101 rangeData.mode1.final=b;
00102 rangeData.mode1.amount=c;
00103 }
00104
00105
00106
00107 TDoubleRange(double a,size_t b,bool c):rangeType(2) {
00108 rangeData.mode2.aperture=a;
00109 rangeData.mode2.amount=b;
00110 rangeData.mode2.negToPos=c;
00111 }
00112 public:
00113
00114
00115
00116
00117 inline static TDoubleRange CreateFromIncrement(double initial,double final,double increment) {
00118 if (increment==0) throw std::logic_error("Invalid increment value.");
00119 return TDoubleRange(initial,final,increment);
00120 }
00121
00122
00123
00124 inline static TDoubleRange CreateFromAmount(double initial,double final,size_t amount) {
00125 return TDoubleRange(initial,final,amount);
00126 }
00127
00128
00129
00130 inline static TDoubleRange CreateFromAperture(double aperture,size_t amount,bool negToPos=true) {
00131 return TDoubleRange(aperture,amount,negToPos);
00132 }
00133
00134
00135
00136
00137 inline double aperture() const {
00138 switch (rangeType) {
00139 case 0:return (sign(rangeData.mode0.increment)==sign(rangeData.mode0.final-rangeData.mode0.initial))?fabs(rangeData.mode0.final-rangeData.mode0.initial):0;
00140 case 1:return rangeData.mode1.final-rangeData.mode1.initial;
00141 case 2:return rangeData.mode2.aperture;
00142 default:throw std::logic_error("Unknown range type.");
00143 }
00144 }
00145
00146
00147
00148
00149 inline double initialValue() const {
00150 switch (rangeType) {
00151 case 0:
00152 case 1:return rangeData.mode0.initial;
00153 case 2:return rangeData.mode2.negToPos?-rangeData.mode2.aperture/2:rangeData.mode2.aperture/2;
00154 default:throw std::logic_error("Unknown range type.");
00155 }
00156 }
00157
00158
00159
00160
00161 inline double finalValue() const {
00162 switch (rangeType) {
00163 case 0:return (sign(rangeData.mode0.increment)==sign(rangeData.mode0.final-rangeData.mode0.initial))?rangeData.mode0.final:rangeData.mode0.initial;
00164 case 1:return rangeData.mode1.final;
00165 case 2:return rangeData.mode2.negToPos?rangeData.mode2.aperture/2:-rangeData.mode2.aperture/2;
00166 default:throw std::logic_error("Unknown range type.");
00167 }
00168 }
00169
00170
00171
00172
00173 inline double increment() const {
00174 switch (rangeType) {
00175 case 0:return rangeData.mode0.increment;
00176 case 1:return (rangeData.mode1.final-rangeData.mode1.initial)/static_cast<double>(rangeData.mode1.amount-1);
00177 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);
00178 default:throw std::logic_error("Unknown range type.");
00179 }
00180 }
00181
00182
00183
00184
00185 inline size_t amount() const {
00186 switch (rangeType) {
00187 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;
00188 case 1:return rangeData.mode1.amount;
00189 case 2:return rangeData.mode2.amount;
00190 default:throw std::logic_error("Unknown range type.");
00191 }
00192 }
00193
00194
00195
00196
00197 void values(vector_double &vals) const;
00198
00199
00200
00201
00202 inline bool negToPos() const {
00203 switch (rangeType) {
00204 case 0:return sign(rangeData.mode0.increment)>0;
00205 case 1:return sign(rangeData.mode1.final-rangeData.mode1.initial)>0;
00206 case 2:return rangeData.mode2.negToPos;
00207 default:throw std::logic_error("Unknown range type.");
00208 }
00209 }
00210 };
00211 protected:
00212
00213
00214
00215 void updateMesh() const;
00216
00217
00218
00219 virtual ~CAngularObservationMesh() {}
00220
00221
00222
00223 mutable std::vector<CSetOfTriangles::TTriangle> triangles;
00224
00225
00226
00227 void addTriangle(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) const;
00228
00229
00230
00231 bool mWireframe;
00232
00233
00234
00235 mutable bool meshUpToDate;
00236
00237
00238
00239 bool mEnableTransparency;
00240
00241
00242
00243 mutable mrpt::math::CMatrixTemplate<TPoint3D> actualMesh;
00244
00245
00246
00247 mutable mrpt::math::CMatrixB validityMatrix;
00248
00249
00250
00251 vector<double> pitchBounds;
00252
00253
00254
00255 vector<CObservation2DRangeScan> scanSet;
00256
00257
00258
00259 CAngularObservationMesh():mWireframe(true),meshUpToDate(false),mEnableTransparency(true),actualMesh(0,0),validityMatrix(0,0),pitchBounds(),scanSet() {}
00260 public:
00261
00262
00263
00264 inline bool isWireframe() const {
00265 return mWireframe;
00266 }
00267
00268
00269
00270 inline void setWireframe(bool enabled=true) {
00271 mWireframe=enabled;
00272 }
00273
00274
00275
00276 inline bool isTransparencyEnabled() const {
00277 return mEnableTransparency;
00278 }
00279
00280
00281
00282 inline void enableTransparency(bool enabled=true) {
00283 mEnableTransparency=enabled;
00284 }
00285
00286
00287
00288
00289 virtual void render() const;
00290
00291
00292
00293
00294 virtual bool traceRay(const mrpt::poses::CPose3D &o,double &dist) const;
00295
00296
00297
00298 void setPitchBounds(const double initial,const double final);
00299
00300
00301
00302 void setPitchBounds(const std::vector<double> bounds);
00303
00304
00305
00306 void getPitchBounds(double &initial,double &final) const;
00307
00308
00309
00310 void getPitchBounds(std::vector<double> &bounds) const;
00311
00312
00313
00314 void getScanSet(std::vector<CObservation2DRangeScan> &scans) const;
00315
00316
00317
00318 bool setScanSet(const std::vector<CObservation2DRangeScan> &scans);
00319
00320
00321
00322
00323 void generateSetOfTriangles(CSetOfTrianglesPtr &res) const;
00324
00325
00326
00327 void generatePointCloud(CPointsMap *out_map) const;
00328
00329
00330
00331
00332 void getTracedRays(CSetOfLinesPtr &res) const;
00333
00334
00335
00336
00337 void getUntracedRays(CSetOfLinesPtr &res,double dist) const;
00338
00339
00340
00341
00342 void generateSetOfTriangles(std::vector<TPolygon3D> &res) const;
00343 private:
00344
00345
00346
00347 template<class T> class FTrace1D {
00348 protected:
00349 const CPose3D &initial;
00350 const T &e;
00351 vector<double> &values;
00352 std::vector<char> &valid;
00353 public:
00354 FTrace1D(const T &s,const CPose3D &p,vector_double &v,std::vector<char> &v2):initial(p),e(s),values(v),valid(v2) {}
00355 void operator()(double yaw) {
00356 double dist;
00357 const CPose3D pNew=initial+CPose3D(0.0,0.0,0.0,yaw,0.0,0.0);
00358 if (e->traceRay(pNew,dist)) {
00359 values.push_back(dist);
00360 valid.push_back(1);
00361 } else {
00362 values.push_back(0);
00363 valid.push_back(0);
00364 }
00365 }
00366 };
00367
00368
00369
00370 template<class T> class FTrace2D {
00371 protected:
00372 const T &e;
00373 const CPose3D &initial;
00374 CAngularObservationMeshPtr &caom;
00375 const CAngularObservationMesh::TDoubleRange &yaws;
00376 std::vector<CObservation2DRangeScan> &vObs;
00377 const CPose3D &pBase;
00378 public:
00379 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) {}
00380 void operator()(double pitch) {
00381 vector_double yValues;
00382 yaws.values(yValues);
00383 CObservation2DRangeScan o=CObservation2DRangeScan();
00384 const CPose3D pNew=initial+CPose3D(0,0,0,0,pitch,0);
00385 vector_double values;
00386 std::vector<char> valid;
00387 size_t nY=yValues.size();
00388 values.reserve(nY);
00389 valid.reserve(nY);
00390 for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,pNew,values,valid));
00391 o.aperture=yaws.aperture();
00392 o.rightToLeft=yaws.negToPos();
00393 o.maxRange=10000;
00394 o.sensorPose=pNew;
00395 o.deltaPitch=0;
00396 mrpt::utils::copy_container_typecasting(values,o.scan);
00397 o.validRange=valid;
00398 vObs.push_back(o);
00399 }
00400 };
00401 public:
00402
00403
00404
00405
00406
00407 template<class T> static void trace2DSetOfRays(const T &e,const CPose3D &initial,CAngularObservationMeshPtr &caom,const TDoubleRange &pitchs,const TDoubleRange &yaws) {
00408 vector_double pValues;
00409 pitchs.values(pValues);
00410 std::vector<CObservation2DRangeScan> vObs;
00411 vObs.reserve(pValues.size());
00412 for_each(pValues.begin(),pValues.end(),FTrace2D<T>(e,initial,caom,yaws,vObs,initial));
00413 caom->mWireframe=false;
00414 caom->mEnableTransparency=false;
00415 caom->setPitchBounds(pValues);
00416 caom->setScanSet(vObs);
00417 }
00418
00419
00420
00421
00422
00423 template<class T> static void trace1DSetOfRays(const T &e,const CPose3D &initial,CObservation2DRangeScan &obs,const TDoubleRange &yaws) {
00424 vector_double yValues;
00425 yaws.values(yValues);
00426 vector_double scanValues;
00427 std::vector<char> valid;
00428 size_t nV=yaws.amount();
00429 scanValues.reserve(nV);
00430 valid.reserve(nV);
00431 for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,initial,scanValues,valid));
00432 obs.aperture=yaws.aperture();
00433 obs.rightToLeft=yaws.negToPos();
00434 obs.maxRange=10000;
00435 obs.sensorPose=initial;
00436 obs.deltaPitch=0;
00437 obs.scan=scanValues;
00438 obs.validRange=valid;
00439 }
00440 };
00441 }
00442 }
00443 #endif