Point Cloud Library (PCL) 1.12.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>
59 class PointRepresentation
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 /** \brief Set the rescale values to use when vectorizing points
162 * \param[in] rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator.
163 */
164 void
165 setRescaleValues (const float *rescale_array)
166 {
167 alpha_.resize (nr_dimensions_);
168 std::copy_n(rescale_array, nr_dimensions_, alpha_.begin());
169 }
170
171 /** \brief Return the number of dimensions in the point's vector representation. */
172 inline int getNumberOfDimensions () const { return (nr_dimensions_); }
173 };
174
175 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
176 /** \brief @b DefaultPointRepresentation extends PointRepresentation to define default behavior for common point types.
177 */
178 template <typename PointDefault>
180 {
181 using PointRepresentation <PointDefault>::nr_dimensions_;
182 using PointRepresentation <PointDefault>::trivial_;
183
184 public:
185 // Boost shared pointers
186 using Ptr = shared_ptr<DefaultPointRepresentation<PointDefault> >;
187 using ConstPtr = shared_ptr<const DefaultPointRepresentation<PointDefault> >;
188
190 {
191 // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
192 nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
193 // Limit the default representation to the first 3 elements
194 if (nr_dimensions_ > 3) nr_dimensions_ = 3;
195
196 trivial_ = true;
197 }
198
200
201 inline Ptr
202 makeShared () const
203 {
204 return (Ptr (new DefaultPointRepresentation<PointDefault> (*this)));
205 }
206
207 void
208 copyToFloatArray (const PointDefault &p, float * out) const override
209 {
210 // If point type is unknown, treat it as a struct/array of floats
211 const float* ptr = reinterpret_cast<const float*> (&p);
212 std::copy_n(ptr, nr_dimensions_, out);
213 }
214 };
215
216 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
217 /** \brief @b DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the
218 * default behavior for feature descriptor types (i.e., copy each element of each field into a float array).
219 */
220 template <typename PointDefault>
222 {
223 protected:
224 using PointRepresentation <PointDefault>::nr_dimensions_;
225
226 private:
227 struct IncrementFunctor
228 {
229 IncrementFunctor (int &n) : n_ (n)
230 {
231 n_ = 0;
232 }
233
234 template<typename Key> inline void operator () ()
235 {
236 n_ += pcl::traits::datatype<PointDefault, Key>::size;
237 }
238
239 private:
240 int &n_;
241 };
242
243 struct NdCopyPointFunctor
244 {
245 using Pod = typename traits::POD<PointDefault>::type;
246
247 NdCopyPointFunctor (const PointDefault &p1, float * p2)
248 : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
249
250 template<typename Key> inline void operator() ()
251 {
252 using FieldT = typename pcl::traits::datatype<PointDefault, Key>::type;
253 const int NrDims = pcl::traits::datatype<PointDefault, Key>::size;
255 }
256
257 // Copy helper for scalar fields
258 template <typename Key, typename FieldT, int NrDims>
259 struct Helper
260 {
261 static void copyPoint (const Pod &p1, float * p2, int &f_idx)
262 {
263 const std::uint8_t * data_ptr = reinterpret_cast<const std::uint8_t *> (&p1) +
264 pcl::traits::offset<PointDefault, Key>::value;
265 p2[f_idx++] = *reinterpret_cast<const FieldT*> (data_ptr);
266 }
267 };
268 // Copy helper for array fields
269 template <typename Key, typename FieldT, int NrDims>
270 struct Helper<Key, FieldT[NrDims], NrDims>
271 {
272 static void copyPoint (const Pod &p1, float * p2, int &f_idx)
273 {
274 const std::uint8_t * data_ptr = reinterpret_cast<const std::uint8_t *> (&p1) +
275 pcl::traits::offset<PointDefault, Key>::value;
276 int nr_dims = NrDims;
277 const FieldT * array = reinterpret_cast<const FieldT *> (data_ptr);
278 for (int i = 0; i < nr_dims; ++i)
279 {
280 p2[f_idx++] = array[i];
281 }
282 }
283 };
284
285 private:
286 const Pod &p1_;
287 float * p2_;
288 int f_idx_;
289 };
290
291 public:
292 // Boost shared pointers
293 using Ptr = shared_ptr<DefaultFeatureRepresentation<PointDefault>>;
294 using ConstPtr = shared_ptr<const DefaultFeatureRepresentation<PointDefault>>;
295 using FieldList = typename pcl::traits::fieldList<PointDefault>::type;
296
298 {
299 nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented
301 }
302
303 inline Ptr
304 makeShared () const
305 {
306 return (Ptr (new DefaultFeatureRepresentation<PointDefault> (*this)));
307 }
308
309 void
310 copyToFloatArray (const PointDefault &p, float * out) const override
311 {
312 pcl::for_each_type <FieldList> (NdCopyPointFunctor (p, out));
313 }
314 };
315
316 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
317 template <>
319 {
320 public:
322 {
323 nr_dimensions_ = 3;
324 trivial_ = true;
325 }
326
327 void
328 copyToFloatArray (const PointXYZ &p, float * out) const override
329 {
330 out[0] = p.x;
331 out[1] = p.y;
332 out[2] = p.z;
333 }
334 };
335
336 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
337 template <>
339 {
340 public:
342 {
343 nr_dimensions_ = 3;
344 trivial_ = true;
345 }
346
347 void
348 copyToFloatArray (const PointXYZI &p, float * out) const override
349 {
350 out[0] = p.x;
351 out[1] = p.y;
352 out[2] = p.z;
353 // By default, p.intensity is not part of the PointXYZI vectorization
354 }
355 };
356
357 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
358 template <>
360 {
361 public:
363 {
364 nr_dimensions_ = 3;
365 trivial_ = true;
366 }
367
368 void
369 copyToFloatArray (const PointNormal &p, float * out) const override
370 {
371 out[0] = p.x;
372 out[1] = p.y;
373 out[2] = p.z;
374 }
375 };
376
377 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
378 template <>
381
382 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
383 template <>
386
387 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
388 template <>
390 {
391 public:
393 {
394 nr_dimensions_ = 4;
395 trivial_ = true;
396 }
397
398 void
399 copyToFloatArray (const PPFSignature &p, float * out) const override
400 {
401 out[0] = p.f1;
402 out[1] = p.f2;
403 out[2] = p.f3;
404 out[3] = p.f4;
405 }
406 };
407
408 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
409 template <>
412
413 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
414 template <>
417
418 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
419 template <>
422
423 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
424 template <>
427
428 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
429 template <>
432
433 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
434 template <>
436 {
437 public:
439 {
440 nr_dimensions_ = 36;
441 trivial_=false;
442 }
443
444 void
445 copyToFloatArray (const Narf36 &p, float * out) const override
446 {
447 for (int i = 0; i < nr_dimensions_; ++i)
448 out[i] = p.descriptor[i];
449 }
450 };
451 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
452 template <>
455
456 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
457 template <>
459 {
460 public:
462 {
463 nr_dimensions_ = 1980;
464 }
465
466 void
467 copyToFloatArray (const ShapeContext1980 &p, float * out) const override
468 {
469 for (int i = 0; i < nr_dimensions_; ++i)
470 out[i] = p.descriptor[i];
471 }
472 };
473
474 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
475 template <>
477 {
478 public:
480 {
481 nr_dimensions_ = 1960;
482 }
483
484 void
485 copyToFloatArray (const UniqueShapeContext1960 &p, float * out) const override
486 {
487 for (int i = 0; i < nr_dimensions_; ++i)
488 out[i] = p.descriptor[i];
489 }
490 };
491
492 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
493 template <>
495 {
496 public:
501
502 void
503 copyToFloatArray (const SHOT352 &p, float * out) const override
504 {
505 for (int i = 0; i < nr_dimensions_; ++i)
506 out[i] = p.descriptor[i];
507 }
508 };
509
510 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
511 template <>
513 {
514 public:
516 {
517 nr_dimensions_ = 1344;
518 }
519
520 void
521 copyToFloatArray (const SHOT1344 &p, float * out) const override
522 {
523 for (int i = 0; i < nr_dimensions_; ++i)
524 out[i] = p.descriptor[i];
525 }
526 };
527
528
529 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
530 /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
531 */
532 template <typename PointDefault>
533 class CustomPointRepresentation : public PointRepresentation <PointDefault>
534 {
535 using PointRepresentation <PointDefault>::nr_dimensions_;
536
537 public:
538 // Boost shared pointers
539 using Ptr = shared_ptr<CustomPointRepresentation<PointDefault> >;
540 using ConstPtr = shared_ptr<const CustomPointRepresentation<PointDefault> >;
541
542 /** \brief Constructor
543 * \param[in] max_dim the maximum number of dimensions to use
544 * \param[in] start_dim the starting dimension
545 */
546 CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0)
547 : max_dim_(max_dim), start_dim_(start_dim)
548 {
549 // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
550 nr_dimensions_ = static_cast<int> (sizeof (PointDefault) / sizeof (float)) - start_dim_;
551 // Limit the default representation to the first 3 elements
554 }
555
556 inline Ptr
557 makeShared () const
558 {
559 return Ptr (new CustomPointRepresentation<PointDefault> (*this));
560 }
561
562 /** \brief Copy the point data into a float array
563 * \param[in] p the input point
564 * \param[out] out the resultant output array
565 */
566 virtual void
567 copyToFloatArray (const PointDefault &p, float *out) const
568 {
569 // If point type is unknown, treat it as a struct/array of floats
570 const float *ptr = (reinterpret_cast<const float*> (&p)) + start_dim_;
571 std::copy_n(ptr, nr_dimensions_, out);
572 }
573
574 protected:
575 /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */
577 /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */
579 };
580}
CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
virtual void copyToFloatArray(const PointDefault &p, float *out) const
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< const DefaultFeatureRepresentation< PointDefault > > ConstPtr
typename pcl::traits::fieldList< PointDefault >::type FieldList
shared_ptr< DefaultFeatureRepresentation< PointDefault > > Ptr
void copyToFloatArray(const PointDefault &p, float *out) const override
void copyToFloatArray(const Narf36 &p, float *out) const override
void copyToFloatArray(const PPFSignature &p, float *out) const override
void copyToFloatArray(const PointNormal &p, float *out) const override
void copyToFloatArray(const PointXYZ &p, float *out) const override
void copyToFloatArray(const PointXYZI &p, float *out) const override
void copyToFloatArray(const SHOT1344 &p, float *out) const override
void copyToFloatArray(const SHOT352 &p, float *out) const override
void copyToFloatArray(const ShapeContext1980 &p, float *out) const override
void copyToFloatArray(const UniqueShapeContext1960 &p, float *out) const override
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
shared_ptr< DefaultPointRepresentation< PointDefault > > Ptr
shared_ptr< const DefaultPointRepresentation< PointDefault > > ConstPtr
void copyToFloatArray(const PointDefault &p, float *out) const override
PointRepresentation provides a set of methods for converting a point structs/object into an n-dimensi...
Definition kdtree.h:48
bool isTrivial() const
Returns whether this point representation is trivial.
shared_ptr< const PointRepresentation< PointT > > ConstPtr
int getNumberOfDimensions() const
Return the number of dimensions in the point's vector representation.
int nr_dimensions_
The number of dimensions in this point's vector (i.e.
virtual ~PointRepresentation()=default
Empty destructor.
void setRescaleValues(const float *rescale_array)
Set the rescale values to use when vectorizing points.
void vectorize(const PointT &p, OutputType &out) const
Convert input point into a vector representation, rescaling by alpha.
virtual void copyToFloatArray(const PointT &p, float *out) const =0
Copy point data from input point to a float array.
std::vector< float > alpha_
A vector containing the rescale factor to apply to each dimension.
shared_ptr< PointRepresentation< PointT > > Ptr
virtual bool isValid(const PointT &p) const
Verify that the input point is valid.
bool trivial_
Indicates whether this point representation is trivial.
Defines all the PCL implemented PointT point type structures.
Defines functions, macros and traits for allocating and using memory.
void for_each_type(F f)
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).