Main MRPT website > C++ reference
MRPT logo

CAngularObservationMesh.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
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           * A mesh built from a set of 2D laser scan observations.
00052           * Each element of this set is a single scan through the yaw, given a specific pitch.
00053           * Each scan has a CPose3D identifying the origin of the scan, which ideally is the
00054           * same for every one of them.
00055           */
00056         class MAPS_IMPEXP CAngularObservationMesh:public CRenderizableDisplayList       {
00057                 DEFINE_SERIALIZABLE(CAngularObservationMesh)
00058         public:
00059                 /**
00060                   * Range specification type, with several uses.
00061                   */
00062                 struct MAPS_IMPEXP TDoubleRange {
00063                 private:
00064                         /**
00065                           * Range type. 
00066                           * If 0, it's specified by an initial and a final value, and an increment.
00067                           * If 1, it's specified by an initial and a final value, and a fixed size of samples.
00068                           * If 2, it's specified by an aperture, a fixed size of samples and a boolean variable controlling direction. This type is always zero-centered.
00069                           */
00070                         char rangeType;
00071                         /**
00072                           * Union type with the actual data.
00073                           * \sa rangeType
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                           * Constructor from initial value, final value and range.
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                           * Constructor from initial value, final value and amount of samples.
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                           * Constructor from aperture, amount of samples and scan direction.
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                           * Creates a range of values from the initial value, the final value and the increment.
00119                           * \throw std::logic_error if the increment is zero.
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                           * Creates a range of values from the initial value, the final value and a desired amount of samples.
00127                           */
00128                         inline static TDoubleRange CreateFromAmount(double initial,double final,size_t amount)  {
00129                                 return TDoubleRange(initial,final,amount);
00130                         }
00131                         /**
00132                           * Creates a zero-centered range of values from an aperture, an amount of samples and a direction.
00133                           */
00134                         inline static TDoubleRange CreateFromAperture(double aperture,size_t amount,bool negToPos=true) {
00135                                 return TDoubleRange(aperture,amount,negToPos);
00136                         }
00137                         /**
00138                           * Returns the total aperture of the range.
00139                           * \throw std::logic_error on invalid range type.
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                           * Returns the first value of the range.
00151                           * \throw std::logic_error on invalid range type.
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                           * Returns the last value of the range.
00163                           * \throw std::logic_error on invalid range type.
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                           * Returns the increment between two consecutive values of the range.
00175                           * \throw std::logic_error on invalid range type.
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                           * Returns the total amount of values in this range.
00187                           * \throw std::logic_error on invalid range type.
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                           * Gets a vector with every value in the range.
00199                           * \throw std::logic_error on invalid range type.
00200                           */
00201                         void values(std::vector<double> &vals) const;
00202                         /**
00203                           * Returns the direction of the scan. True if the increment is positive, false otherwise.
00204                           * \throw std::logic_error on invalid range type.
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                   * Updates the mesh, if needed. It's a const method, but modifies mutable content.
00218                   */
00219                 void updateMesh() const;
00220                 /**
00221                   * Empty destructor.
00222                   */
00223                 virtual ~CAngularObservationMesh()      {}
00224                 /**
00225                   * Actual set of triangles to be displayed.
00226                   */
00227                 mutable std::vector<CSetOfTriangles::TTriangle> triangles;
00228                 /**
00229                   * Internal method to add a triangle to the mutable mesh.
00230                   */
00231                 void addTriangle(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) const;
00232                 /**
00233                   * Whether the mesh will be displayed wireframe or solid.
00234                   */
00235                 bool mWireframe;
00236                 /**
00237                   * Mutable variable which controls if the object has suffered any change since last time the mesh was updated.
00238                   */
00239                 mutable bool meshUpToDate;
00240                 /**
00241                   * Whether the object may present transparencies or not.
00242                   */
00243                 bool mEnableTransparency;
00244                 /**
00245                   * Mutable object with the mesh's points.
00246                   */
00247                 mutable mrpt::math::CMatrixTemplate<TPoint3D> actualMesh;
00248                 /**
00249                   * Scan validity matrix.
00250                   */
00251                 mutable mrpt::math::CMatrixB validityMatrix;
00252                 /**
00253                   * Observation pitch range. When containing exactly two elements, they represent the bounds.
00254                   */
00255                 std::vector<double> pitchBounds;
00256                 /**
00257                   * Actual scan set which is used to generate the mesh.
00258                   */
00259                 std::vector<CObservation2DRangeScan> scanSet;
00260                 /**
00261                   * Basic constructor.
00262                   */
00263                 CAngularObservationMesh():mWireframe(true),meshUpToDate(false),mEnableTransparency(true),actualMesh(0,0),validityMatrix(0,0),pitchBounds(),scanSet()    {}
00264         public:
00265                 /**
00266                   * Returns whether the object is configured as wireframe or solid.
00267                   */
00268                 inline bool isWireframe() const {
00269                         return mWireframe;
00270                 }
00271                 /**
00272                   * Sets the display mode for the object. True=wireframe, False=solid.
00273                   */
00274                 inline void setWireframe(bool enabled=true)     {
00275                         mWireframe=enabled;
00276                         CRenderizableDisplayList::notifyChange();
00277                 }
00278                 /**
00279                   * Returns whether the object may be transparent or not.
00280                   */
00281                 inline bool isTransparencyEnabled() const       {
00282                         return mEnableTransparency;
00283                 }
00284                 /**
00285                   * Enables or disables transparencies.
00286                   */
00287                 inline void enableTransparency(bool enabled=true)       {
00288                         mEnableTransparency=enabled;
00289                         CRenderizableDisplayList::notifyChange();
00290                 }
00291                 /**
00292                   * Renderizes the object.
00293                   * \sa mrpt::opengl::CRenderizable
00294                   */
00295                 virtual void render_dl() const;
00296                 /**
00297                   * Traces a ray to the object, returning the distance to a given pose through its X axis.
00298                   * \sa mrpt::opengl::CRenderizable,trace2DSetOfRays,trace1DSetOfRays
00299                   */
00300                 virtual bool traceRay(const mrpt::poses::CPose3D &o,double &dist) const;
00301                 /**
00302                   * Sets the pitch bounds for this range.
00303                   */
00304                 void setPitchBounds(const double initial,const double final);
00305                 /**
00306                   * Sets the pitch bounds for this range.
00307                   */
00308                 void setPitchBounds(const std::vector<double> bounds);
00309                 /**
00310                   * Gets the initial and final pitch bounds for this range.
00311                   */
00312                 void getPitchBounds(double &initial,double &final) const;
00313                 /**
00314                   * Gets the pitch bounds for this range.
00315                   */
00316                 void getPitchBounds(std::vector<double> &bounds) const;
00317                 /**
00318                   * Gets the scan set.
00319                   */
00320                 void getScanSet(std::vector<CObservation2DRangeScan> &scans) const;
00321                 /**
00322                   * Sets the scan set.
00323                   */
00324                 bool setScanSet(const std::vector<CObservation2DRangeScan> &scans);
00325                 /**
00326                   * Gets the mesh as a set of triangles, for displaying them.
00327                   * \sa generateSetOfTriangles(std::vector<TPolygon3D> &),mrpt::opengl::CSetOfTriangles,mrpt::opengl::CSetOfTriangles::TTriangle
00328                   */
00329                 void generateSetOfTriangles(CSetOfTrianglesPtr &res) const;
00330                 /**
00331                   * Returns the scanned points as a 3D point cloud. The target pointmap must be passed as a pointer to allow the use of any derived class.
00332                   */
00333                 void generatePointCloud(CPointsMap *out_map) const;
00334                 /**
00335                   * Gets a set of lines containing the traced rays, for displaying them.
00336                   * \sa getUntracedRays,mrpt::opengl::CSetOfLines
00337                   */
00338                 void getTracedRays(CSetOfLinesPtr &res) const;
00339                 /**
00340                   * Gets a set of lines containing the untraced rays, up to a specified distance, for displaying them.
00341                   * \sa getTracedRays,mrpt::opengl::CSetOfLines
00342                   */
00343                 void getUntracedRays(CSetOfLinesPtr &res,double dist) const;
00344                 /**
00345                   * Gets the mesh as a set of polygons, to work with them.
00346                   * \sa generateSetOfTriangles(mrpt::opengl::CSetOfTriangles &)
00347                   */
00348                 void generateSetOfTriangles(std::vector<TPolygon3D> &res) const;
00349                 /**
00350                   * Retrieves the full mesh, along with the validity matrix.
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                   * Internal functor class to trace a ray.
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                   * Internal functor class to trace a set of rays.
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                   * 2D ray tracing (will generate a 3D mesh). Given an object and two ranges, realizes a scan from the initial pose and stores it in a CAngularObservationMesh object.
00418                   * The objective may be a COpenGLScene, a CRenderizable or any children of its.
00419                   * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
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                   * 2D ray tracing (will generate a vectorial mesh inside a plane). Given an object and a range, realizes a scan from the initial pose and stores it in a CObservation2DRangeScan object.
00434                   * The objective may be a COpenGLScene, a CRenderizable or any children of its.
00435                   * \sa mrpt::opengl::CRenderizable,mrpt::opengl::COpenGLScene.
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



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011