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 <algorithm>
43 
44 #include <pcl/point_types.h>
45 #include <pcl/pcl_macros.h>
46 #include <pcl/for_each_type.h>
47 
48 namespace pcl
49 {
50  /** \brief @b PointRepresentation provides a set of methods for converting a point structs/object into an
51  * n-dimensional vector.
52  * \note This is an abstract class. Subclasses must set nr_dimensions_ to the appropriate value in the constructor
53  * and provide an implementation of the pure virtual copyToFloatArray method.
54  * \author Michael Dixon
55  */
56  template <typename PointT>
58  {
59  protected:
60  /** \brief The number of dimensions in this point's vector (i.e. the "k" in "k-D") */
61  int nr_dimensions_ = 0;
62  /** \brief A vector containing the rescale factor to apply to each dimension. */
63  std::vector<float> alpha_;
64  /** \brief Indicates whether this point representation is trivial. It is trivial if and only if the following
65  * conditions hold:
66  * - the relevant data consists only of float values
67  * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array
68  * - sizeof(PointT) is a multiple of sizeof(float)
69  * In short, a trivial point representation converts the input point to a float array that is the same as if
70  * the point was reinterpret_casted to a float array of length nr_dimensions_ . This value says that this
71  * representation can be trivial; it is only trivial if setRescaleValues() has not been set.
72  */
73  bool trivial_ = false;
74 
75  public:
76  using Ptr = boost::shared_ptr<PointRepresentation<PointT> >;
77  using ConstPtr = boost::shared_ptr<const PointRepresentation<PointT> >;
78 
79  /** \brief Empty destructor */
80  virtual ~PointRepresentation () = default;
81  //TODO: check if copy and move constructors / assignment operators are needed
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  std::copy_n(rescale_array, nr_dimensions_, alpha_.begin());
167  }
168 
169  /** \brief Return the number of dimensions in the point's vector representation. */
170  inline int getNumberOfDimensions () const { return (nr_dimensions_); }
171  };
172 
173  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174  /** \brief @b DefaultPointRepresentation extends PointRepresentation to define default behavior for common point types.
175  */
176  template <typename PointDefault>
177  class DefaultPointRepresentation : public PointRepresentation <PointDefault>
178  {
181 
182  public:
183  // Boost shared pointers
184  using Ptr = boost::shared_ptr<DefaultPointRepresentation<PointDefault> >;
185  using ConstPtr = boost::shared_ptr<const DefaultPointRepresentation<PointDefault> >;
186 
188  {
189  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
190  nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
191  // Limit the default representation to the first 3 elements
192  if (nr_dimensions_ > 3) nr_dimensions_ = 3;
193 
194  trivial_ = true;
195  }
196 
198 
199  inline Ptr
200  makeShared () const
201  {
202  return (Ptr (new DefaultPointRepresentation<PointDefault> (*this)));
203  }
204 
205  void
206  copyToFloatArray (const PointDefault &p, float * out) const override
207  {
208  // If point type is unknown, treat it as a struct/array of floats
209  const float* ptr = reinterpret_cast<const float*> (&p);
210  std::copy_n(ptr, nr_dimensions_, out);
211  }
212  };
213 
214  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215  /** \brief @b DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the
216  * default behavior for feature descriptor types (i.e., copy each element of each field into a float array).
217  */
218  template <typename PointDefault>
219  class DefaultFeatureRepresentation : public PointRepresentation <PointDefault>
220  {
221  protected:
223 
224  private:
225  struct IncrementFunctor
226  {
227  IncrementFunctor (int &n) : n_ (n)
228  {
229  n_ = 0;
230  }
231 
232  template<typename Key> inline void operator () ()
233  {
235  }
236 
237  private:
238  int &n_;
239  };
240 
241  struct NdCopyPointFunctor
242  {
243  using Pod = typename traits::POD<PointDefault>::type;
244 
245  NdCopyPointFunctor (const PointDefault &p1, float * p2)
246  : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
247 
248  template<typename Key> inline void operator() ()
249  {
250  using FieldT = typename pcl::traits::datatype<PointDefault, Key>::type;
252  Helper<Key, FieldT, NrDims>::copyPoint (p1_, p2_, f_idx_);
253  }
254 
255  // Copy helper for scalar fields
256  template <typename Key, typename FieldT, int NrDims>
257  struct Helper
258  {
259  static void copyPoint (const Pod &p1, float * p2, int &f_idx)
260  {
261  const std::uint8_t * data_ptr = reinterpret_cast<const std::uint8_t *> (&p1) +
263  p2[f_idx++] = *reinterpret_cast<const FieldT*> (data_ptr);
264  }
265  };
266  // Copy helper for array fields
267  template <typename Key, typename FieldT, int NrDims>
268  struct Helper<Key, FieldT[NrDims], NrDims>
269  {
270  static void copyPoint (const Pod &p1, float * p2, int &f_idx)
271  {
272  const std::uint8_t * data_ptr = reinterpret_cast<const std::uint8_t *> (&p1) +
274  int nr_dims = NrDims;
275  const FieldT * array = reinterpret_cast<const FieldT *> (data_ptr);
276  for (int i = 0; i < nr_dims; ++i)
277  {
278  p2[f_idx++] = array[i];
279  }
280  }
281  };
282 
283  private:
284  const Pod &p1_;
285  float * p2_;
286  int f_idx_;
287  };
288 
289  public:
290  // Boost shared pointers
291  using Ptr = typename boost::shared_ptr<DefaultFeatureRepresentation<PointDefault> >;
292  using ConstPtr = typename boost::shared_ptr<const DefaultFeatureRepresentation<PointDefault> >;
294 
296  {
297  nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented
298  pcl::for_each_type <FieldList> (IncrementFunctor (nr_dimensions_));
299  }
300 
301  inline Ptr
302  makeShared () const
303  {
304  return (Ptr (new DefaultFeatureRepresentation<PointDefault> (*this)));
305  }
306 
307  void
308  copyToFloatArray (const PointDefault &p, float * out) const override
309  {
310  pcl::for_each_type <FieldList> (NdCopyPointFunctor (p, out));
311  }
312  };
313 
314  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
315  template <>
317  {
318  public:
320  {
321  nr_dimensions_ = 3;
322  trivial_ = true;
323  }
324 
325  void
326  copyToFloatArray (const PointXYZ &p, float * out) const override
327  {
328  out[0] = p.x;
329  out[1] = p.y;
330  out[2] = p.z;
331  }
332  };
333 
334  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
335  template <>
337  {
338  public:
340  {
341  nr_dimensions_ = 3;
342  trivial_ = true;
343  }
344 
345  void
346  copyToFloatArray (const PointXYZI &p, float * out) const override
347  {
348  out[0] = p.x;
349  out[1] = p.y;
350  out[2] = p.z;
351  // By default, p.intensity is not part of the PointXYZI vectorization
352  }
353  };
354 
355  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
356  template <>
358  {
359  public:
361  {
362  nr_dimensions_ = 3;
363  trivial_ = true;
364  }
365 
366  void
367  copyToFloatArray (const PointNormal &p, float * out) const override
368  {
369  out[0] = p.x;
370  out[1] = p.y;
371  out[2] = p.z;
372  }
373  };
374 
375  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
376  template <>
378  {};
379 
380  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
381  template <>
383  {};
384 
385  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
386  template <>
388  {
389  public:
391  {
392  nr_dimensions_ = 4;
393  trivial_ = true;
394  }
395 
396  void
397  copyToFloatArray (const PPFSignature &p, float * out) const override
398  {
399  out[0] = p.f1;
400  out[1] = p.f2;
401  out[2] = p.f3;
402  out[3] = p.f4;
403  }
404  };
405 
406  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
407  template <>
409  {};
410 
411  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
412  template <>
414  {};
415 
416  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
417  template <>
419  {};
420 
421  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
422  template <>
424  {};
425 
426  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
427  template <>
429  {};
430 
431  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432  template <>
434  {
435  public:
437  {
438  nr_dimensions_ = 36;
439  trivial_=false;
440  }
441 
442  void
443  copyToFloatArray (const Narf36 &p, float * out) const override
444  {
445  for (int i = 0; i < nr_dimensions_; ++i)
446  out[i] = p.descriptor[i];
447  }
448  };
449  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
450  template <>
452  {};
453 
454  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
455  template <>
457  {
458  public:
460  {
461  nr_dimensions_ = 1980;
462  }
463 
464  void
465  copyToFloatArray (const ShapeContext1980 &p, float * out) const override
466  {
467  for (int i = 0; i < nr_dimensions_; ++i)
468  out[i] = p.descriptor[i];
469  }
470  };
471 
472  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
473  template <>
475  {
476  public:
478  {
479  nr_dimensions_ = 1960;
480  }
481 
482  void
483  copyToFloatArray (const UniqueShapeContext1960 &p, float * out) const override
484  {
485  for (int i = 0; i < nr_dimensions_; ++i)
486  out[i] = p.descriptor[i];
487  }
488  };
489 
490  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
491  template <>
493  {
494  public:
496  {
497  nr_dimensions_ = 352;
498  }
499 
500  void
501  copyToFloatArray (const SHOT352 &p, float * out) const override
502  {
503  for (int i = 0; i < nr_dimensions_; ++i)
504  out[i] = p.descriptor[i];
505  }
506  };
507 
508  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
509  template <>
511  {
512  public:
514  {
515  nr_dimensions_ = 1344;
516  }
517 
518  void
519  copyToFloatArray (const SHOT1344 &p, float * out) const override
520  {
521  for (int i = 0; i < nr_dimensions_; ++i)
522  out[i] = p.descriptor[i];
523  }
524  };
525 
526 
527  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
528  /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
529  */
530  template <typename PointDefault>
531  class CustomPointRepresentation : public PointRepresentation <PointDefault>
532  {
534 
535  public:
536  // Boost shared pointers
537  using Ptr = boost::shared_ptr<CustomPointRepresentation<PointDefault> >;
538  using ConstPtr = boost::shared_ptr<const CustomPointRepresentation<PointDefault> >;
539 
540  /** \brief Constructor
541  * \param[in] max_dim the maximum number of dimensions to use
542  * \param[in] start_dim the starting dimension
543  */
544  CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0)
545  : max_dim_(max_dim), start_dim_(start_dim)
546  {
547  // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
548  nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_;
549  // Limit the default representation to the first 3 elements
550  if (nr_dimensions_ > max_dim_)
551  nr_dimensions_ = max_dim_;
552  }
553 
554  inline Ptr
555  makeShared () const
556  {
557  return Ptr (new CustomPointRepresentation<PointDefault> (*this));
558  }
559 
560  /** \brief Copy the point data into a float array
561  * \param[in] p the input point
562  * \param[out] out the resultant output array
563  */
564  virtual void
565  copyToFloatArray (const PointDefault &p, float *out) const
566  {
567  // If point type is unknown, treat it as a struct/array of floats
568  const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_;
569  std::copy_n(ptr, nr_dimensions_, out);
570  }
571 
572  protected:
573  /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */
574  int max_dim_;
575  /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */
577  };
578 }
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.
void vectorize(const PointT &p, OutputType &out) const
Convert input point into a vector representation, rescaling by alpha.
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
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.
int getNumberOfDimensions() const
Return the number of dimensions in the point&#39;s vector representation.
boost::shared_ptr< const PointRepresentation< SHOT352 > > 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 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.
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.
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.
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]
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
Defines all the PCL and non-PCL macros used.
boost::shared_ptr< PointRepresentation< SHOT352 > > 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.