Point Cloud Library (PCL)  1.14.1
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 }
void copyToFloatArray(const PointNormal &p, float *out) const override
Copy point data from input point to a float array.
A point structure representing a Shape Context.
shared_ptr< DefaultPointRepresentation< PointDefault > > Ptr
bool isTrivial() const
Returns whether this point representation is trivial.
A point structure representing the Normal Based Signature for a feature matrix of 4-by-3...
A point structure representing a Unique Shape Context.
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.
virtual bool isValid(const PointT &p) const
Verify that the input point is valid.
void copyToFloatArray(const PointDefault &p, float *out) const override
Copy point data from input point to a float array.
shared_ptr< const DefaultPointRepresentation< PointDefault > > ConstPtr
void copyToFloatArray(const PPFSignature &p, float *out) const override
Copy point data from input point to a float array.
A point structure representing the Fast Point Feature Histogram (FPFH).
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...
shared_ptr< DefaultFeatureRepresentation< PointDefault >> Ptr
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's vector (i.e.
void vectorize(const PointT &p, std::vector< float > &out) const
shared_ptr< const PointRepresentation< Narf * > > ConstPtr
shared_ptr< CustomPointRepresentation< PointDefault > > Ptr
A point structure representing Euclidean xyz coordinates.
void copyToFloatArray(const PointDefault &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...
virtual ~PointRepresentation()=default
Empty destructor.
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.
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, together with normal coordinates and the su...
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.
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...
virtual void copyToFloatArray(const PointT &p, float *out) const =0
Copy point data from input point to a float array.
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
void copyToFloatArray(const UniqueShapeContext1960 &p, float *out) const override
Copy point data from input point to a float array.
shared_ptr< const CustomPointRepresentation< PointDefault > > ConstPtr
A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape+col...
int getNumberOfDimensions() const
Return the number of dimensions in the point's vector representation.
void copyToFloatArray(const PointXYZ &p, float *out) const override
Copy point data from input point to a float array.
A point structure representing the Viewpoint Feature Histogram (VFH).
void copyToFloatArray(const ShapeContext1980 &p, float *out) const override
Copy point data from input point to a float array.
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]
shared_ptr< PointRepresentation< Narf * > > Ptr
A point structure representing the Globally Aligned Spatial Distribution (GASD) shape descriptor...
float descriptor[36]
~DefaultPointRepresentation() override=default
void copyToFloatArray(const SHOT352 &p, float *out) const override
Copy point data from input point to a float array.
A point structure for storing the Point Pair Feature (PPF) values.
typename pcl::traits::fieldList< PFHSignature125 >::type FieldList
void vectorize(const PointT &p, float *out) const