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