Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
50namespace 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") */
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>
198 {
199 using PointRepresentation <PointDefault>::nr_dimensions_;
200 using PointRepresentation <PointDefault>::trivial_;
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>
240 {
241 protected:
242 using PointRepresentation <PointDefault>::nr_dimensions_;
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;
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 <>
399
400 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
401 template <>
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 <>
430
431 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432 template <>
435
436 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
437 template <>
440
441 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
442 template <>
445
446 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
447 template <>
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 <>
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:
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 {
553 using PointRepresentation <PointDefault>::nr_dimensions_;
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
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 */
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
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).