Point Cloud Library (PCL)
1.3.1
|
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_