Point Cloud Library (PCL)  1.7.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 */
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_ = 352;
466  }
467 
468  virtual void
469  copyToFloatArray (const SHOT352 &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_ = 1344;
484  }
485 
486  virtual void
487  copyToFloatArray (const SHOT1344 &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  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
496  /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
497  */
498  template <typename PointDefault>
499  class CustomPointRepresentation : public PointRepresentation <PointDefault>
500  {
502 
503  public:
504  // Boost shared pointers
505  typedef boost::shared_ptr<CustomPointRepresentation<PointDefault> > Ptr;
506  typedef boost::shared_ptr<const CustomPointRepresentation<PointDefault> > ConstPtr;
507 
508  /** \brief Constructor
509  * \param[in] max_dim the maximum number of dimensions to use
510  * \param[in] start_dim the starting dimension
511  */
512  CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0)
513  : max_dim_(max_dim), start_dim_(start_dim)
514  {
515  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
516  nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_;
517  // Limit the default representation to the first 3 elements
518  if (nr_dimensions_ > max_dim_)
520  }
521 
522  inline Ptr
523  makeShared () const
524  {
525  return Ptr (new CustomPointRepresentation<PointDefault> (*this));
526  }
527 
528  /** \brief Copy the point data into a float array
529  * \param[in] p the input point
530  * \param[out] out the resultant output array
531  */
532  virtual void
533  copyToFloatArray (const PointDefault &p, float *out) const
534  {
535  // If point type is unknown, treat it as a struct/array of floats
536  const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_;
537  for (int i = 0; i < nr_dimensions_; ++i)
538  out[i] = ptr[i];
539  }
540 
541  protected:
542  /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */
543  int max_dim_;
544  /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */
546  };
547 }
548 
549 #endif // #ifndef PCL_POINT_REPRESENTATION_H_