point_representation.h

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: point_representation.h 3006 2011-10-31 23:25:06Z rusu $
00037  *
00038  */
00039 #ifndef PCL_POINT_REPRESENTATION_H_
00040 #define PCL_POINT_REPRESENTATION_H_
00041 
00042 #include "pcl/point_types.h"
00043 #include "pcl/win32_macros.h"
00044 #include "pcl/ros/for_each_type.h"
00045 
00046 namespace pcl
00047 {
00054   template <typename PointT> 
00055   class PointRepresentation
00056   {
00057     protected:
00059       int nr_dimensions_;
00061       std::vector<float> alpha_;
00062       
00063     public:
00064       typedef boost::shared_ptr<PointRepresentation<PointT> > Ptr;
00065       typedef boost::shared_ptr<const PointRepresentation<PointT> > ConstPtr;
00066 
00068       PointRepresentation () : nr_dimensions_ (0), alpha_ (0) {}
00069       
00074       virtual void copyToFloatArray (const PointT &p, float *out) const = 0;
00075       
00079       virtual bool 
00080       isValid (const PointT &p) const
00081       {
00082         float *temp = new float[nr_dimensions_];
00083         copyToFloatArray (p, temp);
00084         bool is_valid = true;
00085         for (int i = 0; i < nr_dimensions_; ++i)
00086         {
00087           if (!pcl_isfinite (temp[i]))
00088           {
00089             is_valid = false;
00090             break;
00091           }
00092         }
00093         delete [] temp;
00094         return (is_valid);
00095       }
00096       
00101       template <typename OutputType> void
00102       vectorize (const PointT &p, OutputType &out) const
00103       {
00104         float *temp = new float[nr_dimensions_];
00105         copyToFloatArray (p, temp);
00106         if (alpha_.empty ())
00107         {
00108           for (int i = 0; i < nr_dimensions_; ++i)
00109             out[i] = temp[i];
00110         }
00111         else
00112         {
00113           for (int i = 0; i < nr_dimensions_; ++i)
00114             out[i] = temp[i] * alpha_[i];
00115         }
00116         delete [] temp;
00117       }
00118       
00122       void 
00123       setRescaleValues (const float *rescale_array)
00124       {
00125         alpha_.resize (nr_dimensions_);
00126         for (int i = 0; i < nr_dimensions_; ++i)
00127           alpha_[i] = rescale_array[i];
00128       }
00129       
00131       inline int getNumberOfDimensions () const { return (nr_dimensions_); }
00132   };
00133 
00135 
00137   template <typename PointDefault>
00138   class DefaultPointRepresentation : public PointRepresentation <PointDefault>
00139   {
00140     using PointRepresentation <PointDefault>::nr_dimensions_;
00141 
00142     public:
00143       // Boost shared pointers
00144       typedef boost::shared_ptr<DefaultPointRepresentation<PointDefault> > Ptr;
00145       typedef boost::shared_ptr<const DefaultPointRepresentation<PointDefault> > ConstPtr;
00146 
00147       DefaultPointRepresentation ()
00148       {
00149         // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
00150         nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
00151         // Limit the default representation to the first 3 elements
00152         if (nr_dimensions_ > 3) nr_dimensions_ = 3;
00153       }
00154 
00155       inline Ptr 
00156       makeShared () const 
00157       { 
00158         return (Ptr (new DefaultPointRepresentation<PointDefault> (*this)));
00159       }
00160 
00161       virtual void 
00162       copyToFloatArray (const PointDefault &p, float * out) const
00163       {
00164         // If point type is unknown, treat it as a struct/array of floats
00165         const float * ptr = (float *)&p;
00166         for (int i = 0; i < nr_dimensions_; ++i)
00167           out[i] = ptr[i];
00168       }
00169   };
00170 
00172 
00175   template <typename PointDefault>
00176   class DefaultFeatureRepresentation : public PointRepresentation <PointDefault>
00177   {
00178     protected:
00179       using PointRepresentation <PointDefault>::nr_dimensions_;
00180 
00181     private:
00182       struct IncrementFunctor
00183       {
00184         IncrementFunctor (int &n) : n_ (n)
00185         {
00186           n_ = 0;
00187         }
00188         
00189         template<typename Key> inline void operator () ()
00190         {
00191           n_ += pcl::traits::datatype<PointDefault, Key>::size;
00192         }
00193         
00194       private:
00195         int &n_;
00196       };
00197 
00198     struct NdCopyPointFunctor
00199     {
00200       typedef typename traits::POD<PointDefault>::type Pod;
00201       
00202       NdCopyPointFunctor (const PointDefault &p1, float * p2)
00203         : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
00204       
00205       template<typename Key> inline void operator() ()
00206       {  
00207         typedef typename pcl::traits::datatype<PointDefault, Key>::type FieldT;
00208         const int NrDims = pcl::traits::datatype<PointDefault, Key>::size;
00209         Helper<Key, FieldT, NrDims>::copyPoint (p1_, p2_, f_idx_);
00210       }
00211       
00212       // Copy helper for scalar fields
00213       template <typename Key, typename FieldT, int NrDims>
00214       struct Helper
00215       {
00216         static void copyPoint (const Pod &p1, float * p2, int &f_idx) 
00217         {
00218           const uint8_t * data_ptr = reinterpret_cast<const uint8_t *> (&p1) + 
00219             pcl::traits::offset<PointDefault, Key>::value;
00220           p2[f_idx++] = *reinterpret_cast<const FieldT*> (data_ptr);
00221         }
00222       };
00223       // Copy helper for array fields
00224       template <typename Key, typename FieldT, int NrDims>
00225       struct Helper<Key, FieldT[NrDims], NrDims>
00226       {
00227         static void copyPoint (const Pod &p1, float * p2, int &f_idx)
00228         {
00229           const uint8_t * data_ptr = reinterpret_cast<const uint8_t *> (&p1) + 
00230             pcl::traits::offset<PointDefault, Key>::value;
00231           int nr_dims = NrDims;
00232           const FieldT * array = reinterpret_cast<const FieldT *> (data_ptr);
00233           for (int i = 0; i < nr_dims; ++i)
00234           {
00235             p2[f_idx++] = array[i];
00236           }
00237         }
00238       };
00239   
00240     private:
00241       const Pod &p1_;
00242       float * p2_;
00243       int f_idx_;
00244     };
00245 
00246     public:
00247       // Boost shared pointers
00248       typedef typename boost::shared_ptr<DefaultFeatureRepresentation<PointDefault> > Ptr;
00249       typedef typename boost::shared_ptr<const DefaultFeatureRepresentation<PointDefault> > ConstPtr;
00250       typedef typename pcl::traits::fieldList<PointDefault>::type FieldList;
00251 
00252       DefaultFeatureRepresentation ()
00253       {      
00254         nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented
00255         pcl::for_each_type <FieldList> (IncrementFunctor (nr_dimensions_));
00256       }
00257 
00258       inline Ptr 
00259       makeShared () const 
00260       { 
00261         return (Ptr (new DefaultFeatureRepresentation<PointDefault> (*this)));
00262       }
00263 
00264       virtual void 
00265       copyToFloatArray (const PointDefault &p, float * out) const
00266       {
00267         pcl::for_each_type <FieldList> (NdCopyPointFunctor (p, out));
00268       }
00269   };
00270 
00272   template <>
00273   class DefaultPointRepresentation <PointXYZ> : public  PointRepresentation <PointXYZ>
00274   {
00275     public:
00276       DefaultPointRepresentation ()
00277       {
00278         nr_dimensions_ = 3;
00279       }
00280 
00281       virtual void 
00282       copyToFloatArray (const PointXYZ &p, float * out) const
00283       {
00284         out[0] = p.x;
00285         out[1] = p.y;
00286         out[2] = p.z;
00287       }
00288   };
00289 
00291   template <>
00292   class DefaultPointRepresentation <PointXYZI> : public  PointRepresentation <PointXYZI>
00293   {
00294     public:
00295       DefaultPointRepresentation ()
00296       {
00297         nr_dimensions_ = 3;
00298       }
00299 
00300       virtual void 
00301       copyToFloatArray (const PointXYZI &p, float * out) const
00302       {
00303         out[0] = p.x;
00304         out[1] = p.y;
00305         out[2] = p.z;
00306         // By default, p.intensity is not part of the PointXYZI vectorization
00307       }
00308   };
00309 
00311   template <>
00312   class DefaultPointRepresentation <PointNormal> : public  PointRepresentation <PointNormal>
00313   {
00314     public:
00315       DefaultPointRepresentation ()
00316       {
00317         nr_dimensions_ = 3;
00318       }
00319 
00320       virtual void 
00321       copyToFloatArray (const PointNormal &p, float * out) const
00322       {
00323         out[0] = p.x;
00324         out[1] = p.y;
00325         out[2] = p.z;
00326       }
00327   };
00328 
00330   template <>
00331   class DefaultPointRepresentation <PFHSignature125> : public DefaultFeatureRepresentation <PFHSignature125>
00332   {};
00333 
00335   template <>
00336   class DefaultPointRepresentation <PFHRGBSignature250> : public DefaultFeatureRepresentation <PFHRGBSignature250>
00337   {};
00338 
00340   template <>
00341   class DefaultPointRepresentation <PPFSignature> : public DefaultFeatureRepresentation <PPFSignature>
00342   {
00343     public:
00344       DefaultPointRepresentation ()
00345       {
00346         nr_dimensions_ = 4;
00347       }
00348 
00349       virtual void
00350       copyToFloatArray (const PPFSignature &p, float * out) const
00351       {
00352         out[0] = p.f1;
00353         out[1] = p.f2;
00354         out[2] = p.f3;
00355         out[3] = p.f4;
00356       }
00357   };
00358 
00360   template <>
00361   class DefaultPointRepresentation <FPFHSignature33> : public DefaultFeatureRepresentation <FPFHSignature33>
00362   {};
00363 
00365   template <>
00366   class DefaultPointRepresentation <VFHSignature308> : public DefaultFeatureRepresentation <VFHSignature308>
00367   {};
00368 
00370   template <>
00371   class DefaultPointRepresentation <NormalBasedSignature12> : 
00372     public DefaultFeatureRepresentation <NormalBasedSignature12>
00373   {};
00374 
00376   template <>
00377   class DefaultPointRepresentation<SHOT> : public PointRepresentation<SHOT>
00378   {
00379     public:
00380       DefaultPointRepresentation ()
00381       {
00382         nr_dimensions_ = 352;
00383       }
00384 
00385       virtual void
00386       copyToFloatArray (const SHOT &p, float * out) const
00387       {
00388         for (int i = 0; i < nr_dimensions_; ++i)
00389           out[i] = p.descriptor[i];
00390       }
00391   };
00392 
00393 
00395 
00397   template <typename PointDefault>
00398   class CustomPointRepresentation : public PointRepresentation <PointDefault>
00399   {
00400     using PointRepresentation <PointDefault>::nr_dimensions_;
00401 
00402     public:
00403       // Boost shared pointers
00404       typedef boost::shared_ptr<CustomPointRepresentation<PointDefault> > Ptr;
00405       typedef boost::shared_ptr<const CustomPointRepresentation<PointDefault> > ConstPtr;
00406 
00411       CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0) 
00412         : max_dim_(max_dim), start_dim_(start_dim)
00413       {
00414         // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
00415         nr_dimensions_ = sizeof (PointDefault) / sizeof (float) - start_dim_;
00416         // Limit the default representation to the first 3 elements
00417         if (nr_dimensions_ > max_dim_) 
00418           nr_dimensions_ = max_dim_;
00419       }
00420 
00421       inline Ptr 
00422       makeShared () const 
00423       { 
00424         return Ptr (new CustomPointRepresentation<PointDefault> (*this)); 
00425       }
00426 
00431       virtual void
00432       copyToFloatArray (const PointDefault &p, float *out) const
00433       {
00434         // If point type is unknown, treat it as a struct/array of floats
00435         const float *ptr = ((float*)&p) + start_dim_;
00436         for (int i = 0; i < nr_dimensions_; ++i)
00437           out[i] = ptr[i];
00438       }
00439 
00440     protected:
00442       int max_dim_;
00444       int start_dim_;
00445   };
00446 }
00447 
00448 #endif // #ifndef PCL_POINT_REPRESENTATION_H_