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 namespace mrpt {
00039 namespace opengl {
00040 using namespace mrpt::utils;
00041 using namespace mrpt::slam;
00042 using namespace mrpt::poses;
00043 class MRPTDLLIMPEXP CAngularObservationMesh;
00044 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(CAngularObservationMesh,CRenderizable)
00050 class MRPTDLLIMPEXP CAngularObservationMesh:public CRenderizable {
00051 DEFINE_SERIALIZABLE(CAngularObservationMesh)
00052 public:
00053 struct TFloatRange {
00054 private:
00055 char rangeType;
00056 union rd {
00057 struct {
00058 float initial;
00059 float final;
00060 float increment;
00061 } mode0;
00062 struct {
00063 float initial;
00064 float final;
00065 size_t amount;
00066 } mode1;
00067 struct {
00068 float aperture;
00069 size_t amount;
00070 bool negToPos;
00071 } mode2;
00072 } rangeData;
00073 TFloatRange(float a,float b,float c):rangeType(0) {
00074 rangeData.mode0.initial=a;
00075 rangeData.mode0.final=b;
00076 rangeData.mode0.increment=c;
00077 }
00078 TFloatRange(float a,float b,size_t c):rangeType(1) {
00079 rangeData.mode1.initial=a;
00080 rangeData.mode1.final=b;
00081 rangeData.mode1.amount=c;
00082 }
00083 TFloatRange(float a,size_t b,bool c):rangeType(2) {
00084 rangeData.mode2.aperture=a;
00085 rangeData.mode2.amount=b;
00086 rangeData.mode2.negToPos=c;
00087 }
00088 public:
00089 inline static TFloatRange CreateFromIncrement(float initial,float final,float increment) {
00090 if (increment==0) throw std::logic_error("Invalid increment value.");
00091 return TFloatRange(initial,final,increment);
00092 }
00093 inline static TFloatRange CreateFromAmount(float initial,float final,size_t amount) {
00094 return TFloatRange(initial,final,amount);
00095 }
00096 inline static TFloatRange CreateFromAperture(float aperture,size_t amount,bool negToPos=true) {
00097 return TFloatRange(aperture,amount,negToPos);
00098 }
00099 inline float aperture() const {
00100 switch (rangeType) {
00101 case 0:return (sign(rangeData.mode0.increment)==sign(rangeData.mode0.final-rangeData.mode0.initial))?fabs(rangeData.mode0.final-rangeData.mode0.initial):0;
00102 case 1:return rangeData.mode1.final-rangeData.mode1.initial;
00103 case 2:return rangeData.mode2.aperture;
00104 default:throw std::logic_error("Unknown range type.");
00105 }
00106 }
00107 inline float initialValue() const {
00108 switch (rangeType) {
00109 case 0:
00110 case 1:return rangeData.mode0.initial;
00111 case 2:return rangeData.mode2.negToPos?-rangeData.mode2.aperture/2:rangeData.mode2.aperture/2;
00112 default:throw std::logic_error("Unknown range type.");
00113 }
00114 }
00115 inline float finalValue() const {
00116 switch (rangeType) {
00117 case 0:return (sign(rangeData.mode0.increment)==sign(rangeData.mode0.final-rangeData.mode0.initial))?rangeData.mode0.final:rangeData.mode0.initial;
00118 case 1:return rangeData.mode1.final;
00119 case 2:return rangeData.mode2.negToPos?rangeData.mode2.aperture/2:-rangeData.mode2.aperture/2;
00120 default:throw std::logic_error("Unknown range type.");
00121 }
00122 }
00123 inline float increment() const {
00124 switch (rangeType) {
00125 case 0:return rangeData.mode0.increment;
00126 case 1:return (rangeData.mode1.final-rangeData.mode1.initial)/static_cast<float>(rangeData.mode1.amount-1);
00127 case 2:return rangeData.mode2.negToPos?rangeData.mode2.aperture/static_cast<float>(rangeData.mode2.amount-1):-rangeData.mode2.aperture/static_cast<float>(rangeData.mode2.amount-1);
00128 default:throw std::logic_error("Unknown range type.");
00129 }
00130 }
00131 inline size_t amount() const {
00132 switch (rangeType) {
00133 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;
00134 case 1:return rangeData.mode1.amount;
00135 case 2:return rangeData.mode2.amount;
00136 default:throw std::logic_error("Unknown range type.");
00137 }
00138 }
00139 void values(vector_float &vals) const;
00140 inline bool negToPos() const {
00141 switch (rangeType) {
00142 case 0:return sign(rangeData.mode0.increment)>0;
00143 case 1:return sign(rangeData.mode1.final-rangeData.mode1.initial)>0;
00144 case 2:return rangeData.mode2.negToPos;
00145 default:throw std::logic_error("Unknown range type.");
00146 }
00147 }
00148 };
00149 protected:
00150 void updateMesh() const;
00151 virtual ~CAngularObservationMesh() {}
00152 mutable std::vector<CSetOfTriangles::TTriangle> triangles;
00153 void addTriangle(const CPoint3D &p1,const CPoint3D &p2,const CPoint3D &p3) const;
00154 bool mWireframe;
00155 mutable bool meshUpToDate;
00156 bool mEnableTransparency;
00157 mutable mrpt::math::CMatrixTemplate<CPoint3D> actualMesh;
00158 mutable mrpt::math::CMatrixB validityMatrix;
00159 vector_serializable<float> pitchBounds;
00160 vector_serializable<CObservation2DRangeScan> scanSet;
00161 CAngularObservationMesh():mWireframe(true),meshUpToDate(false),mEnableTransparency(true),actualMesh(0,0),validityMatrix(0,0),pitchBounds(),scanSet() {}
00162 public:
00163 inline bool isWireframe() const {
00164 return mWireframe;
00165 }
00166 inline void setWireframe(bool enabled) {
00167 mWireframe=enabled;
00168 }
00169 inline bool isTransparencyEnabled() const {
00170 return mEnableTransparency;
00171 }
00172 inline void enableTransparency(bool enabled=true) {
00173 mEnableTransparency=enabled;
00174 }
00175 virtual void render() const;
00176 virtual bool traceRay(const mrpt::poses::CPose3D &o,float &dist) const;
00177 void setPitchBounds(const float initial,const float final);
00178 void setPitchBounds(const std::vector<float> bounds);
00179 void getPitchBounds(float &initial,float &final) const;
00180 void getPitchBounds(std::vector<float> &bounds) const;
00181 void getScanSet(std::vector<CObservation2DRangeScan> &scans) const;
00182 bool setScanSet(const std::vector<CObservation2DRangeScan> scans);
00183 inline static CAngularObservationMeshPtr Create() {
00184 return CAngularObservationMeshPtr(new CAngularObservationMesh());
00185 }
00186 void generateSetOfTriangles(CSetOfTrianglesPtr &res) const;
00187 void generatePointCloud(CPointsMap *out_map) const;
00188 void getTracedRays(CSetOfLinesPtr &res) const;
00189 void getUntracedRays(CSetOfLinesPtr &res,float dist) const;
00190 private:
00191 template<class T>
00192 class FTrace1D {
00193 protected:
00194 const CPose3D &initial;
00195 const T &e;
00196 vector_float &values;
00197 std::vector<char> &valid;
00198 public:
00199 FTrace1D(const T &s,const CPose3D &p,vector_float &v,std::vector<char> &v2):initial(p),e(s),values(v),valid(v2) {}
00200 void operator()(float yaw) {
00201 float dist;
00202 const CPose3D pNew=initial+CPose3D(0.0,0.0,0.0,yaw,0.0,0.0);
00203 if (e->traceRay(pNew,dist)) {
00204 values.push_back(dist);
00205 valid.push_back(1);
00206 } else {
00207 values.push_back(0);
00208 valid.push_back(0);
00209 }
00210 }
00211 };
00212 template<class T>
00213 class FTrace2D {
00214 protected:
00215 const T &e;
00216 const CPose3D &initial;
00217 CAngularObservationMeshPtr &caom;
00218 const CAngularObservationMesh::TFloatRange &yaws;
00219 std::vector<CObservation2DRangeScan> &vObs;
00220 const CPose3D &pBase;
00221 public:
00222 FTrace2D(const T &s,const CPose3D &p,CAngularObservationMeshPtr &om,const CAngularObservationMesh::TFloatRange &y,std::vector<CObservation2DRangeScan> &obs,const CPose3D &b):e(s),initial(p),caom(om),yaws(y),vObs(obs),pBase(b) {}
00223 void operator()(float pitch) {
00224 vector_float yValues;
00225 yaws.values(yValues);
00226 CObservation2DRangeScan o;
00227 const CPose3D pNew=initial+CPose3D(0,0,0,0,pitch,0);
00228 vector_float values;
00229 std::vector<char> valid;
00230 size_t nY=yValues.size();
00231 values.reserve(nY);
00232 valid.reserve(nY);
00233 for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,pNew,values,valid));
00234 o.aperture=yaws.aperture();
00235 o.rightToLeft=yaws.negToPos();
00236 o.maxRange=10000;
00237 o.sensorPose=pNew;
00238 o.deltaPitch=0;
00239 o.scan=values;
00240 o.validRange=valid;
00241 vObs.push_back(o);
00242 }
00243 };
00244 public:
00245
00246 template<class T>
00247 static void trace2DSetOfRays(const T &e,const CPose3D &initial,CAngularObservationMeshPtr &caom,const TFloatRange &pitchs,const TFloatRange &yaws) {
00248 vector_float pValues;
00249 pitchs.values(pValues);
00250 std::vector<CObservation2DRangeScan> vObs;
00251 vObs.reserve(pValues.size());
00252 for_each(pValues.begin(),pValues.end(),FTrace2D<T>(e,initial,caom,yaws,vObs,initial));
00253 caom->mWireframe=false;
00254 caom->mEnableTransparency=false;
00255 caom->setPitchBounds(pValues);
00256 caom->setScanSet(vObs);
00257 }
00258 template<class T>
00259 static void trace1DSetOfRays(const T &e,const CPose3D &initial,CObservation2DRangeScan &obs,const TFloatRange &yaws) {
00260 vector_float yValues;
00261 yaws.values(yValues);
00262 vector_float scanValues;
00263 std::vector<char> valid;
00264 size_t nV=yaws.amount();
00265 scanValues.reserve(nV);
00266 valid.reserve(nV);
00267 for_each(yValues.begin(),yValues.end(),FTrace1D<T>(e,initial,scanValues,valid));
00268 obs.aperture=yaws.aperture();
00269 obs.rightToLeft=yaws.negToPos();
00270 obs.maxRange=10000;
00271 obs.sensorPose=initial;
00272 obs.deltaPitch=0;
00273 obs.scan=scanValues;
00274 obs.validRange=valid;
00275 }
00276 };
00277 }
00278 }
00279 #endif