Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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$ 00037 * 00038 */ 00039 #ifndef PCL_POINT_REPRESENTATION_H_ 00040 #define PCL_POINT_REPRESENTATION_H_ 00041 00042 #include <pcl/point_types.h> 00043 #include <pcl/pcl_macros.h> 00044 #include <pcl/for_each_type.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief @b PointRepresentation provides a set of methods for converting a point structs/object into an 00049 * n-dimensional vector. 00050 * \note This is an abstract class. Subclasses must set nr_dimensions_ to the appropriate value in the constructor 00051 * and provide an implemention of the pure virtual copyToFloatArray method. 00052 * \author Michael Dixon 00053 */ 00054 template <typename PointT> 00055 class PointRepresentation 00056 { 00057 protected: 00058 /** \brief The number of dimensions in this point's vector (i.e. the "k" in "k-D") */ 00059 int nr_dimensions_; 00060 /** \brief A vector containing the rescale factor to apply to each dimension. */ 00061 std::vector<float> alpha_; 00062 /** \brief Indicates whether this point representation is trivial. It is trivial if and only if the following 00063 * conditions hold: 00064 * - the relevant data consists only of float values 00065 * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array 00066 * - sizeof(PointT) is a multiple of sizeof(float) 00067 * In short, a trivial point representation converts the input point to a float array that is the same as if 00068 * the point was reinterpret_casted to a float array of length nr_dimensions_ . This value says that this 00069 * representation can be trivial; it is only trivial if setRescaleValues() has not been set. 00070 */ 00071 bool trivial_; 00072 00073 public: 00074 typedef boost::shared_ptr<PointRepresentation<PointT> > Ptr; 00075 typedef boost::shared_ptr<const PointRepresentation<PointT> > ConstPtr; 00076 00077 /** \brief Empty constructor */ 00078 PointRepresentation () : nr_dimensions_ (0), alpha_ (0), trivial_ (false) {} 00079 00080 /** \brief Empty destructor */ 00081 virtual ~PointRepresentation () {} 00082 00083 /** \brief Copy point data from input point to a float array. This method must be overriden in all subclasses. 00084 * \param[in] p The input point 00085 * \param[out] out A pointer to a float array. 00086 */ 00087 virtual void copyToFloatArray (const PointT &p, float *out) const = 0; 00088 00089 /** \brief Returns whether this point representation is trivial. It is trivial if and only if the following 00090 * conditions hold: 00091 * - the relevant data consists only of float values 00092 * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array 00093 * - sizeof(PointT) is a multiple of sizeof(float) 00094 * In short, a trivial point representation converts the input point to a float array that is the same as if 00095 * the point was reinterpret_casted to a float array of length nr_dimensions_ . */ 00096 inline bool isTrivial() const { return trivial_ && alpha_.empty (); } 00097 00098 /** \brief Verify that the input point is valid. 00099 * \param p The point to validate 00100 */ 00101 virtual bool 00102 isValid (const PointT &p) const 00103 { 00104 bool is_valid = true; 00105 00106 if (trivial_) 00107 { 00108 const float* temp = reinterpret_cast<const float*>(&p); 00109 00110 for (int i = 0; i < nr_dimensions_; ++i) 00111 { 00112 if (!pcl_isfinite (temp[i])) 00113 { 00114 is_valid = false; 00115 break; 00116 } 00117 } 00118 } 00119 else 00120 { 00121 float *temp = new float[nr_dimensions_]; 00122 copyToFloatArray (p, temp); 00123 00124 for (int i = 0; i < nr_dimensions_; ++i) 00125 { 00126 if (!pcl_isfinite (temp[i])) 00127 { 00128 is_valid = false; 00129 break; 00130 } 00131 } 00132 delete [] temp; 00133 } 00134 return (is_valid); 00135 } 00136 00137 /** \brief Convert input point into a vector representation, rescaling by \a alpha. 00138 * \param[in] p the input point 00139 * \param[out] out The output vector. Can be of any type that implements the [] operator. 00140 */ 00141 template <typename OutputType> void 00142 vectorize (const PointT &p, OutputType &out) const 00143 { 00144 float *temp = new float[nr_dimensions_]; 00145 copyToFloatArray (p, temp); 00146 if (alpha_.empty ()) 00147 { 00148 for (int i = 0; i < nr_dimensions_; ++i) 00149 out[i] = temp[i]; 00150 } 00151 else 00152 { 00153 for (int i = 0; i < nr_dimensions_; ++i) 00154 out[i] = temp[i] * alpha_[i]; 00155 } 00156 delete [] temp; 00157 } 00158 00159 /** \brief Set the rescale values to use when vectorizing points 00160 * \param[in] rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator. 00161 */ 00162 void 00163 setRescaleValues (const float *rescale_array) 00164 { 00165 alpha_.resize (nr_dimensions_); 00166 for (int i = 0; i < nr_dimensions_; ++i) 00167 alpha_[i] = rescale_array[i]; 00168 } 00169 00170 /** \brief Return the number of dimensions in the point's vector representation. */ 00171 inline int getNumberOfDimensions () const { return (nr_dimensions_); } 00172 }; 00173 00174 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00175 /** \brief @b DefaultPointRepresentation extends PointRepresentation to define default behavior for common point types. 00176 */ 00177 template <typename PointDefault> 00178 class DefaultPointRepresentation : public PointRepresentation <PointDefault> 00179 { 00180 using PointRepresentation <PointDefault>::nr_dimensions_; 00181 using PointRepresentation <PointDefault>::trivial_; 00182 00183 public: 00184 // Boost shared pointers 00185 typedef boost::shared_ptr<DefaultPointRepresentation<PointDefault> > Ptr; 00186 typedef boost::shared_ptr<const DefaultPointRepresentation<PointDefault> > ConstPtr; 00187 00188 DefaultPointRepresentation () 00189 { 00190 // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions 00191 nr_dimensions_ = sizeof (PointDefault) / sizeof (float); 00192 // Limit the default representation to the first 3 elements 00193 if (nr_dimensions_ > 3) nr_dimensions_ = 3; 00194 00195 trivial_ = true; 00196 } 00197 00198 virtual ~DefaultPointRepresentation () {} 00199 00200 inline Ptr 00201 makeShared () const 00202 { 00203 return (Ptr (new DefaultPointRepresentation<PointDefault> (*this))); 00204 } 00205 00206 virtual void 00207 copyToFloatArray (const PointDefault &p, float * out) const 00208 { 00209 // If point type is unknown, treat it as a struct/array of floats 00210 const float* ptr = reinterpret_cast<const float*> (&p); 00211 for (int i = 0; i < nr_dimensions_; ++i) 00212 out[i] = ptr[i]; 00213 } 00214 }; 00215 00216 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00217 /** \brief @b DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the 00218 * default behavior for feature descriptor types (i.e., copy each element of each field into a float array). 00219 */ 00220 template <typename PointDefault> 00221 class DefaultFeatureRepresentation : public PointRepresentation <PointDefault> 00222 { 00223 protected: 00224 using PointRepresentation <PointDefault>::nr_dimensions_; 00225 00226 private: 00227 struct IncrementFunctor 00228 { 00229 IncrementFunctor (int &n) : n_ (n) 00230 { 00231 n_ = 0; 00232 } 00233 00234 template<typename Key> inline void operator () () 00235 { 00236 n_ += pcl::traits::datatype<PointDefault, Key>::size; 00237 } 00238 00239 private: 00240 int &n_; 00241 }; 00242 00243 struct NdCopyPointFunctor 00244 { 00245 typedef typename traits::POD<PointDefault>::type Pod; 00246 00247 NdCopyPointFunctor (const PointDefault &p1, float * p2) 00248 : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { } 00249 00250 template<typename Key> inline void operator() () 00251 { 00252 typedef typename pcl::traits::datatype<PointDefault, Key>::type FieldT; 00253 const int NrDims = pcl::traits::datatype<PointDefault, Key>::size; 00254 Helper<Key, FieldT, NrDims>::copyPoint (p1_, p2_, f_idx_); 00255 } 00256 00257 // Copy helper for scalar fields 00258 template <typename Key, typename FieldT, int NrDims> 00259 struct Helper 00260 { 00261 static void copyPoint (const Pod &p1, float * p2, int &f_idx) 00262 { 00263 const uint8_t * data_ptr = reinterpret_cast<const uint8_t *> (&p1) + 00264 pcl::traits::offset<PointDefault, Key>::value; 00265 p2[f_idx++] = *reinterpret_cast<const FieldT*> (data_ptr); 00266 } 00267 }; 00268 // Copy helper for array fields 00269 template <typename Key, typename FieldT, int NrDims> 00270 struct Helper<Key, FieldT[NrDims], NrDims> 00271 { 00272 static void copyPoint (const Pod &p1, float * p2, int &f_idx) 00273 { 00274 const uint8_t * data_ptr = reinterpret_cast<const uint8_t *> (&p1) + 00275 pcl::traits::offset<PointDefault, Key>::value; 00276 int nr_dims = NrDims; 00277 const FieldT * array = reinterpret_cast<const FieldT *> (data_ptr); 00278 for (int i = 0; i < nr_dims; ++i) 00279 { 00280 p2[f_idx++] = array[i]; 00281 } 00282 } 00283 }; 00284 00285 private: 00286 const Pod &p1_; 00287 float * p2_; 00288 int f_idx_; 00289 }; 00290 00291 public: 00292 // Boost shared pointers 00293 typedef typename boost::shared_ptr<DefaultFeatureRepresentation<PointDefault> > Ptr; 00294 typedef typename boost::shared_ptr<const DefaultFeatureRepresentation<PointDefault> > ConstPtr; 00295 typedef typename pcl::traits::fieldList<PointDefault>::type FieldList; 00296 00297 DefaultFeatureRepresentation () 00298 { 00299 nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented 00300 pcl::for_each_type <FieldList> (IncrementFunctor (nr_dimensions_)); 00301 } 00302 00303 inline Ptr 00304 makeShared () const 00305 { 00306 return (Ptr (new DefaultFeatureRepresentation<PointDefault> (*this))); 00307 } 00308 00309 virtual void 00310 copyToFloatArray (const PointDefault &p, float * out) const 00311 { 00312 pcl::for_each_type <FieldList> (NdCopyPointFunctor (p, out)); 00313 } 00314 }; 00315 00316 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00317 template <> 00318 class DefaultPointRepresentation <PointXYZ> : public PointRepresentation <PointXYZ> 00319 { 00320 public: 00321 DefaultPointRepresentation () 00322 { 00323 nr_dimensions_ = 3; 00324 trivial_ = true; 00325 } 00326 00327 virtual void 00328 copyToFloatArray (const PointXYZ &p, float * out) const 00329 { 00330 out[0] = p.x; 00331 out[1] = p.y; 00332 out[2] = p.z; 00333 } 00334 }; 00335 00336 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00337 template <> 00338 class DefaultPointRepresentation <PointXYZI> : public PointRepresentation <PointXYZI> 00339 { 00340 public: 00341 DefaultPointRepresentation () 00342 { 00343 nr_dimensions_ = 3; 00344 trivial_ = true; 00345 } 00346 00347 virtual void 00348 copyToFloatArray (const PointXYZI &p, float * out) const 00349 { 00350 out[0] = p.x; 00351 out[1] = p.y; 00352 out[2] = p.z; 00353 // By default, p.intensity is not part of the PointXYZI vectorization 00354 } 00355 }; 00356 00357 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00358 template <> 00359 class DefaultPointRepresentation <PointNormal> : public PointRepresentation <PointNormal> 00360 { 00361 public: 00362 DefaultPointRepresentation () 00363 { 00364 nr_dimensions_ = 3; 00365 trivial_ = true; 00366 } 00367 00368 virtual void 00369 copyToFloatArray (const PointNormal &p, float * out) const 00370 { 00371 out[0] = p.x; 00372 out[1] = p.y; 00373 out[2] = p.z; 00374 } 00375 }; 00376 00377 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00378 template <> 00379 class DefaultPointRepresentation <PFHSignature125> : public DefaultFeatureRepresentation <PFHSignature125> 00380 {}; 00381 00382 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00383 template <> 00384 class DefaultPointRepresentation <PFHRGBSignature250> : public DefaultFeatureRepresentation <PFHRGBSignature250> 00385 {}; 00386 00387 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00388 template <> 00389 class DefaultPointRepresentation <PPFSignature> : public DefaultFeatureRepresentation <PPFSignature> 00390 { 00391 public: 00392 DefaultPointRepresentation () 00393 { 00394 nr_dimensions_ = 4; 00395 trivial_ = true; 00396 } 00397 00398 virtual void 00399 copyToFloatArray (const PPFSignature &p, float * out) const 00400 { 00401 out[0] = p.f1; 00402 out[1] = p.f2; 00403 out[2] = p.f3; 00404 out[3] = p.f4; 00405 } 00406 }; 00407 00408 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00409 template <> 00410 class DefaultPointRepresentation <FPFHSignature33> : public DefaultFeatureRepresentation <FPFHSignature33> 00411 {}; 00412 00413 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00414 template <> 00415 class DefaultPointRepresentation <VFHSignature308> : public DefaultFeatureRepresentation <VFHSignature308> 00416 {}; 00417 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00418 template <> 00419 class DefaultPointRepresentation <Narf36> : public PointRepresentation <Narf36> 00420 { 00421 public: 00422 DefaultPointRepresentation () 00423 { 00424 nr_dimensions_ = 36; 00425 trivial_=false; 00426 } 00427 00428 virtual void 00429 copyToFloatArray (const Narf36 &p, float * out) const 00430 { 00431 for (int i = 0; i < nr_dimensions_; ++i) 00432 out[i] = p.descriptor[i]; 00433 } 00434 }; 00435 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00436 template <> 00437 class DefaultPointRepresentation<NormalBasedSignature12> : public DefaultFeatureRepresentation <NormalBasedSignature12> 00438 {}; 00439 00440 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00441 template <> 00442 class DefaultPointRepresentation<ShapeContext1980> : public PointRepresentation<ShapeContext1980> 00443 { 00444 public: 00445 DefaultPointRepresentation () 00446 { 00447 nr_dimensions_ = 1980; 00448 } 00449 00450 virtual void 00451 copyToFloatArray (const ShapeContext1980 &p, float * out) const 00452 { 00453 for (int i = 0; i < nr_dimensions_; ++i) 00454 out[i] = p.descriptor[i]; 00455 } 00456 }; 00457 00458 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00459 template <> 00460 class DefaultPointRepresentation<SHOT352> : public PointRepresentation<SHOT352> 00461 { 00462 public: 00463 DefaultPointRepresentation () 00464 { 00465 nr_dimensions_ = 352; 00466 } 00467 00468 virtual void 00469 copyToFloatArray (const SHOT352 &p, float * out) const 00470 { 00471 for (int i = 0; i < nr_dimensions_; ++i) 00472 out[i] = p.descriptor[i]; 00473 } 00474 }; 00475 00476 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00477 template <> 00478 class DefaultPointRepresentation<SHOT1344> : public PointRepresentation<SHOT1344> 00479 { 00480 public: 00481 DefaultPointRepresentation () 00482 { 00483 nr_dimensions_ = 1344; 00484 } 00485 00486 virtual void 00487 copyToFloatArray (const SHOT1344 &p, float * out) const 00488 { 00489 for (int i = 0; i < nr_dimensions_; ++i) 00490 out[i] = p.descriptor[i]; 00491 } 00492 }; 00493 00494 00495 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00496 /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point. 00497 */ 00498 template <typename PointDefault> 00499 class CustomPointRepresentation : public PointRepresentation <PointDefault> 00500 { 00501 using PointRepresentation <PointDefault>::nr_dimensions_; 00502 00503 public: 00504 // Boost shared pointers 00505 typedef boost::shared_ptr<CustomPointRepresentation<PointDefault> > Ptr; 00506 typedef boost::shared_ptr<const CustomPointRepresentation<PointDefault> > ConstPtr; 00507 00508 /** \brief Constructor 00509 * \param[in] max_dim the maximum number of dimensions to use 00510 * \param[in] start_dim the starting dimension 00511 */ 00512 CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0) 00513 : max_dim_(max_dim), start_dim_(start_dim) 00514 { 00515 // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions 00516 nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_; 00517 // Limit the default representation to the first 3 elements 00518 if (nr_dimensions_ > max_dim_) 00519 nr_dimensions_ = max_dim_; 00520 } 00521 00522 inline Ptr 00523 makeShared () const 00524 { 00525 return Ptr (new CustomPointRepresentation<PointDefault> (*this)); 00526 } 00527 00528 /** \brief Copy the point data into a float array 00529 * \param[in] p the input point 00530 * \param[out] out the resultant output array 00531 */ 00532 virtual void 00533 copyToFloatArray (const PointDefault &p, float *out) const 00534 { 00535 // If point type is unknown, treat it as a struct/array of floats 00536 const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_; 00537 for (int i = 0; i < nr_dimensions_; ++i) 00538 out[i] = ptr[i]; 00539 } 00540 00541 protected: 00542 /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */ 00543 int max_dim_; 00544 /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */ 00545 int start_dim_; 00546 }; 00547 } 00548 00549 #endif // #ifndef PCL_POINT_REPRESENTATION_H_