Go to the documentation of this file.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
00029
00030
00031
00032
00033
00034
00035
00036
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 {
00054 template <typename PointT>
00055 class PointRepresentation
00056 {
00057 protected:
00059 int nr_dimensions_;
00061 std::vector<float> alpha_;
00071 bool trivial_;
00072
00073 public:
00074 typedef boost::shared_ptr<PointRepresentation<PointT> > Ptr;
00075 typedef boost::shared_ptr<const PointRepresentation<PointT> > ConstPtr;
00076
00078 PointRepresentation () : nr_dimensions_ (0), alpha_ (0), trivial_ (false) {}
00079
00081 virtual ~PointRepresentation () {}
00082
00087 virtual void copyToFloatArray (const PointT &p, float *out) const = 0;
00088
00096 inline bool isTrivial() const { return trivial_ && alpha_.empty (); }
00097
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
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
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
00171 inline int getNumberOfDimensions () const { return (nr_dimensions_); }
00172 };
00173
00175
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
00185 typedef boost::shared_ptr<DefaultPointRepresentation<PointDefault> > Ptr;
00186 typedef boost::shared_ptr<const DefaultPointRepresentation<PointDefault> > ConstPtr;
00187
00188 DefaultPointRepresentation ()
00189 {
00190
00191 nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
00192
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
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
00217
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
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
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
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;
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
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
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
00354 }
00355 };
00356
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
00378 template <>
00379 class DefaultPointRepresentation <PFHSignature125> : public DefaultFeatureRepresentation <PFHSignature125>
00380 {};
00381
00383 template <>
00384 class DefaultPointRepresentation <PFHRGBSignature250> : public DefaultFeatureRepresentation <PFHRGBSignature250>
00385 {};
00386
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
00409 template <>
00410 class DefaultPointRepresentation <FPFHSignature33> : public DefaultFeatureRepresentation <FPFHSignature33>
00411 {};
00412
00414 template <>
00415 class DefaultPointRepresentation <VFHSignature308> : public DefaultFeatureRepresentation <VFHSignature308>
00416 {};
00417
00419 template <>
00420 class DefaultPointRepresentation<NormalBasedSignature12> : public DefaultFeatureRepresentation <NormalBasedSignature12>
00421 {};
00422
00424 template <>
00425 class DefaultPointRepresentation<ShapeContext> : public PointRepresentation<ShapeContext>
00426 {
00427 public:
00428 DefaultPointRepresentation ()
00429 {
00430 nr_dimensions_ = 1980;
00431 }
00432
00433 virtual void
00434 copyToFloatArray (const ShapeContext &p, float * out) const
00435 {
00436 for (int i = 0; i < nr_dimensions_; ++i)
00437 out[i] = p.descriptor[i];
00438 }
00439 };
00440
00442 template <>
00443 class
00444 PCL_DEPRECATED_CLASS (DefaultPointRepresentation, "SHOT POINT IS DEPRECATED, USE SHOT352 FOR SHAPE AND SHOT1344 FOR SHAPE+COLOR INSTEAD")
00445 <SHOT>
00446 : public PointRepresentation<SHOT>
00447 {
00448 public:
00449 DefaultPointRepresentation ()
00450 {
00451 nr_dimensions_ = 352;
00452 }
00453
00454 virtual void
00455 copyToFloatArray (const SHOT &p, float * out) const
00456 {
00457 for (int i = 0; i < nr_dimensions_; ++i)
00458 out[i] = p.descriptor[i];
00459 }
00460 };
00461
00463 template <>
00464 class DefaultPointRepresentation<SHOT352> : public PointRepresentation<SHOT352>
00465 {
00466 public:
00467 DefaultPointRepresentation ()
00468 {
00469 nr_dimensions_ = 352;
00470 }
00471
00472 virtual void
00473 copyToFloatArray (const SHOT352 &p, float * out) const
00474 {
00475 for (int i = 0; i < nr_dimensions_; ++i)
00476 out[i] = p.descriptor[i];
00477 }
00478 };
00479
00481 template <>
00482 class DefaultPointRepresentation<SHOT1344> : public PointRepresentation<SHOT1344>
00483 {
00484 public:
00485 DefaultPointRepresentation ()
00486 {
00487 nr_dimensions_ = 1344;
00488 }
00489
00490 virtual void
00491 copyToFloatArray (const SHOT1344 &p, float * out) const
00492 {
00493 for (int i = 0; i < nr_dimensions_; ++i)
00494 out[i] = p.descriptor[i];
00495 }
00496 };
00497
00498
00500
00502 template <typename PointDefault>
00503 class CustomPointRepresentation : public PointRepresentation <PointDefault>
00504 {
00505 using PointRepresentation <PointDefault>::nr_dimensions_;
00506
00507 public:
00508
00509 typedef boost::shared_ptr<CustomPointRepresentation<PointDefault> > Ptr;
00510 typedef boost::shared_ptr<const CustomPointRepresentation<PointDefault> > ConstPtr;
00511
00516 CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0)
00517 : max_dim_(max_dim), start_dim_(start_dim)
00518 {
00519
00520 nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_;
00521
00522 if (nr_dimensions_ > max_dim_)
00523 nr_dimensions_ = max_dim_;
00524 }
00525
00526 inline Ptr
00527 makeShared () const
00528 {
00529 return Ptr (new CustomPointRepresentation<PointDefault> (*this));
00530 }
00531
00536 virtual void
00537 copyToFloatArray (const PointDefault &p, float *out) const
00538 {
00539
00540 const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_;
00541 for (int i = 0; i < nr_dimensions_; ++i)
00542 out[i] = ptr[i];
00543 }
00544
00545 protected:
00547 int max_dim_;
00549 int start_dim_;
00550 };
00551 }
00552
00553 #endif // #ifndef PCL_POINT_REPRESENTATION_H_