Point Cloud Library (PCL)  1.11.0
point_cloud.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #pragma once
40 
41 #ifdef __GNUC__
42 #pragma GCC system_header
43 #endif
44 
45 #include <Eigen/StdVector>
46 #include <Eigen/Geometry>
47 #include <pcl/PCLHeader.h>
48 #include <pcl/exceptions.h>
49 #include <pcl/memory.h>
50 #include <pcl/pcl_macros.h>
51 #include <pcl/type_traits.h>
52 
53 #include <algorithm>
54 #include <utility>
55 #include <vector>
56 
57 namespace pcl
58 {
59  namespace detail
60  {
61  struct FieldMapping
62  {
63  std::size_t serialized_offset;
64  std::size_t struct_offset;
65  std::size_t size;
66  };
67  } // namespace detail
68 
69  // Forward declarations
70  template <typename PointT> class PointCloud;
71  using MsgFieldMap = std::vector<detail::FieldMapping>;
72 
73  /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */
74  template <typename PointOutT>
76  {
78 
79  /** \brief Constructor
80  * \param[in] p1 the input Eigen type
81  * \param[out] p2 the output Point type
82  */
83  NdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointOutT &p2)
84  : p1_ (p1),
85  p2_ (reinterpret_cast<Pod&>(p2)),
86  f_idx_ (0) { }
87 
88  /** \brief Operator. Data copy happens here. */
89  template<typename Key> inline void
91  {
92  //boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++];
94  std::uint8_t* data_ptr = reinterpret_cast<std::uint8_t*>(&p2_) + pcl::traits::offset<PointOutT, Key>::value;
95  *reinterpret_cast<T*>(data_ptr) = static_cast<T> (p1_[f_idx_++]);
96  }
97 
98  private:
99  const Eigen::VectorXf &p1_;
100  Pod &p2_;
101  int f_idx_;
102  public:
104  };
105 
106  /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */
107  template <typename PointInT>
109  {
111 
112  /** \brief Constructor
113  * \param[in] p1 the input Point type
114  * \param[out] p2 the output Eigen type
115  */
116  NdCopyPointEigenFunctor (const PointInT &p1, Eigen::VectorXf &p2)
117  : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
118 
119  /** \brief Operator. Data copy happens here. */
120  template<typename Key> inline void
122  {
123  //p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_);
125  const std::uint8_t* data_ptr = reinterpret_cast<const std::uint8_t*>(&p1_) + pcl::traits::offset<PointInT, Key>::value;
126  p2_[f_idx_++] = static_cast<float> (*reinterpret_cast<const T*>(data_ptr));
127  }
128 
129  private:
130  const Pod &p1_;
131  Eigen::VectorXf &p2_;
132  int f_idx_;
133  public:
135  };
136 
137  namespace detail
138  {
139  template <typename PointT>
140  PCL_DEPRECATED(1, 12, "use createMapping() instead")
141  shared_ptr<pcl::MsgFieldMap>&
143  } // namespace detail
144 
145  /** \brief PointCloud represents the base class in PCL for storing collections of 3D points.
146  *
147  * The class is templated, which means you need to specify the type of data
148  * that it should contain. For example, to create a point cloud that holds 4
149  * random XYZ data points, use:
150  *
151  * \code
152  * pcl::PointCloud<pcl::PointXYZ> cloud;
153  * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
154  * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
155  * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
156  * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
157  * \endcode
158  *
159  * The PointCloud class contains the following elements:
160  * - \b width - specifies the width of the point cloud dataset in the number of points. WIDTH has two meanings:
161  * - it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets;
162  * - it can specify the width (total number of points in a row) of an organized point cloud dataset.
163  * \a Mandatory.
164  * - \b height - specifies the height of the point cloud dataset in the number of points. HEIGHT has two meanings:
165  * - it can specify the height (total number of rows) of an organized point cloud dataset;
166  * - it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not).
167  * \a Mandatory.
168  * - \b points - the data array where all points of type <b>PointT</b> are stored. \a Mandatory.
169  *
170  * - \b is_dense - specifies if all the data in <b>points</b> is finite (true), or whether it might contain Inf/NaN values
171  * (false). \a Mandatory.
172  *
173  * - \b sensor_origin_ - specifies the sensor acquisition pose (origin/translation). \a Optional.
174  * - \b sensor_orientation_ - specifies the sensor acquisition pose (rotation). \a Optional.
175  *
176  * \author Patrick Mihelich, Radu B. Rusu
177  */
178  template <typename PointT>
179  class PCL_EXPORTS PointCloud
180  {
181  public:
182  /** \brief Default constructor. Sets \ref is_dense to true, \ref width
183  * and \ref height to 0, and the \ref sensor_origin_ and \ref
184  * sensor_orientation_ to identity.
185  */
186  PointCloud () = default;
187 
188  /** \brief Copy constructor.
189  * \param[in] pc the cloud to copy into this
190  * \todo Erase once mapping_ is removed.
191  */
192  // Ignore unknown pragma warning on MSVC (4996)
193  #pragma warning(push)
194  #pragma warning(disable: 4068)
195  // Ignore deprecated warning on clang compilers
196  #pragma clang diagnostic push
197  #pragma clang diagnostic ignored "-Wdeprecated-declarations"
198  PointCloud (const PointCloud<PointT> &pc) = default;
199  #pragma clang diagnostic pop
200  #pragma warning(pop)
201 
202  /** \brief Copy constructor from point cloud subset
203  * \param[in] pc the cloud to copy into this
204  * \param[in] indices the subset to copy
205  */
207  const std::vector<int> &indices) :
208  header (pc.header), points (indices.size ()), width (indices.size ()), height (1), is_dense (pc.is_dense),
209  sensor_origin_ (pc.sensor_origin_), sensor_orientation_ (pc.sensor_orientation_)
210  {
211  // Copy the obvious
212  assert (indices.size () <= pc.size ());
213  for (std::size_t i = 0; i < indices.size (); i++)
214  points[i] = pc.points[indices[i]];
215  }
216 
217  /** \brief Allocate constructor from point cloud subset
218  * \param[in] width_ the cloud width
219  * \param[in] height_ the cloud height
220  * \param[in] value_ default value
221  */
222  PointCloud (std::uint32_t width_, std::uint32_t height_, const PointT& value_ = PointT ())
223  : points (width_ * height_, value_)
224  , width (width_)
225  , height (height_)
226  {}
227 
228  //TODO: check if copy/move contructors/assignment operators are needed
229 
230  /** \brief Add a point cloud to the current cloud.
231  * \param[in] rhs the cloud to add to the current cloud
232  * \return the new cloud as a concatenation of the current cloud and the new given cloud
233  */
234  inline PointCloud&
235  operator += (const PointCloud& rhs)
236  {
237  concatenate((*this), rhs);
238  return (*this);
239  }
240 
241  /** \brief Add a point cloud to another cloud.
242  * \param[in] rhs the cloud to add to the current cloud
243  * \return the new cloud as a concatenation of the current cloud and the new given cloud
244  */
245  inline PointCloud
246  operator + (const PointCloud& rhs)
247  {
248  return (PointCloud (*this) += rhs);
249  }
250 
251  inline static bool
253  const pcl::PointCloud<PointT> &cloud2)
254  {
255  // Make the resultant point cloud take the newest stamp
256  cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);
257 
258  // libstdc++ (GCC) on calling reserve allocates new memory, copies and deallocates old vector
259  // This causes a drastic performance hit. Prefer not to use reserve with libstdc++ (default on clang)
260  cloud1.points.insert (cloud1.points.end (), cloud2.points.begin (), cloud2.points.end ());
261 
262  cloud1.width = static_cast<std::uint32_t>(cloud1.points.size ());
263  cloud1.height = 1;
264  cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
265  return true;
266  }
267 
268  inline static bool
270  const pcl::PointCloud<PointT> &cloud2,
271  pcl::PointCloud<PointT> &cloud_out)
272  {
273  cloud_out = cloud1;
274  return concatenate(cloud_out, cloud2);
275  }
276 
277  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
278  * datasets (those that have height != 1).
279  * \param[in] column the column coordinate
280  * \param[in] row the row coordinate
281  */
282  inline const PointT&
283  at (int column, int row) const
284  {
285  if (this->height > 1)
286  return (points.at (row * this->width + column));
287  else
288  throw UnorganizedPointCloudException ("Can't use 2D indexing with an unorganized point cloud");
289  }
290 
291  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
292  * datasets (those that have height != 1).
293  * \param[in] column the column coordinate
294  * \param[in] row the row coordinate
295  */
296  inline PointT&
297  at (int column, int row)
298  {
299  if (this->height > 1)
300  return (points.at (row * this->width + column));
301  else
302  throw UnorganizedPointCloudException ("Can't use 2D indexing with an unorganized point cloud");
303  }
304 
305  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
306  * datasets (those that have height != 1).
307  * \param[in] column the column coordinate
308  * \param[in] row the row coordinate
309  */
310  inline const PointT&
311  operator () (std::size_t column, std::size_t row) const
312  {
313  return (points[row * this->width + column]);
314  }
315 
316  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
317  * datasets (those that have height != 1).
318  * \param[in] column the column coordinate
319  * \param[in] row the row coordinate
320  */
321  inline PointT&
322  operator () (std::size_t column, std::size_t row)
323  {
324  return (points[row * this->width + column]);
325  }
326 
327  /** \brief Return whether a dataset is organized (e.g., arranged in a structured grid).
328  * \note The height value must be different than 1 for a dataset to be organized.
329  */
330  inline bool
331  isOrganized () const
332  {
333  return (height > 1);
334  }
335 
336  /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
337  * \note This method is for advanced users only! Use with care!
338  *
339  * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
340  * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure,
341  * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix
342  *
343  * \param[in] dim the number of dimensions to consider for each point
344  * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns)
345  * \param[in] offset the number of dimensions to skip from the beginning of each point
346  * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point)
347  * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment.
348  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
349  */
350  inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
351  getMatrixXfMap (int dim, int stride, int offset)
352  {
353  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
354  return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, points.size (), dim, Eigen::OuterStride<> (stride)));
355  else
356  return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, dim, points.size (), Eigen::OuterStride<> (stride)));
357  }
358 
359  /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
360  * \note This method is for advanced users only! Use with care!
361  *
362  * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
363  * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure,
364  * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix
365  *
366  * \param[in] dim the number of dimensions to consider for each point
367  * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns)
368  * \param[in] offset the number of dimensions to skip from the beginning of each point
369  * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point)
370  * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment.
371  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
372  */
373  inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
374  getMatrixXfMap (int dim, int stride, int offset) const
375  {
376  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
377  return (Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(const_cast<PointT*>(&points[0]))+offset, points.size (), dim, Eigen::OuterStride<> (stride)));
378  else
379  return (Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(const_cast<PointT*>(&points[0]))+offset, dim, points.size (), Eigen::OuterStride<> (stride)));
380  }
381 
382  /**
383  * \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
384  * \note This method is for advanced users only! Use with care!
385  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
386  * \overload Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > pcl::PointCloud::getMatrixXfMap ()
387  */
388  inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
390  {
391  return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0));
392  }
393 
394  /**
395  * \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
396  * \note This method is for advanced users only! Use with care!
397  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
398  * \overload const Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > pcl::PointCloud::getMatrixXfMap () const
399  */
400  inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
401  getMatrixXfMap () const
402  {
403  return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0));
404  }
405 
406  /** \brief The point cloud header. It contains information about the acquisition time. */
408 
409  /** \brief The point data. */
410  std::vector<PointT, Eigen::aligned_allocator<PointT> > points;
411 
412  /** \brief The point cloud width (if organized as an image-structure). */
413  std::uint32_t width = 0;
414  /** \brief The point cloud height (if organized as an image-structure). */
415  std::uint32_t height = 0;
416 
417  /** \brief True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields). */
418  bool is_dense = true;
419 
420  /** \brief Sensor acquisition pose (origin/translation). */
421  Eigen::Vector4f sensor_origin_ = Eigen::Vector4f::Zero ();
422  /** \brief Sensor acquisition pose (rotation). */
423  Eigen::Quaternionf sensor_orientation_ = Eigen::Quaternionf::Identity ();
424 
425  using PointType = PointT; // Make the template class available from the outside
426  using VectorType = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
427  using CloudVectorType = std::vector<PointCloud<PointT>, Eigen::aligned_allocator<PointCloud<PointT> > >;
428  using Ptr = shared_ptr<PointCloud<PointT> >;
429  using ConstPtr = shared_ptr<const PointCloud<PointT> >;
430 
431  // std container compatibility typedefs according to
432  // http://en.cppreference.com/w/cpp/concept/Container
434  using reference = PointT&;
435  using const_reference = const PointT&;
436  using difference_type = typename VectorType::difference_type;
437  using size_type = typename VectorType::size_type;
438 
439  // iterators
440  using iterator = typename VectorType::iterator;
441  using const_iterator = typename VectorType::const_iterator;
442  inline iterator begin () { return (points.begin ()); }
443  inline iterator end () { return (points.end ()); }
444  inline const_iterator begin () const { return (points.begin ()); }
445  inline const_iterator end () const { return (points.end ()); }
446 
447  //capacity
448  inline std::size_t size () const { return (points.size ()); }
449  inline void reserve (std::size_t n) { points.reserve (n); }
450  inline bool empty () const { return points.empty (); }
451 
452  /** \brief Resize the cloud
453  * \param[in] n the new cloud size
454  */
455  inline void resize (std::size_t n)
456  {
457  points.resize (n);
458  if (width * height != n)
459  {
460  width = static_cast<std::uint32_t> (n);
461  height = 1;
462  }
463  }
464 
465  //element access
466  inline const PointT& operator[] (std::size_t n) const { return (points[n]); }
467  inline PointT& operator[] (std::size_t n) { return (points[n]); }
468  inline const PointT& at (std::size_t n) const { return (points.at (n)); }
469  inline PointT& at (std::size_t n) { return (points.at (n)); }
470  inline const PointT& front () const { return (points.front ()); }
471  inline PointT& front () { return (points.front ()); }
472  inline const PointT& back () const { return (points.back ()); }
473  inline PointT& back () { return (points.back ()); }
474 
475  /** \brief Insert a new point in the cloud, at the end of the container.
476  * \note This breaks the organized structure of the cloud by setting the height to 1!
477  * \param[in] pt the point to insert
478  */
479  inline void
480  push_back (const PointT& pt)
481  {
482  points.push_back (pt);
483  width = static_cast<std::uint32_t> (points.size ());
484  height = 1;
485  }
486 
487  /** \brief Emplace a new point in the cloud, at the end of the container.
488  * \note This breaks the organized structure of the cloud by setting the height to 1!
489  * \param[in] args the parameters to forward to the point to construct
490  * \return reference to the emplaced point
491  */
492  template <class... Args> inline reference
493  emplace_back (Args&& ...args)
494  {
495  points.emplace_back (std::forward<Args> (args)...);
496  width = static_cast<std::uint32_t> (points.size ());
497  height = 1;
498  return points.back();
499  }
500 
501  /** \brief Insert a new point in the cloud, given an iterator.
502  * \note This breaks the organized structure of the cloud by setting the height to 1!
503  * \param[in] position where to insert the point
504  * \param[in] pt the point to insert
505  * \return returns the new position iterator
506  */
507  inline iterator
508  insert (iterator position, const PointT& pt)
509  {
510  iterator it = points.insert (position, pt);
511  width = static_cast<std::uint32_t> (points.size ());
512  height = 1;
513  return (it);
514  }
515 
516  /** \brief Insert a new point in the cloud N times, given an iterator.
517  * \note This breaks the organized structure of the cloud by setting the height to 1!
518  * \param[in] position where to insert the point
519  * \param[in] n the number of times to insert the point
520  * \param[in] pt the point to insert
521  */
522  inline void
523  insert (iterator position, std::size_t n, const PointT& pt)
524  {
525  points.insert (position, n, pt);
526  width = static_cast<std::uint32_t> (points.size ());
527  height = 1;
528  }
529 
530  /** \brief Insert a new range of points in the cloud, at a certain position.
531  * \note This breaks the organized structure of the cloud by setting the height to 1!
532  * \param[in] position where to insert the data
533  * \param[in] first where to start inserting the points from
534  * \param[in] last where to stop inserting the points from
535  */
536  template <class InputIterator> inline void
537  insert (iterator position, InputIterator first, InputIterator last)
538  {
539  points.insert (position, first, last);
540  width = static_cast<std::uint32_t> (points.size ());
541  height = 1;
542  }
543 
544  /** \brief Emplace a new point in the cloud, given an iterator.
545  * \note This breaks the organized structure of the cloud by setting the height to 1!
546  * \param[in] position iterator before which the point will be emplaced
547  * \param[in] args the parameters to forward to the point to construct
548  * \return returns the new position iterator
549  */
550  template <class... Args> inline iterator
551  emplace (iterator position, Args&& ...args)
552  {
553  iterator it = points.emplace (position, std::forward<Args> (args)...);
554  width = static_cast<std::uint32_t> (points.size ());
555  height = 1;
556  return (it);
557  }
558 
559  /** \brief Erase a point in the cloud.
560  * \note This breaks the organized structure of the cloud by setting the height to 1!
561  * \param[in] position what data point to erase
562  * \return returns the new position iterator
563  */
564  inline iterator
565  erase (iterator position)
566  {
567  iterator it = points.erase (position);
568  width = static_cast<std::uint32_t> (points.size ());
569  height = 1;
570  return (it);
571  }
572 
573  /** \brief Erase a set of points given by a (first, last) iterator pair
574  * \note This breaks the organized structure of the cloud by setting the height to 1!
575  * \param[in] first where to start erasing points from
576  * \param[in] last where to stop erasing points from
577  * \return returns the new position iterator
578  */
579  inline iterator
580  erase (iterator first, iterator last)
581  {
582  iterator it = points.erase (first, last);
583  width = static_cast<std::uint32_t> (points.size ());
584  height = 1;
585  return (it);
586  }
587 
588  /** \brief Swap a point cloud with another cloud.
589  * \param[in,out] rhs point cloud to swap this with
590  */
591  inline void
593  {
594  std::swap (header, rhs.header);
595  this->points.swap (rhs.points);
596  std::swap (width, rhs.width);
597  std::swap (height, rhs.height);
598  std::swap (is_dense, rhs.is_dense);
599  std::swap (sensor_origin_, rhs.sensor_origin_);
600  std::swap (sensor_orientation_, rhs.sensor_orientation_);
601  }
602 
603  /** \brief Removes all points in a cloud and sets the width and height to 0. */
604  inline void
605  clear ()
606  {
607  points.clear ();
608  width = 0;
609  height = 0;
610  }
611 
612  /** \brief Copy the cloud to the heap and return a smart pointer
613  * Note that deep copy is performed, so avoid using this function on non-empty clouds.
614  * The changes of the returned cloud are not mirrored back to this one.
615  * \return shared pointer to the copy of the cloud
616  */
617  inline Ptr
618  makeShared () const { return Ptr (new PointCloud<PointT> (*this)); }
619 
620  protected:
621  /** \brief This is motivated by ROS integration. Users should not need to access mapping_.
622  * \todo Once mapping_ is removed, erase the explicitly defined copy constructor in PointCloud.
623  */
624  PCL_DEPRECATED(1, 12, "rewrite your code to avoid using this protected field") shared_ptr<MsgFieldMap> mapping_;
625 
626  friend shared_ptr<MsgFieldMap>& detail::getMapping<PointT>(pcl::PointCloud<PointT> &p);
627 
628  public:
630  };
631 
632  namespace detail
633  {
634  template <typename PointT> shared_ptr<pcl::MsgFieldMap>&
636  {
637  return (p.mapping_);
638  }
639  } // namespace detail
640 
641  template <typename PointT> std::ostream&
642  operator << (std::ostream& s, const pcl::PointCloud<PointT> &p)
643  {
644  s << "header: " << p.header << std::endl;
645  s << "points[]: " << p.points.size () << std::endl;
646  s << "width: " << p.width << std::endl;
647  s << "height: " << p.height << std::endl;
648  s << "is_dense: " << p.is_dense << std::endl;
649  s << "sensor origin (xyz): [" <<
650  p.sensor_origin_.x () << ", " <<
651  p.sensor_origin_.y () << ", " <<
652  p.sensor_origin_.z () << "] / orientation (xyzw): [" <<
653  p.sensor_orientation_.x () << ", " <<
654  p.sensor_orientation_.y () << ", " <<
655  p.sensor_orientation_.z () << ", " <<
656  p.sensor_orientation_.w () << "]" <<
657  std::endl;
658  return (s);
659  }
660 }
661 
662 #define PCL_INSTANTIATE_PointCloud(T) template class PCL_EXPORTS pcl::PointCloud<T>;
pcl::PCLHeader::stamp
std::uint64_t stamp
A timestamp associated with the time when the data was acquired.
Definition: PCLHeader.h:18
pcl::PointCloud::swap
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
Definition: point_cloud.h:592
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
Definition: convolution.h:46
pcl::PointCloud::makeShared
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
Definition: point_cloud.h:618
pcl::NdCopyEigenPointFunctor
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:76
pcl::PointCloud::end
iterator end()
Definition: point_cloud.h:443
pcl::traits::offset
Definition: type_traits.h:169
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::PointCloud::erase
iterator erase(iterator first, iterator last)
Erase a set of points given by a (first, last) iterator pair.
Definition: point_cloud.h:580
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::traits::datatype
Definition: type_traits.h:181
pcl::PointCloud::end
const_iterator end() const
Definition: point_cloud.h:445
pcl::NdCopyEigenPointFunctor::Pod
typename traits::POD< PointOutT >::type Pod
Definition: point_cloud.h:77
pcl::NdCopyEigenPointFunctor::operator()
void operator()()
Operator.
Definition: point_cloud.h:90
pcl::PointCloud::at
const PointT & at(std::size_t n) const
Definition: point_cloud.h:468
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:450
pcl::PointCloud::resize
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:455
pcl::NdCopyPointEigenFunctor::Pod
typename traits::POD< PointInT >::type Pod
Definition: point_cloud.h:110
pcl::PointCloud::erase
iterator erase(iterator position)
Erase a point in the cloud.
Definition: point_cloud.h:565
pcl::PointCloud::emplace
iterator emplace(iterator position, Args &&...args)
Emplace a new point in the cloud, given an iterator.
Definition: point_cloud.h:551
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::PointCloud::at
PointT & at(int column, int row)
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:297
pcl::PointCloud< pcl::RGB >::VectorType
std::vector< pcl::RGB, Eigen::aligned_allocator< pcl::RGB > > VectorType
Definition: point_cloud.h:426
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::NdCopyPointEigenFunctor::operator()
void operator()()
Operator.
Definition: point_cloud.h:121
pcl::PointCloud::at
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:283
pcl::PointCloud::front
PointT & front()
Definition: point_cloud.h:471
pcl::detail::FieldMapping
Definition: point_cloud.h:62
pcl::PointCloud::isOrganized
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:331
pcl::PointCloud::getMatrixXfMap
const Eigen::Map< const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap() const
Definition: point_cloud.h:401
pcl::PointCloud< pcl::RGB >::CloudVectorType
std::vector< PointCloud< pcl::RGB >, Eigen::aligned_allocator< PointCloud< pcl::RGB > > > CloudVectorType
Definition: point_cloud.h:427
pcl::operator<<
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
Definition: bivariate_polynomial.hpp:240
pcl::detail::FieldMapping::size
std::size_t size
Definition: point_cloud.h:65
PCL_DEPRECATED
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
Definition: pcl_macros.h:150
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::PointCloud< pcl::RGB >::difference_type
typename VectorType::difference_type difference_type
Definition: point_cloud.h:436
pcl::PointCloud::getMatrixXfMap
Eigen::Map< Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap(int dim, int stride, int offset)
Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
Definition: point_cloud.h:351
pcl::PointCloud::mapping_
shared_ptr< MsgFieldMap > mapping_
This is motivated by ROS integration.
Definition: point_cloud.h:624
pcl::PointCloud< pcl::RGB >::size_type
typename VectorType::size_type size_type
Definition: point_cloud.h:437
pcl::PointCloud::back
const PointT & back() const
Definition: point_cloud.h:472
pcl::PointCloud::getMatrixXfMap
const Eigen::Map< const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap(int dim, int stride, int offset) const
Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
Definition: point_cloud.h:374
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
pcl::RGB
A structure representing RGB color information.
Definition: point_types.hpp:346
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition: point_cloud.h:449
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
pcl::detail::getMapping
shared_ptr< pcl::MsgFieldMap > & getMapping(pcl::PointCloud< PointT > &p)
Definition: point_cloud.h:635
pcl::PointCloud::PointCloud
PointCloud(const PointCloud< PointT > &pc)=default
Copy constructor.
pcl::NdCopyEigenPointFunctor::NdCopyEigenPointFunctor
NdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointOutT &p2)
Constructor.
Definition: point_cloud.h:83
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:418
pcl::PointCloud::PointCloud
PointCloud(const PointCloud< PointT > &pc, const std::vector< int > &indices)
Copy constructor from point cloud subset.
Definition: point_cloud.h:206
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
pcl::PointCloud::begin
const_iterator begin() const
Definition: point_cloud.h:444
pcl::NdCopyPointEigenFunctor
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:109
pcl::UnorganizedPointCloudException
An exception that is thrown when an organized point cloud is needed but not provided.
Definition: exceptions.h:209
pcl::PointCloud::concatenate
static bool concatenate(pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2)
Definition: point_cloud.h:252
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:448
pcl::PointCloud::insert
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
Definition: point_cloud.h:508
pcl::PointCloud< pcl::RGB >::Ptr
shared_ptr< PointCloud< pcl::RGB > > Ptr
Definition: point_cloud.h:428
pcl::concatenate
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud<PointT>
Definition: io.h:282
pcl::PointCloud::emplace_back
reference emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
Definition: point_cloud.h:493
pcl::PointCloud::insert
void insert(iterator position, std::size_t n, const PointT &pt)
Insert a new point in the cloud N times, given an iterator.
Definition: point_cloud.h:523
pcl::PointCloud::front
const PointT & front() const
Definition: point_cloud.h:470
pcl::PointCloud::insert
void insert(iterator position, InputIterator first, InputIterator last)
Insert a new range of points in the cloud, at a certain position.
Definition: point_cloud.h:537
pcl::PointCloud< pcl::RGB >::const_iterator
typename VectorType::const_iterator const_iterator
Definition: point_cloud.h:441
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:605
pcl::PointCloud::back
PointT & back()
Definition: point_cloud.h:473
pcl::uint8_t
std::uint8_t uint8_t
Definition: types.h:54
pcl::PointCloud< pcl::RGB >::ConstPtr
shared_ptr< const PointCloud< pcl::RGB > > ConstPtr
Definition: point_cloud.h:429
pcl::PointCloud::begin
iterator begin()
Definition: point_cloud.h:442
pcl::PointCloud::PointCloud
PointCloud(std::uint32_t width_, std::uint32_t height_, const PointT &value_=PointT())
Allocate constructor from point cloud subset.
Definition: point_cloud.h:222
pcl::PointCloud::concatenate
static bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Definition: point_cloud.h:269
pcl::detail::FieldMapping::serialized_offset
std::size_t serialized_offset
Definition: point_cloud.h:63
pcl::PointCloud::getMatrixXfMap
Eigen::Map< Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap()
Definition: point_cloud.h:389
pcl::MsgFieldMap
std::vector< detail::FieldMapping > MsgFieldMap
Definition: point_cloud.h:71
pcl::PointCloud::PointCloud
PointCloud()=default
Default constructor.
pcl::PointCloud< pcl::RGB >::iterator
typename VectorType::iterator iterator
Definition: point_cloud.h:440
pcl::PointCloud::push_back
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
pcl::NdCopyPointEigenFunctor::NdCopyPointEigenFunctor
NdCopyPointEigenFunctor(const PointInT &p1, Eigen::VectorXf &p2)
Constructor.
Definition: point_cloud.h:116
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::PCLHeader
Definition: PCLHeader.h:11
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:331
pcl::detail::FieldMapping::struct_offset
std::size_t struct_offset
Definition: point_cloud.h:64
pcl::PointCloud::at
PointT & at(std::size_t n)
Definition: point_cloud.h:469