Point Cloud Library (PCL)  1.8.0
point_representation.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 #ifndef PCL_POINT_REPRESENTATION_H_
40 #define PCL_POINT_REPRESENTATION_H_
41 
42 #include <pcl/point_types.h>
43 #include <pcl/pcl_macros.h>
44 #include <pcl/for_each_type.h>
45 
46 namespace pcl
47 {
48  /** \brief @b PointRepresentation provides a set of methods for converting a point structs/object into an
49  * n-dimensional vector.
50  * \note This is an abstract class. Subclasses must set nr_dimensions_ to the appropriate value in the constructor
51  * and provide an implemention of the pure virtual copyToFloatArray method.
52  * \author Michael Dixon
53  */
54  template <typename PointT>
56  {
57  protected:
58  /** \brief The number of dimensions in this point's vector (i.e. the "k" in "k-D") */
60  /** \brief A vector containing the rescale factor to apply to each dimension. */
61  std::vector<float> alpha_;
62  /** \brief Indicates whether this point representation is trivial. It is trivial if and only if the following
63  * conditions hold:
64  * - the relevant data consists only of float values
65  * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array
66  * - sizeof(PointT) is a multiple of sizeof(float)
67  * In short, a trivial point representation converts the input point to a float array that is the same as if
68  * the point was reinterpret_casted to a float array of length nr_dimensions_ . This value says that this
69  * representation can be trivial; it is only trivial if setRescaleValues() has not been set.
70  */
71  bool trivial_;
72 
73  public:
74  typedef boost::shared_ptr<PointRepresentation<PointT> > Ptr;
75  typedef boost::shared_ptr<const PointRepresentation<PointT> > ConstPtr;
76 
77  /** \brief Empty constructor */
78  PointRepresentation () : nr_dimensions_ (0), alpha_ (0), trivial_ (false) {}
79 
80  /** \brief Empty destructor */
81  virtual ~PointRepresentation () {}
82 
83  /** \brief Copy point data from input point to a float array. This method must be overriden in all subclasses.
84  * \param[in] p The input point
85  * \param[out] out A pointer to a float array.
86  */
87  virtual void copyToFloatArray (const PointT &p, float *out) const = 0;
88 
89  /** \brief Returns whether this point representation is trivial. It is trivial if and only if the following
90  * conditions hold:
91  * - the relevant data consists only of float values
92  * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array
93  * - sizeof(PointT) is a multiple of sizeof(float)
94  * In short, a trivial point representation converts the input point to a float array that is the same as if
95  * the point was reinterpret_casted to a float array of length nr_dimensions_ . */
96  inline bool isTrivial() const { return trivial_ && alpha_.empty (); }
97 
98  /** \brief Verify that the input point is valid.
99  * \param p The point to validate
100  */
101  virtual bool
102  isValid (const PointT &p) const
103  {
104  bool is_valid = true;
105 
106  if (trivial_)
107  {
108  const float* temp = reinterpret_cast<const float*>(&p);
109 
110  for (int i = 0; i < nr_dimensions_; ++i)
111  {
112  if (!pcl_isfinite (temp[i]))
113  {
114  is_valid = false;
115  break;
116  }
117  }
118  }
119  else
120  {
121  float *temp = new float[nr_dimensions_];
122  copyToFloatArray (p, temp);
123 
124  for (int i = 0; i < nr_dimensions_; ++i)
125  {
126  if (!pcl_isfinite (temp[i]))
127  {
128  is_valid = false;
129  break;
130  }
131  }
132  delete [] temp;
133  }
134  return (is_valid);
135  }
136 
137  /** \brief Convert input point into a vector representation, rescaling by \a alpha.
138  * \param[in] p the input point
139  * \param[out] out The output vector. Can be of any type that implements the [] operator.
140  */
141  template <typename OutputType> void
142  vectorize (const PointT &p, OutputType &out) const
143  {
144  float *temp = new float[nr_dimensions_];
145  copyToFloatArray (p, temp);
146  if (alpha_.empty ())
147  {
148  for (int i = 0; i < nr_dimensions_; ++i)
149  out[i] = temp[i];
150  }
151  else
152  {
153  for (int i = 0; i < nr_dimensions_; ++i)
154  out[i] = temp[i] * alpha_[i];
155  }
156  delete [] temp;
157  }
158 
159  /** \brief Set the rescale values to use when vectorizing points
160  * \param[in] rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator.
161  */
162  void
163  setRescaleValues (const float *rescale_array)
164  {
165  alpha_.resize (nr_dimensions_);
166  for (int i = 0; i < nr_dimensions_; ++i)
167  alpha_[i] = rescale_array[i];
168  }
169 
170  /** \brief Return the number of dimensions in the point's vector representation. */
171  inline int getNumberOfDimensions () const { return (nr_dimensions_); }
172  };
173 
174  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
175  /** \brief @b DefaultPointRepresentation extends PointRepresentation to define default behavior for common point types.
176  */
177  template <typename PointDefault>
178  class DefaultPointRepresentation : public PointRepresentation <PointDefault>
179  {
182 
183  public:
184  // Boost shared pointers
185  typedef boost::shared_ptr<DefaultPointRepresentation<PointDefault> > Ptr;
186  typedef boost::shared_ptr<const DefaultPointRepresentation<PointDefault> > ConstPtr;
187 
189  {
190  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
191  nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
192  // Limit the default representation to the first 3 elements
193  if (nr_dimensions_ > 3) nr_dimensions_ = 3;
194 
195  trivial_ = true;
196  }
197 
199 
200  inline Ptr
201  makeShared () const
202  {
203  return (Ptr (new DefaultPointRepresentation<PointDefault> (*this)));
204  }
205 
206  virtual void
207  copyToFloatArray (const PointDefault &p, float * out) const
208  {
209  // If point type is unknown, treat it as a struct/array of floats
210  const float* ptr = reinterpret_cast<const float*> (&p);
211  for (int i = 0; i < nr_dimensions_; ++i)
212  out[i] = ptr[i];
213  }
214  };
215 
216  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
217  /** \brief @b DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the
218  * default behavior for feature descriptor types (i.e., copy each element of each field into a float array).
219  */
220  template <typename PointDefault>
221  class DefaultFeatureRepresentation : public PointRepresentation <PointDefault>
222  {
223  protected:
225 
226  private:
227  struct IncrementFunctor
228  {
229  IncrementFunctor (int &n) : n_ (n)
230  {
231  n_ = 0;
232  }
233 
234  template<typename Key> inline void operator () ()
235  {
237  }
238 
239  private:
240  int &n_;
241  };
242 
243  struct NdCopyPointFunctor
244  {
245  typedef typename traits::POD<PointDefault>::type Pod;
246 
247  NdCopyPointFunctor (const PointDefault &p1, float * p2)
248  : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
249 
250  template<typename Key> inline void operator() ()
251  {
252  typedef typename pcl::traits::datatype<PointDefault, Key>::type FieldT;
254  Helper<Key, FieldT, NrDims>::copyPoint (p1_, p2_, f_idx_);
255  }
256 
257  // Copy helper for scalar fields
258  template <typename Key, typename FieldT, int NrDims>
259  struct Helper
260  {
261  static void copyPoint (const Pod &p1, float * p2, int &f_idx)
262  {
263  const uint8_t * data_ptr = reinterpret_cast<const uint8_t *> (&p1) +
265  p2[f_idx++] = *reinterpret_cast<const FieldT*> (data_ptr);
266  }
267  };
268  // Copy helper for array fields
269  template <typename Key, typename FieldT, int NrDims>
270  struct Helper<Key, FieldT[NrDims], NrDims>
271  {
272  static void copyPoint (const Pod &p1, float * p2, int &f_idx)
273  {
274  const uint8_t * data_ptr = reinterpret_cast<const uint8_t *> (&p1) +
276  int nr_dims = NrDims;
277  const FieldT * array = reinterpret_cast<const FieldT *> (data_ptr);
278  for (int i = 0; i < nr_dims; ++i)
279  {
280  p2[f_idx++] = array[i];
281  }
282  }
283  };
284 
285  private:
286  const Pod &p1_;
287  float * p2_;
288  int f_idx_;
289  };
290 
291  public:
292  // Boost shared pointers
293  typedef typename boost::shared_ptr<DefaultFeatureRepresentation<PointDefault> > Ptr;
294  typedef typename boost::shared_ptr<const DefaultFeatureRepresentation<PointDefault> > ConstPtr;
296 
298  {
299  nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented
300  pcl::for_each_type <FieldList> (IncrementFunctor (nr_dimensions_));
301  }
302 
303  inline Ptr
304  makeShared () const
305  {
306  return (Ptr (new DefaultFeatureRepresentation<PointDefault> (*this)));
307  }
308 
309  virtual void
310  copyToFloatArray (const PointDefault &p, float * out) const
311  {
312  pcl::for_each_type <FieldList> (NdCopyPointFunctor (p, out));
313  }
314  };
315 
316  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
317  template <>
319  {
320  public:
322  {
323  nr_dimensions_ = 3;
324  trivial_ = true;
325  }
326 
327  virtual void
328  copyToFloatArray (const PointXYZ &p, float * out) const
329  {
330  out[0] = p.x;
331  out[1] = p.y;
332  out[2] = p.z;
333  }
334  };
335 
336  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
337  template <>
339  {
340  public:
342  {
343  nr_dimensions_ = 3;
344  trivial_ = true;
345  }
346 
347  virtual void
348  copyToFloatArray (const PointXYZI &p, float * out) const
349  {
350  out[0] = p.x;
351  out[1] = p.y;
352  out[2] = p.z;
353  // By default, p.intensity is not part of the PointXYZI vectorization
354  }
355  };
356 
357  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
358  template <>
360  {
361  public:
363  {
364  nr_dimensions_ = 3;
365  trivial_ = true;
366  }
367 
368  virtual void
369  copyToFloatArray (const PointNormal &p, float * out) const
370  {
371  out[0] = p.x;
372  out[1] = p.y;
373  out[2] = p.z;
374  }
375  };
376 
377  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
378  template <>
380  {};
381 
382  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
383  template <>
385  {};
386 
387  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
388  template <>
390  {
391  public:
393  {
394  nr_dimensions_ = 4;
395  trivial_ = true;
396  }
397 
398  virtual void
399  copyToFloatArray (const PPFSignature &p, float * out) const
400  {
401  out[0] = p.f1;
402  out[1] = p.f2;
403  out[2] = p.f3;
404  out[3] = p.f4;
405  }
406  };
407 
408  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
409  template <>
411  {};
412 
413  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
414  template <>
416  {};
417  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
418  template <>
420  {
421  public:
423  {
424  nr_dimensions_ = 36;
425  trivial_=false;
426  }
427 
428  virtual void
429  copyToFloatArray (const Narf36 &p, float * out) const
430  {
431  for (int i = 0; i < nr_dimensions_; ++i)
432  out[i] = p.descriptor[i];
433  }
434  };
435  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
436  template <>
438  {};
439 
440  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
441  template <>
443  {
444  public:
446  {
447  nr_dimensions_ = 1980;
448  }
449 
450  virtual void
451  copyToFloatArray (const ShapeContext1980 &p, float * out) const
452  {
453  for (int i = 0; i < nr_dimensions_; ++i)
454  out[i] = p.descriptor[i];
455  }
456  };
457 
458  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
459  template <>
461  {
462  public:
464  {
465  nr_dimensions_ = 1960;
466  }
467 
468  virtual void
469  copyToFloatArray (const UniqueShapeContext1960 &p, float * out) const
470  {
471  for (int i = 0; i < nr_dimensions_; ++i)
472  out[i] = p.descriptor[i];
473  }
474  };
475 
476  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
477  template <>
479  {
480  public:
482  {
483  nr_dimensions_ = 352;
484  }
485 
486  virtual void
487  copyToFloatArray (const SHOT352 &p, float * out) const
488  {
489  for (int i = 0; i < nr_dimensions_; ++i)
490  out[i] = p.descriptor[i];
491  }
492  };
493 
494  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
495  template <>
497  {
498  public:
500  {
501  nr_dimensions_ = 1344;
502  }
503 
504  virtual void
505  copyToFloatArray (const SHOT1344 &p, float * out) const
506  {
507  for (int i = 0; i < nr_dimensions_; ++i)
508  out[i] = p.descriptor[i];
509  }
510  };
511 
512 
513  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
514  /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
515  */
516  template <typename PointDefault>
517  class CustomPointRepresentation : public PointRepresentation <PointDefault>
518  {
520 
521  public:
522  // Boost shared pointers
523  typedef boost::shared_ptr<CustomPointRepresentation<PointDefault> > Ptr;
524  typedef boost::shared_ptr<const CustomPointRepresentation<PointDefault> > ConstPtr;
525 
526  /** \brief Constructor
527  * \param[in] max_dim the maximum number of dimensions to use
528  * \param[in] start_dim the starting dimension
529  */
530  CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0)
531  : max_dim_(max_dim), start_dim_(start_dim)
532  {
533  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
534  nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_;
535  // Limit the default representation to the first 3 elements
536  if (nr_dimensions_ > max_dim_)
537  nr_dimensions_ = max_dim_;
538  }
539 
540  inline Ptr
541  makeShared () const
542  {
543  return Ptr (new CustomPointRepresentation<PointDefault> (*this));
544  }
545 
546  /** \brief Copy the point data into a float array
547  * \param[in] p the input point
548  * \param[out] out the resultant output array
549  */
550  virtual void
551  copyToFloatArray (const PointDefault &p, float *out) const
552  {
553  // If point type is unknown, treat it as a struct/array of floats
554  const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_;
555  for (int i = 0; i < nr_dimensions_; ++i)
556  out[i] = ptr[i];
557  }
558 
559  protected:
560  /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */
561  int max_dim_;
562  /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */
564  };
565 }
566 
567 #endif // #ifndef PCL_POINT_REPRESENTATION_H_
virtual void copyToFloatArray(const PointT &p, float *out) const =0
Copy point data from input point to a float array.
A point structure representing a Shape Context.
virtual void copyToFloatArray(const PointDefault &p, float *out) const
Copy point data from input point to a float array.
void vectorize(const PointT &p, OutputType &out) const
Convert input point into a vector representation, rescaling by alpha.
A point structure representing the Normal Based Signature for a feature matrix of 4-by-3...
boost::shared_ptr< const DefaultPointRepresentation< PointDefault > > ConstPtr
A point structure representing a Unique Shape Context.
bool isTrivial() const
Returns whether this point representation is trivial.
virtual void copyToFloatArray(const PointDefault &p, float *out) const
Copy point data from input point to a float array.
pcl::traits::fieldList< PointDefault >::type FieldList
virtual void copyToFloatArray(const Narf36 &p, float *out) const
Copy point data from input point to a float array.
CustomPointRepresentation(const int max_dim=3, const int start_dim=0)
Constructor.
boost::shared_ptr< const PointRepresentation< PointT > > ConstPtr
virtual void copyToFloatArray(const SHOT352 &p, float *out) const
Copy point data from input point to a float array.
boost::shared_ptr< DefaultFeatureRepresentation< PointDefault > > Ptr
virtual bool isValid(const PointT &p) const
Verify that the input point is valid.
boost::shared_ptr< const DefaultFeatureRepresentation< PointDefault > > ConstPtr
virtual void copyToFloatArray(const UniqueShapeContext1960 &p, float *out) const
Copy point data from input point to a float array.
int getNumberOfDimensions() const
Return the number of dimensions in the point&#39;s vector representation.
A point structure representing the Fast Point Feature Histogram (FPFH).
virtual void copyToFloatArray(const ShapeContext1980 &p, float *out) const
Copy point data from input point to a float array.
int max_dim_
Use at most this many dimensions (i.e.
boost::shared_ptr< DefaultPointRepresentation< PointDefault > > Ptr
PointRepresentation()
Empty constructor.
PointRepresentation provides a set of methods for converting a point structs/object into an n-dimensi...
CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point...
A point structure representing the Point Feature Histogram with colors (PFHRGB).
boost::shared_ptr< PointRepresentation< PointT > > Ptr
int start_dim_
Use dimensions only starting with this one (i.e.
int nr_dimensions_
The number of dimensions in this point&#39;s vector (i.e.
Defines all the PCL implemented PointT point type structures.
virtual void copyToFloatArray(const PointNormal &p, float *out) const
Copy point data from input point to a float array.
A point structure representing Euclidean xyz coordinates.
virtual void copyToFloatArray(const SHOT1344 &p, float *out) const
Copy point data from input point to a float array.
boost::shared_ptr< CustomPointRepresentation< PointDefault > > Ptr
virtual void copyToFloatArray(const PPFSignature &p, float *out) const
Copy point data from input point to a float array.
virtual ~PointRepresentation()
Empty destructor.
virtual void copyToFloatArray(const PointDefault &p, float *out) const
Copy the point data into a float array.
static void copyPoint(const Pod &p1, float *p2, int &f_idx)
A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape onl...
std::vector< float > alpha_
A vector containing the rescale factor to apply to each dimension.
bool trivial_
Indicates whether this point representation is trivial.
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
boost::shared_ptr< const CustomPointRepresentation< PointDefault > > ConstPtr
A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape+col...
A point structure representing the Viewpoint Feature Histogram (VFH).
A point structure representing the Narf descriptor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
float descriptor[1344]
A point structure representing the Point Feature Histogram (PFH).
void setRescaleValues(const float *rescale_array)
Set the rescale values to use when vectorizing points.
float descriptor[352]
float descriptor[36]
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:138
virtual void copyToFloatArray(const PointXYZI &p, float *out) const
Copy point data from input point to a float array.
A point structure for storing the Point Pair Feature (PPF) values.
virtual void copyToFloatArray(const PointXYZ &p, float *out) const
Copy point data from input point to a float array.