Point Cloud Library (PCL)  1.14.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:
78  using Ptr = shared_ptr<PointRepresentation<PointT> >;
79  using ConstPtr = shared_ptr<const PointRepresentation<PointT> >;
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  void
162  vectorize (const PointT &p, float* out) const
163  {
164  copyToFloatArray (p, out);
165  if (!alpha_.empty ())
166  for (int i = 0; i < nr_dimensions_; ++i)
167  out[i] *= alpha_[i];
168  }
169 
170  void
171  vectorize (const PointT &p, std::vector<float> &out) const
172  {
173  copyToFloatArray (p, out.data());
174  if (!alpha_.empty ())
175  for (int i = 0; i < nr_dimensions_; ++i)
176  out[i] *= alpha_[i];
177  }
178 
179  /** \brief Set the rescale values to use when vectorizing points
180  * \param[in] rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator.
181  */
182  void
183  setRescaleValues (const float *rescale_array)
184  {
185  alpha_.resize (nr_dimensions_);
186  std::copy(rescale_array, rescale_array + nr_dimensions_, alpha_.begin());
187  }
188 
189  /** \brief Return the number of dimensions in the point's vector representation. */
190  inline int getNumberOfDimensions () const { return (nr_dimensions_); }
191  };
192 
193  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
194  /** \brief @b DefaultPointRepresentation extends PointRepresentation to define default behavior for common point types.
195  */
196  template <typename PointDefault>
197  class DefaultPointRepresentation : public PointRepresentation <PointDefault>
198  {
201 
202  public:
203  // Boost shared pointers
204  using Ptr = shared_ptr<DefaultPointRepresentation<PointDefault> >;
205  using ConstPtr = shared_ptr<const DefaultPointRepresentation<PointDefault> >;
206 
208  {
209  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
210  nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
211  // Limit the default representation to the first 3 elements
212  if (nr_dimensions_ > 3) nr_dimensions_ = 3;
213 
214  trivial_ = true;
215  }
216 
217  ~DefaultPointRepresentation () override = default;
218 
219  inline Ptr
220  makeShared () const
221  {
222  return (Ptr (new DefaultPointRepresentation<PointDefault> (*this)));
223  }
224 
225  void
226  copyToFloatArray (const PointDefault &p, float * out) const override
227  {
228  // If point type is unknown, treat it as a struct/array of floats
229  const float* ptr = reinterpret_cast<const float*> (&p);
230  std::copy(ptr, ptr + nr_dimensions_, out);
231  }
232  };
233 
234  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
235  /** \brief @b DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the
236  * default behavior for feature descriptor types (i.e., copy each element of each field into a float array).
237  */
238  template <typename PointDefault>
239  class DefaultFeatureRepresentation : public PointRepresentation <PointDefault>
240  {
241  protected:
243 
244  private:
245  struct IncrementFunctor
246  {
247  IncrementFunctor (int &n) : n_ (n)
248  {
249  n_ = 0;
250  }
251 
252  template<typename Key> inline void operator () ()
253  {
254  n_ += pcl::traits::datatype<PointDefault, Key>::size;
255  }
256 
257  private:
258  int &n_;
259  };
260 
261  struct NdCopyPointFunctor
262  {
263  using Pod = typename traits::POD<PointDefault>::type;
264 
265  NdCopyPointFunctor (const PointDefault &p1, float * p2)
266  : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2) {}
267 
268  template<typename Key> inline void operator() ()
269  {
270  using FieldT = typename pcl::traits::datatype<PointDefault, Key>::type;
271  constexpr int NrDims = pcl::traits::datatype<PointDefault, Key>::size;
272  Helper<Key, FieldT, NrDims>::copyPoint (p1_, p2_, f_idx_);
273  }
274 
275  // Copy helper for scalar fields
276  template <typename Key, typename FieldT, int NrDims>
277  struct Helper
278  {
279  static void copyPoint (const Pod &p1, float * p2, int &f_idx)
280  {
281  const std::uint8_t * data_ptr = reinterpret_cast<const std::uint8_t *> (&p1) +
282  pcl::traits::offset<PointDefault, Key>::value;
283  p2[f_idx++] = *reinterpret_cast<const FieldT*> (data_ptr);
284  }
285  };
286  // Copy helper for array fields
287  template <typename Key, typename FieldT, int NrDims>
288  struct Helper<Key, FieldT[NrDims], NrDims>
289  {
290  static void copyPoint (const Pod &p1, float * p2, int &f_idx)
291  {
292  const std::uint8_t * data_ptr = reinterpret_cast<const std::uint8_t *> (&p1) +
293  pcl::traits::offset<PointDefault, Key>::value;
294  int nr_dims = NrDims;
295  const FieldT * array = reinterpret_cast<const FieldT *> (data_ptr);
296  for (int i = 0; i < nr_dims; ++i)
297  {
298  p2[f_idx++] = array[i];
299  }
300  }
301  };
302 
303  private:
304  const Pod &p1_;
305  float * p2_;
306  int f_idx_{0};
307  };
308 
309  public:
310  // Boost shared pointers
311  using Ptr = shared_ptr<DefaultFeatureRepresentation<PointDefault>>;
312  using ConstPtr = shared_ptr<const DefaultFeatureRepresentation<PointDefault>>;
313  using FieldList = typename pcl::traits::fieldList<PointDefault>::type;
314 
316  {
317  nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented
318  pcl::for_each_type <FieldList> (IncrementFunctor (nr_dimensions_));
319  }
320 
321  inline Ptr
322  makeShared () const
323  {
324  return (Ptr (new DefaultFeatureRepresentation<PointDefault> (*this)));
325  }
326 
327  void
328  copyToFloatArray (const PointDefault &p, float * out) const override
329  {
330  pcl::for_each_type <FieldList> (NdCopyPointFunctor (p, out));
331  }
332  };
333 
334  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
335  template <>
337  {
338  public:
340  {
341  nr_dimensions_ = 3;
342  trivial_ = true;
343  }
344 
345  void
346  copyToFloatArray (const PointXYZ &p, float * out) const override
347  {
348  out[0] = p.x;
349  out[1] = p.y;
350  out[2] = p.z;
351  }
352  };
353 
354  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
355  template <>
357  {
358  public:
360  {
361  nr_dimensions_ = 3;
362  trivial_ = true;
363  }
364 
365  void
366  copyToFloatArray (const PointXYZI &p, float * out) const override
367  {
368  out[0] = p.x;
369  out[1] = p.y;
370  out[2] = p.z;
371  // By default, p.intensity is not part of the PointXYZI vectorization
372  }
373  };
374 
375  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
376  template <>
378  {
379  public:
381  {
382  nr_dimensions_ = 3;
383  trivial_ = true;
384  }
385 
386  void
387  copyToFloatArray (const PointNormal &p, float * out) const override
388  {
389  out[0] = p.x;
390  out[1] = p.y;
391  out[2] = p.z;
392  }
393  };
394 
395  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
396  template <>
398  {};
399 
400  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
401  template <>
403  {};
404 
405  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
406  template <>
408  {
409  public:
411  {
412  nr_dimensions_ = 4;
413  trivial_ = true;
414  }
415 
416  void
417  copyToFloatArray (const PPFSignature &p, float * out) const override
418  {
419  out[0] = p.f1;
420  out[1] = p.f2;
421  out[2] = p.f3;
422  out[3] = p.f4;
423  }
424  };
425 
426  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
427  template <>
429  {};
430 
431  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432  template <>
434  {};
435 
436  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
437  template <>
439  {};
440 
441  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
442  template <>
444  {};
445 
446  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
447  template <>
449  {};
450 
451  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
452  template <>
454  {
455  public:
457  {
458  nr_dimensions_ = 36;
459  trivial_=false;
460  }
461 
462  void
463  copyToFloatArray (const Narf36 &p, float * out) const override
464  {
465  for (int i = 0; i < nr_dimensions_; ++i)
466  out[i] = p.descriptor[i];
467  }
468  };
469  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
470  template <>
472  {};
473 
474  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
475  template <>
477  {
478  public:
480  {
481  nr_dimensions_ = 1980;
482  }
483 
484  void
485  copyToFloatArray (const ShapeContext1980 &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_ = 1960;
500  }
501 
502  void
503  copyToFloatArray (const UniqueShapeContext1960 &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_ = 352;
518  }
519 
520  void
521  copyToFloatArray (const SHOT352 &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  template <>
531  {
532  public:
534  {
535  nr_dimensions_ = 1344;
536  }
537 
538  void
539  copyToFloatArray (const SHOT1344 &p, float * out) const override
540  {
541  for (int i = 0; i < nr_dimensions_; ++i)
542  out[i] = p.descriptor[i];
543  }
544  };
545 
546 
547  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
548  /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
549  */
550  template <typename PointDefault>
551  class CustomPointRepresentation : public PointRepresentation <PointDefault>
552  {
554 
555  public:
556  // Boost shared pointers
557  using Ptr = shared_ptr<CustomPointRepresentation<PointDefault> >;
558  using ConstPtr = shared_ptr<const CustomPointRepresentation<PointDefault> >;
559 
560  /** \brief Constructor
561  * \param[in] max_dim the maximum number of dimensions to use
562  * \param[in] start_dim the starting dimension
563  */
564  CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0)
565  : max_dim_(max_dim), start_dim_(start_dim)
566  {
567  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
568  nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_;
569  // Limit the default representation to the first 3 elements
570  if (nr_dimensions_ > max_dim_)
572  }
573 
574  inline Ptr
575  makeShared () const
576  {
577  return Ptr (new CustomPointRepresentation<PointDefault> (*this));
578  }
579 
580  /** \brief Copy the point data into a float array
581  * \param[in] p the input point
582  * \param[out] out the resultant output array
583  */
584  void
585  copyToFloatArray (const PointDefault &p, float *out) const override
586  {
587  // If point type is unknown, treat it as a struct/array of floats
588  const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_;
589  std::copy(ptr, ptr + nr_dimensions_, out);
590  }
591 
592  protected:
593  /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */
594  int max_dim_;
595  /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */
597  };
598 }
CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
void copyToFloatArray(const PointDefault &p, float *out) const override
Copy the point data into a float array.
CustomPointRepresentation(const int max_dim=3, const int start_dim=0)
Constructor.
int max_dim_
Use at most this many dimensions (i.e.
shared_ptr< CustomPointRepresentation< PointDefault > > Ptr
shared_ptr< const CustomPointRepresentation< PointDefault > > ConstPtr
int start_dim_
Use dimensions only starting with this one (i.e.
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
shared_ptr< DefaultFeatureRepresentation< PointDefault > > Ptr
typename pcl::traits::fieldList< PointDefault >::type FieldList
void copyToFloatArray(const PointDefault &p, float *out) const override
Copy point data from input point to a float array.
shared_ptr< const DefaultFeatureRepresentation< PointDefault > > ConstPtr
void copyToFloatArray(const Narf36 &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const PPFSignature &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const PointNormal &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const PointXYZ &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const PointXYZI &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.
void copyToFloatArray(const SHOT352 &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const ShapeContext1980 &p, float *out) const override
Copy point data from input point to a float array.
void copyToFloatArray(const UniqueShapeContext1960 &p, float *out) const override
Copy point data from input point to a float array.
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
shared_ptr< DefaultPointRepresentation< PointDefault > > Ptr
shared_ptr< const DefaultPointRepresentation< PointDefault > > ConstPtr
~DefaultPointRepresentation() override=default
void copyToFloatArray(const PointDefault &p, float *out) const override
Copy point data from input point to a float array.
PointRepresentation provides a set of methods for converting a point structs/object into an n-dimensi...
void vectorize(const PointT &p, float *out) const
shared_ptr< const PointRepresentation< PointT > > ConstPtr
virtual void copyToFloatArray(const PointT &p, float *out) const =0
Copy point data from input point to a float array.
void vectorize(const PointT &p, std::vector< float > &out) const
int nr_dimensions_
The number of dimensions in this point's vector (i.e.
bool trivial_
Indicates whether this point representation is trivial.
int getNumberOfDimensions() const
Return the number of dimensions in the point's vector representation.
bool isTrivial() const
Returns whether this point representation is trivial.
void setRescaleValues(const float *rescale_array)
Set the rescale values to use when vectorizing points.
shared_ptr< PointRepresentation< PointT > > Ptr
std::vector< float > alpha_
A vector containing the rescale factor to apply to each dimension.
virtual bool isValid(const PointT &p) const
Verify that the input point is valid.
void vectorize(const PointT &p, OutputType &out) const
Convert input point into a vector representation, rescaling by alpha.
virtual ~PointRepresentation()=default
Empty destructor.
Defines all the PCL implemented PointT point type structures.
Defines functions, macros and traits for allocating and using memory.
Defines all the PCL and non-PCL macros used.
static void copyPoint(const Pod &p1, float *p2, int &f_idx)
A point structure representing the Fast Point Feature Histogram (FPFH).
A point structure representing the Globally Aligned Spatial Distribution (GASD) shape descriptor.
A point structure representing the Globally Aligned Spatial Distribution (GASD) shape and color descr...
A point structure representing the Globally Aligned Spatial Distribution (GASD) shape and color descr...
A point structure representing the Narf descriptor.
float descriptor[36]
A point structure representing the Normal Based Signature for a feature matrix of 4-by-3.
A point structure representing the Point Feature Histogram with colors (PFHRGB).
A point structure representing the Point Feature Histogram (PFH).
A point structure for storing the Point Pair Feature (PPF) values.
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape+col...
float descriptor[1344]
A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape onl...
float descriptor[352]
A point structure representing a Shape Context.
A point structure representing a Unique Shape Context.
A point structure representing the Viewpoint Feature Histogram (VFH).