Point Cloud Library (PCL)  1.9.1-dev
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 
40 #pragma once
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 implementation 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  using Ptr = boost::shared_ptr<PointRepresentation<PointT> >;
75  using ConstPtr = boost::shared_ptr<const PointRepresentation<PointT> >;
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 overridden 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 (!std::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 (!std::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  using Ptr = boost::shared_ptr<DefaultPointRepresentation<PointDefault> >;
186  using ConstPtr = boost::shared_ptr<const DefaultPointRepresentation<PointDefault> >;
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  void
207  copyToFloatArray (const PointDefault &p, float * out) const override
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  using Pod = typename traits::POD<PointDefault>::type;
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  using FieldT = typename pcl::traits::datatype<PointDefault, Key>::type;
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  using Ptr = typename boost::shared_ptr<DefaultFeatureRepresentation<PointDefault> >;
294  using ConstPtr = typename boost::shared_ptr<const DefaultFeatureRepresentation<PointDefault> >;
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  void
310  copyToFloatArray (const PointDefault &p, float * out) const override
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  void
328  copyToFloatArray (const PointXYZ &p, float * out) const override
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  void
348  copyToFloatArray (const PointXYZI &p, float * out) const override
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  void
369  copyToFloatArray (const PointNormal &p, float * out) const override
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  void
399  copyToFloatArray (const PPFSignature &p, float * out) const override
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  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
419  template <>
421  {};
422 
423  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
424  template <>
426  {};
427 
428  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
429  template <>
431  {};
432 
433  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
434  template <>
436  {
437  public:
439  {
440  nr_dimensions_ = 36;
441  trivial_=false;
442  }
443 
444  void
445  copyToFloatArray (const Narf36 &p, float * out) const override
446  {
447  for (int i = 0; i < nr_dimensions_; ++i)
448  out[i] = p.descriptor[i];
449  }
450  };
451  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
452  template <>
454  {};
455 
456  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
457  template <>
459  {
460  public:
462  {
463  nr_dimensions_ = 1980;
464  }
465 
466  void
467  copyToFloatArray (const ShapeContext1980 &p, float * out) const override
468  {
469  for (int i = 0; i < nr_dimensions_; ++i)
470  out[i] = p.descriptor[i];
471  }
472  };
473 
474  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
475  template <>
477  {
478  public:
480  {
481  nr_dimensions_ = 1960;
482  }
483 
484  void
485  copyToFloatArray (const UniqueShapeContext1960 &p, float * out) const override
486  {
487  for (int i = 0; i < nr_dimensions_; ++i)
488  out[i] = p.descriptor[i];
489  }
490  };
491 
492  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
493  template <>
495  {
496  public:
498  {
499  nr_dimensions_ = 352;
500  }
501 
502  void
503  copyToFloatArray (const SHOT352 &p, float * out) const override
504  {
505  for (int i = 0; i < nr_dimensions_; ++i)
506  out[i] = p.descriptor[i];
507  }
508  };
509 
510  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
511  template <>
513  {
514  public:
516  {
517  nr_dimensions_ = 1344;
518  }
519 
520  void
521  copyToFloatArray (const SHOT1344 &p, float * out) const override
522  {
523  for (int i = 0; i < nr_dimensions_; ++i)
524  out[i] = p.descriptor[i];
525  }
526  };
527 
528 
529  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
530  /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
531  */
532  template <typename PointDefault>
533  class CustomPointRepresentation : public PointRepresentation <PointDefault>
534  {
536 
537  public:
538  // Boost shared pointers
539  using Ptr = boost::shared_ptr<CustomPointRepresentation<PointDefault> >;
540  using ConstPtr = boost::shared_ptr<const CustomPointRepresentation<PointDefault> >;
541 
542  /** \brief Constructor
543  * \param[in] max_dim the maximum number of dimensions to use
544  * \param[in] start_dim the starting dimension
545  */
546  CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0)
547  : max_dim_(max_dim), start_dim_(start_dim)
548  {
549  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
550  nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_;
551  // Limit the default representation to the first 3 elements
552  if (nr_dimensions_ > max_dim_)
553  nr_dimensions_ = max_dim_;
554  }
555 
556  inline Ptr
557  makeShared () const
558  {
559  return Ptr (new CustomPointRepresentation<PointDefault> (*this));
560  }
561 
562  /** \brief Copy the point data into a float array
563  * \param[in] p the input point
564  * \param[out] out the resultant output array
565  */
566  virtual void
567  copyToFloatArray (const PointDefault &p, float *out) const
568  {
569  // If point type is unknown, treat it as a struct/array of floats
570  const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_;
571  for (int i = 0; i < nr_dimensions_; ++i)
572  out[i] = ptr[i];
573  }
574 
575  protected:
576  /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */
577  int max_dim_;
578  /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */
580  };
581 }
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.
void copyToFloatArray(const PointXYZI &p, float *out) const override
Copy point data from input point to a float array.
bool isTrivial() const
Returns whether this point representation is trivial.
boost::shared_ptr< const CustomPointRepresentation< PointDefault > > ConstPtr
A point structure representing the Normal Based Signature for a feature matrix of 4-by-3...
A point structure representing a Unique Shape Context.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
CustomPointRepresentation(const int max_dim=3, const int start_dim=0)
Constructor.
virtual bool isValid(const PointT &p) const
Verify that the input point is valid.
boost::shared_ptr< const PointRepresentation< Narf * > > ConstPtr
A point structure representing the Fast Point Feature Histogram (FPFH).
void copyToFloatArray(const UniqueShapeContext1960 &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const SHOT1344 &p, float *out) const override
Copy point data from input point to a float array.
int max_dim_
Use at most this many dimensions (i.e.
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).
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.
void copyToFloatArray(const PointDefault &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const Narf36 &p, float *out) const override
Copy point data from input point to a float array.
A point structure representing Euclidean xyz coordinates.
virtual ~PointRepresentation()
Empty destructor.
A point structure representing the Globally Aligned Spatial Distribution (GASD) shape and color descr...
void copyToFloatArray(const PointDefault &p, float *out) const override
Copy point data from input point to a float array.
static void copyPoint(const Pod &p1, float *p2, int &f_idx)
void vectorize(const PointT &p, OutputType &out) const
Convert input point into a vector representation, rescaling by alpha.
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.
boost::shared_ptr< CustomPointRepresentation< PointDefault > > Ptr
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
void copyToFloatArray(const PointNormal &p, float *out) const override
Copy point data from input point to a float array.
A point structure representing the Globally Aligned Spatial Distribution (GASD) shape and color descr...
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape+col...
void copyToFloatArray(const SHOT352 &p, float *out) const override
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 Viewpoint Feature Histogram (VFH).
boost::shared_ptr< const DefaultPointRepresentation< PointDefault > > ConstPtr
A point structure representing the Narf descriptor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
float descriptor[1344]
boost::shared_ptr< DefaultPointRepresentation< PointDefault > > Ptr
A point structure representing the Point Feature Histogram (PFH).
void setRescaleValues(const float *rescale_array)
Set the rescale values to use when vectorizing points.
void copyToFloatArray(const PointXYZ &p, float *out) const override
Copy point data from input point to a float array.
float descriptor[352]
virtual void copyToFloatArray(const PointDefault &p, float *out) const
Copy the point data into a float array.
A point structure representing the Globally Aligned Spatial Distribution (GASD) shape descriptor...
float descriptor[36]
void copyToFloatArray(const ShapeContext1980 &p, float *out) const override
Copy point data from input point to a float array.
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
boost::shared_ptr< PointRepresentation< Narf * > > Ptr
A point structure for storing the Point Pair Feature (PPF) values.
typename pcl::traits::fieldList< PFHSignature125 >::type FieldList
void copyToFloatArray(const PPFSignature &p, float *out) const override
Copy point data from input point to a float array.