Point Cloud Library (PCL)  1.10.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 <algorithm>
43 #include <vector>
44 
45 #include <pcl/point_types.h>
46 #include <pcl/memory.h>
47 #include <pcl/pcl_macros.h>
48 #include <pcl/for_each_type.h>
49 
50 namespace pcl
51 {
52  /** \brief @b PointRepresentation provides a set of methods for converting a point structs/object into an
53  * n-dimensional vector.
54  * \note This is an abstract class. Subclasses must set nr_dimensions_ to the appropriate value in the constructor
55  * and provide an implementation of the pure virtual copyToFloatArray method.
56  * \author Michael Dixon
57  */
58  template <typename PointT>
60  {
61  protected:
62  /** \brief The number of dimensions in this point's vector (i.e. the "k" in "k-D") */
63  int nr_dimensions_ = 0;
64  /** \brief A vector containing the rescale factor to apply to each dimension. */
65  std::vector<float> alpha_;
66  /** \brief Indicates whether this point representation is trivial. It is trivial if and only if the following
67  * conditions hold:
68  * - the relevant data consists only of float values
69  * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array
70  * - sizeof(PointT) is a multiple of sizeof(float)
71  * In short, a trivial point representation converts the input point to a float array that is the same as if
72  * the point was reinterpret_casted to a float array of length nr_dimensions_ . This value says that this
73  * representation can be trivial; it is only trivial if setRescaleValues() has not been set.
74  */
75  bool trivial_ = false;
76 
77  public:
80 
81  /** \brief Empty destructor */
82  virtual ~PointRepresentation () = default;
83  //TODO: check if copy and move constructors / assignment operators are needed
84 
85  /** \brief Copy point data from input point to a float array. This method must be overridden in all subclasses.
86  * \param[in] p The input point
87  * \param[out] out A pointer to a float array.
88  */
89  virtual void copyToFloatArray (const PointT &p, float *out) const = 0;
90 
91  /** \brief Returns whether this point representation is trivial. It is trivial if and only if the following
92  * conditions hold:
93  * - the relevant data consists only of float values
94  * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array
95  * - sizeof(PointT) is a multiple of sizeof(float)
96  * In short, a trivial point representation converts the input point to a float array that is the same as if
97  * the point was reinterpret_casted to a float array of length nr_dimensions_ . */
98  inline bool isTrivial() const { return trivial_ && alpha_.empty (); }
99 
100  /** \brief Verify that the input point is valid.
101  * \param p The point to validate
102  */
103  virtual bool
104  isValid (const PointT &p) const
105  {
106  bool is_valid = true;
107 
108  if (trivial_)
109  {
110  const float* temp = reinterpret_cast<const float*>(&p);
111 
112  for (int i = 0; i < nr_dimensions_; ++i)
113  {
114  if (!std::isfinite (temp[i]))
115  {
116  is_valid = false;
117  break;
118  }
119  }
120  }
121  else
122  {
123  float *temp = new float[nr_dimensions_];
124  copyToFloatArray (p, temp);
125 
126  for (int i = 0; i < nr_dimensions_; ++i)
127  {
128  if (!std::isfinite (temp[i]))
129  {
130  is_valid = false;
131  break;
132  }
133  }
134  delete [] temp;
135  }
136  return (is_valid);
137  }
138 
139  /** \brief Convert input point into a vector representation, rescaling by \a alpha.
140  * \param[in] p the input point
141  * \param[out] out The output vector. Can be of any type that implements the [] operator.
142  */
143  template <typename OutputType> void
144  vectorize (const PointT &p, OutputType &out) const
145  {
146  float *temp = new float[nr_dimensions_];
147  copyToFloatArray (p, temp);
148  if (alpha_.empty ())
149  {
150  for (int i = 0; i < nr_dimensions_; ++i)
151  out[i] = temp[i];
152  }
153  else
154  {
155  for (int i = 0; i < nr_dimensions_; ++i)
156  out[i] = temp[i] * alpha_[i];
157  }
158  delete [] temp;
159  }
160 
161  /** \brief Set the rescale values to use when vectorizing points
162  * \param[in] rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator.
163  */
164  void
165  setRescaleValues (const float *rescale_array)
166  {
167  alpha_.resize (nr_dimensions_);
168  std::copy_n(rescale_array, nr_dimensions_, alpha_.begin());
169  }
170 
171  /** \brief Return the number of dimensions in the point's vector representation. */
172  inline int getNumberOfDimensions () const { return (nr_dimensions_); }
173  };
174 
175  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
176  /** \brief @b DefaultPointRepresentation extends PointRepresentation to define default behavior for common point types.
177  */
178  template <typename PointDefault>
179  class DefaultPointRepresentation : public PointRepresentation <PointDefault>
180  {
183 
184  public:
185  // Boost shared pointers
188 
190  {
191  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
192  nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
193  // Limit the default representation to the first 3 elements
194  if (nr_dimensions_ > 3) nr_dimensions_ = 3;
195 
196  trivial_ = true;
197  }
198 
200 
201  inline Ptr
202  makeShared () const
203  {
204  return (Ptr (new DefaultPointRepresentation<PointDefault> (*this)));
205  }
206 
207  void
208  copyToFloatArray (const PointDefault &p, float * out) const override
209  {
210  // If point type is unknown, treat it as a struct/array of floats
211  const float* ptr = reinterpret_cast<const float*> (&p);
212  std::copy_n(ptr, nr_dimensions_, out);
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 std::uint8_t * data_ptr = reinterpret_cast<const std::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 std::uint8_t * data_ptr = reinterpret_cast<const std::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
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
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  std::copy_n(ptr, nr_dimensions_, out);
572  }
573 
574  protected:
575  /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */
576  int max_dim_;
577  /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */
579  };
580 }
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.
shared_ptr< DefaultPointRepresentation< PointDefault > > Ptr
Defines functions, macros and traits for allocating and using memory.
void copyToFloatArray(const PointXYZI &p, float *out) const override
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...
A point structure representing a Unique Shape Context.
bool isTrivial() const
Returns whether this point representation is trivial.
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.
shared_ptr< const DefaultPointRepresentation< PointDefault > > ConstPtr
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).
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 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.
Defines all the PCL implemented PointT point type structures.
void copyToFloatArray(const Narf36 &p, float *out) const override
Copy point data from input point to a float array.
shared_ptr< const PointRepresentation< SHOT352 > > ConstPtr
shared_ptr< CustomPointRepresentation< PointDefault > > Ptr
A point structure representing Euclidean xyz coordinates.
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.
virtual ~PointRepresentation()=default
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...
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 ...
shared_ptr< const CustomPointRepresentation< PointDefault > > ConstPtr
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.
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.
void copyToFloatArray(const PointXYZ &p, float *out) const override
Copy point data from input point to a float array.
float descriptor[352]
shared_ptr< PointRepresentation< SHOT352 > > Ptr
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< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
Defines all the PCL and non-PCL macros used.
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.