Point Cloud Library (PCL)  1.11.0-dev
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 #include <pcl/types.h>
53 
54 #include <algorithm>
55 #include <utility>
56 #include <vector>
57 
58 namespace pcl
59 {
60  namespace detail
61  {
62  struct FieldMapping
63  {
64  std::size_t serialized_offset;
65  std::size_t struct_offset;
66  std::size_t size;
67  };
68  } // namespace detail
69 
70  // Forward declarations
71  template <typename PointT> class PointCloud;
72  using MsgFieldMap = std::vector<detail::FieldMapping>;
73 
74  /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */
75  template <typename PointOutT>
77  {
78  using Pod = typename traits::POD<PointOutT>::type;
79 
80  /** \brief Constructor
81  * \param[in] p1 the input Eigen type
82  * \param[out] p2 the output Point type
83  */
84  NdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointOutT &p2)
85  : p1_ (p1),
86  p2_ (reinterpret_cast<Pod&>(p2)),
87  f_idx_ (0) { }
88 
89  /** \brief Operator. Data copy happens here. */
90  template<typename Key> inline void
92  {
93  //boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++];
94  using T = typename pcl::traits::datatype<PointOutT, Key>::type;
95  std::uint8_t* data_ptr = reinterpret_cast<std::uint8_t*>(&p2_) + pcl::traits::offset<PointOutT, Key>::value;
96  *reinterpret_cast<T*>(data_ptr) = static_cast<T> (p1_[f_idx_++]);
97  }
98 
99  private:
100  const Eigen::VectorXf &p1_;
101  Pod &p2_;
102  int f_idx_;
103  public:
105  };
106 
107  /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */
108  template <typename PointInT>
110  {
111  using Pod = typename traits::POD<PointInT>::type;
112 
113  /** \brief Constructor
114  * \param[in] p1 the input Point type
115  * \param[out] p2 the output Eigen type
116  */
117  NdCopyPointEigenFunctor (const PointInT &p1, Eigen::VectorXf &p2)
118  : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
119 
120  /** \brief Operator. Data copy happens here. */
121  template<typename Key> inline void
123  {
124  //p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_);
125  using T = typename pcl::traits::datatype<PointInT, Key>::type;
126  const std::uint8_t* data_ptr = reinterpret_cast<const std::uint8_t*>(&p1_) + pcl::traits::offset<PointInT, Key>::value;
127  p2_[f_idx_++] = static_cast<float> (*reinterpret_cast<const T*>(data_ptr));
128  }
129 
130  private:
131  const Pod &p1_;
132  Eigen::VectorXf &p2_;
133  int f_idx_;
134  public:
136  };
137 
138  namespace detail
139  {
140  template <typename PointT>
141  PCL_DEPRECATED(1, 12, "use createMapping() instead")
142  shared_ptr<pcl::MsgFieldMap>&
144  } // namespace detail
145 
146  /** \brief PointCloud represents the base class in PCL for storing collections of 3D points.
147  *
148  * The class is templated, which means you need to specify the type of data
149  * that it should contain. For example, to create a point cloud that holds 4
150  * random XYZ data points, use:
151  *
152  * \code
153  * pcl::PointCloud<pcl::PointXYZ> cloud;
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  * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
158  * \endcode
159  *
160  * The PointCloud class contains the following elements:
161  * - \b width - specifies the width of the point cloud dataset in the number of points. WIDTH has two meanings:
162  * - it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets;
163  * - it can specify the width (total number of points in a row) of an organized point cloud dataset.
164  * \a Mandatory.
165  * - \b height - specifies the height of the point cloud dataset in the number of points. HEIGHT has two meanings:
166  * - it can specify the height (total number of rows) of an organized point cloud dataset;
167  * - it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not).
168  * \a Mandatory.
169  * - \b points - the data array where all points of type <b>PointT</b> are stored. \a Mandatory.
170  *
171  * - \b is_dense - specifies if all the data in <b>points</b> is finite (true), or whether it might contain Inf/NaN values
172  * (false). \a Mandatory.
173  *
174  * - \b sensor_origin_ - specifies the sensor acquisition pose (origin/translation). \a Optional.
175  * - \b sensor_orientation_ - specifies the sensor acquisition pose (rotation). \a Optional.
176  *
177  * \author Patrick Mihelich, Radu B. Rusu
178  */
179  template <typename PointT>
180  class PCL_EXPORTS PointCloud
181  {
182  public:
183  /** \brief Default constructor. Sets \ref is_dense to true, \ref width
184  * and \ref height to 0, and the \ref sensor_origin_ and \ref
185  * sensor_orientation_ to identity.
186  */
187  PointCloud () = default;
188 
189  /** \brief Copy constructor.
190  * \param[in] pc the cloud to copy into this
191  * \todo Erase once mapping_ is removed.
192  */
193  // Ignore unknown pragma warning on MSVC (4996)
194  #pragma warning(push)
195  #pragma warning(disable: 4068)
196  // Ignore deprecated warning on clang compilers
197  #pragma clang diagnostic push
198  #pragma clang diagnostic ignored "-Wdeprecated-declarations"
199  PointCloud (const PointCloud<PointT> &pc) = default;
200  #pragma clang diagnostic pop
201  #pragma warning(pop)
202 
203  /** \brief Copy constructor from point cloud subset
204  * \param[in] pc the cloud to copy into this
205  * \param[in] indices the subset to copy
206  */
208  const Indices &indices) :
209  header (pc.header), points (indices.size ()), width (indices.size ()), height (1), is_dense (pc.is_dense),
210  sensor_origin_ (pc.sensor_origin_), sensor_orientation_ (pc.sensor_orientation_)
211  {
212  // Copy the obvious
213  assert (indices.size () <= pc.size ());
214  for (std::size_t i = 0; i < indices.size (); i++)
215  points[i] = pc[indices[i]];
216  }
217 
218  /** \brief Allocate constructor from point cloud subset
219  * \param[in] width_ the cloud width
220  * \param[in] height_ the cloud height
221  * \param[in] value_ default value
222  */
223  PointCloud (std::uint32_t width_, std::uint32_t height_, const PointT& value_ = PointT ())
224  : points (width_ * height_, value_)
225  , width (width_)
226  , height (height_)
227  {}
228 
229  //TODO: check if copy/move contructors/assignment operators are needed
230 
231  /** \brief Add a point cloud to the current cloud.
232  * \param[in] rhs the cloud to add to the current cloud
233  * \return the new cloud as a concatenation of the current cloud and the new given cloud
234  */
235  inline PointCloud&
236  operator += (const PointCloud& rhs)
237  {
238  concatenate((*this), rhs);
239  return (*this);
240  }
241 
242  /** \brief Add a point cloud to another cloud.
243  * \param[in] rhs the cloud to add to the current cloud
244  * \return the new cloud as a concatenation of the current cloud and the new given cloud
245  */
246  inline PointCloud
247  operator + (const PointCloud& rhs)
248  {
249  return (PointCloud (*this) += rhs);
250  }
251 
252  inline static bool
254  const pcl::PointCloud<PointT> &cloud2)
255  {
256  // Make the resultant point cloud take the newest stamp
257  cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);
258 
259  // libstdc++ (GCC) on calling reserve allocates new memory, copies and deallocates old vector
260  // This causes a drastic performance hit. Prefer not to use reserve with libstdc++ (default on clang)
261  cloud1.points.insert (cloud1.points.end (), cloud2.points.begin (), cloud2.points.end ());
262 
263  cloud1.width = cloud1.size ();
264  cloud1.height = 1;
265  cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
266  return true;
267  }
268 
269  inline static bool
271  const pcl::PointCloud<PointT> &cloud2,
272  pcl::PointCloud<PointT> &cloud_out)
273  {
274  cloud_out = cloud1;
275  return concatenate(cloud_out, cloud2);
276  }
277 
278  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
279  * datasets (those that have height != 1).
280  * \param[in] column the column coordinate
281  * \param[in] row the row coordinate
282  */
283  inline const PointT&
284  at (int column, int row) const
285  {
286  if (this->height > 1)
287  return (points.at (row * this->width + column));
288  else
289  throw UnorganizedPointCloudException ("Can't use 2D indexing with an unorganized point cloud");
290  }
291 
292  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
293  * datasets (those that have height != 1).
294  * \param[in] column the column coordinate
295  * \param[in] row the row coordinate
296  */
297  inline PointT&
298  at (int column, int row)
299  {
300  if (this->height > 1)
301  return (points.at (row * this->width + column));
302  else
303  throw UnorganizedPointCloudException ("Can't use 2D indexing with an unorganized point cloud");
304  }
305 
306  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
307  * datasets (those that have height != 1).
308  * \param[in] column the column coordinate
309  * \param[in] row the row coordinate
310  */
311  inline const PointT&
312  operator () (std::size_t column, std::size_t row) const
313  {
314  return (points[row * this->width + column]);
315  }
316 
317  /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
318  * datasets (those that have height != 1).
319  * \param[in] column the column coordinate
320  * \param[in] row the row coordinate
321  */
322  inline PointT&
323  operator () (std::size_t column, std::size_t row)
324  {
325  return (points[row * this->width + column]);
326  }
327 
328  /** \brief Return whether a dataset is organized (e.g., arranged in a structured grid).
329  * \note The height value must be different than 1 for a dataset to be organized.
330  */
331  inline bool
332  isOrganized () const
333  {
334  return (height > 1);
335  }
336 
337  /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
338  * \note This method is for advanced users only! Use with care!
339  *
340  * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
341  * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure,
342  * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix
343  *
344  * \param[in] dim the number of dimensions to consider for each point
345  * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns)
346  * \param[in] offset the number of dimensions to skip from the beginning of each point
347  * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point)
348  * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment.
349  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
350  */
351  inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
352  getMatrixXfMap (int dim, int stride, int offset)
353  {
354  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
355  return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, size (), dim, Eigen::OuterStride<> (stride)));
356  else
357  return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, dim, size (), Eigen::OuterStride<> (stride)));
358  }
359 
360  /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud.
361  * \note This method is for advanced users only! Use with care!
362  *
363  * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL
364  * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure,
365  * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix
366  *
367  * \param[in] dim the number of dimensions to consider for each point
368  * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns)
369  * \param[in] offset the number of dimensions to skip from the beginning of each point
370  * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point)
371  * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment.
372  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
373  */
374  inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
375  getMatrixXfMap (int dim, int stride, int offset) const
376  {
377  if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
378  return (Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(const_cast<PointT*>(&points[0]))+offset, size (), dim, Eigen::OuterStride<> (stride)));
379  else
380  return (Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(const_cast<PointT*>(&points[0]))+offset, dim, size (), Eigen::OuterStride<> (stride)));
381  }
382 
383  /**
384  * \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
385  * \note This method is for advanced users only! Use with care!
386  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
387  * \overload Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > pcl::PointCloud::getMatrixXfMap ()
388  */
389  inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
391  {
392  return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0));
393  }
394 
395  /**
396  * \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
397  * \note This method is for advanced users only! Use with care!
398  * \attention PointT types are most of the time aligned, so the offsets are not continuous!
399  * \overload const Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > pcl::PointCloud::getMatrixXfMap () const
400  */
401  inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
402  getMatrixXfMap () const
403  {
404  return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0));
405  }
406 
407  /** \brief The point cloud header. It contains information about the acquisition time. */
409 
410  /** \brief The point data. */
411  std::vector<PointT, Eigen::aligned_allocator<PointT> > points;
412 
413  /** \brief The point cloud width (if organized as an image-structure). */
414  std::uint32_t width = 0;
415  /** \brief The point cloud height (if organized as an image-structure). */
416  std::uint32_t height = 0;
417 
418  /** \brief True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields). */
419  bool is_dense = true;
420 
421  /** \brief Sensor acquisition pose (origin/translation). */
422  Eigen::Vector4f sensor_origin_ = Eigen::Vector4f::Zero ();
423  /** \brief Sensor acquisition pose (rotation). */
424  Eigen::Quaternionf sensor_orientation_ = Eigen::Quaternionf::Identity ();
425 
426  using PointType = PointT; // Make the template class available from the outside
427  using VectorType = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
428  using CloudVectorType = std::vector<PointCloud<PointT>, Eigen::aligned_allocator<PointCloud<PointT> > >;
429  using Ptr = shared_ptr<PointCloud<PointT> >;
430  using ConstPtr = shared_ptr<const PointCloud<PointT> >;
431 
432  // std container compatibility typedefs according to
433  // http://en.cppreference.com/w/cpp/concept/Container
435  using reference = PointT&;
436  using const_reference = const PointT&;
437  using difference_type = typename VectorType::difference_type;
438  using size_type = typename VectorType::size_type;
439 
440  // iterators
441  using iterator = typename VectorType::iterator;
442  using const_iterator = typename VectorType::const_iterator;
443  using reverse_iterator = typename VectorType::reverse_iterator;
444  using const_reverse_iterator = typename VectorType::const_reverse_iterator;
445  inline iterator begin () noexcept { return (points.begin ()); }
446  inline iterator end () noexcept { return (points.end ()); }
447  inline const_iterator begin () const noexcept { return (points.begin ()); }
448  inline const_iterator end () const noexcept { return (points.end ()); }
449  inline const_iterator cbegin () const noexcept { return (points.cbegin ()); }
450  inline const_iterator cend () const noexcept { return (points.cend ()); }
451  inline reverse_iterator rbegin () noexcept { return (points.rbegin ()); }
452  inline reverse_iterator rend () noexcept { return (points.rend ()); }
453  inline const_reverse_iterator rbegin () const noexcept { return (points.rbegin ()); }
454  inline const_reverse_iterator rend () const noexcept { return (points.rend ()); }
455  inline const_reverse_iterator crbegin () const noexcept { return (points.crbegin ()); }
456  inline const_reverse_iterator crend () const noexcept { return (points.crend ()); }
457 
458  //capacity
459  inline std::size_t size () const { return points.size (); }
460  index_t max_size() const noexcept { return static_cast<index_t>(points.max_size()); }
461  inline void reserve (std::size_t n) { points.reserve (n); }
462  inline bool empty () const { return points.empty (); }
463  PointT* data() noexcept { return points.data(); }
464  const PointT* data() const noexcept { return points.data(); }
465 
466  /**
467  * \brief Resizes the container to contain `count` elements
468  * \details
469  * * If the current size is greater than `count`, the pointcloud is reduced to its
470  * first `count` elements
471  * * If the current size is less than `count`, additional default-inserted points
472  * are appended
473  * \note This potentially breaks the organized structure of the cloud by setting
474  * the height to 1 IFF `width * height != count`!
475  * \param[in] count new size of the point cloud
476  */
477  inline void
478  resize(std::size_t count)
479  {
480  points.resize(count);
481  if (width * height != count) {
482  width = static_cast<std::uint32_t>(count);
483  height = 1;
484  }
485  }
486 
487  /**
488  * \brief Resizes the container to contain count elements
489  * \details
490  * * If the current size is greater than `count`, the pointcloud is reduced to its
491  * first `count` elements
492  * * If the current size is less than `count`, additional copies of `value` are
493  * appended
494  * \note This potentially breaks the organized structure of the cloud by setting
495  * the height to 1 IFF `width * height != count`!
496  * \param[in] count new size of the point cloud
497  * \param[in] value the value to initialize the new points with
498  */
499  void
500  resize(index_t count, const PointT& value)
501  {
502  points.resize(count, value);
503  if (width * height != count) {
504  width = count;
505  height = 1;
506  }
507  }
508 
509  //element access
510  inline const PointT& operator[] (std::size_t n) const { return (points[n]); }
511  inline PointT& operator[] (std::size_t n) { return (points[n]); }
512  inline const PointT& at (std::size_t n) const { return (points.at (n)); }
513  inline PointT& at (std::size_t n) { return (points.at (n)); }
514  inline const PointT& front () const { return (points.front ()); }
515  inline PointT& front () { return (points.front ()); }
516  inline const PointT& back () const { return (points.back ()); }
517  inline PointT& back () { return (points.back ()); }
518 
519  /**
520  * \brief Replaces the points with `count` copies of `value`
521  * \note This breaks the organized structure of the cloud by setting the height to
522  * 1!
523  */
524  void
525  assign(index_t count, const PointT& value)
526  {
527  points.assign(count, value);
528  width = static_cast<std::uint32_t>(size());
529  height = 1;
530  }
531 
532  /**
533  * \brief Replaces the points with copies of those in the range `[first, last)`
534  * \details The behavior is undefined if either argument is an iterator into
535  * `*this`
536  * \note This breaks the organized structure of the cloud by setting the height to
537  * 1!
538  */
539  template <class InputIt>
540  void
541  assign(InputIt first, InputIt last)
542  {
543  points.assign(std::move(first), std::move(last));
544  width = static_cast<std::uint32_t>(size());
545  height = 1;
546  }
547 
548  /**
549  * \brief Replaces the points with the elements from the initializer list `ilist`
550  * \note This breaks the organized structure of the cloud by setting the height to
551  * 1!
552  */
553  void
554  assign(std::initializer_list<PointT> ilist)
555  {
556  points.assign(std::move(ilist));
557  width = static_cast<std::uint32_t>(size());
558  height = 1;
559  }
560 
561  /** \brief Insert a new point in the cloud, at the end of the container.
562  * \note This breaks the organized structure of the cloud by setting the height to 1!
563  * \param[in] pt the point to insert
564  */
565  inline void
566  push_back (const PointT& pt)
567  {
568  points.push_back (pt);
569  width = size ();
570  height = 1;
571  }
572 
573  /** \brief Emplace a new point in the cloud, at the end of the container.
574  * \note This breaks the organized structure of the cloud by setting the height to 1!
575  * \param[in] args the parameters to forward to the point to construct
576  * \return reference to the emplaced point
577  */
578  template <class... Args> inline reference
579  emplace_back (Args&& ...args)
580  {
581  points.emplace_back (std::forward<Args> (args)...);
582  width = size ();
583  height = 1;
584  return points.back();
585  }
586 
587  /** \brief Insert a new point in the cloud, given an iterator.
588  * \note This breaks the organized structure of the cloud by setting the height to 1!
589  * \param[in] position where to insert the point
590  * \param[in] pt the point to insert
591  * \return returns the new position iterator
592  */
593  inline iterator
594  insert (iterator position, const PointT& pt)
595  {
596  iterator it = points.insert (position, pt);
597  width = size ();
598  height = 1;
599  return (it);
600  }
601 
602  /** \brief Insert a new point in the cloud N times, given an iterator.
603  * \note This breaks the organized structure of the cloud by setting the height to 1!
604  * \param[in] position where to insert the point
605  * \param[in] n the number of times to insert the point
606  * \param[in] pt the point to insert
607  */
608  inline void
609  insert (iterator position, std::size_t n, const PointT& pt)
610  {
611  points.insert (position, n, pt);
612  width = size ();
613  height = 1;
614  }
615 
616  /** \brief Insert a new range of points in the cloud, at a certain position.
617  * \note This breaks the organized structure of the cloud by setting the height to 1!
618  * \param[in] position where to insert the data
619  * \param[in] first where to start inserting the points from
620  * \param[in] last where to stop inserting the points from
621  */
622  template <class InputIterator> inline void
623  insert (iterator position, InputIterator first, InputIterator last)
624  {
625  points.insert (position, first, last);
626  width = size ();
627  height = 1;
628  }
629 
630  /** \brief Emplace a new point in the cloud, given an iterator.
631  * \note This breaks the organized structure of the cloud by setting the height to 1!
632  * \param[in] position iterator before which the point will be emplaced
633  * \param[in] args the parameters to forward to the point to construct
634  * \return returns the new position iterator
635  */
636  template <class... Args> inline iterator
637  emplace (iterator position, Args&& ...args)
638  {
639  iterator it = points.emplace (position, std::forward<Args> (args)...);
640  width = size ();
641  height = 1;
642  return (it);
643  }
644 
645  /** \brief Erase a point in the cloud.
646  * \note This breaks the organized structure of the cloud by setting the height to 1!
647  * \param[in] position what data point to erase
648  * \return returns the new position iterator
649  */
650  inline iterator
651  erase (iterator position)
652  {
653  iterator it = points.erase (position);
654  width = size ();
655  height = 1;
656  return (it);
657  }
658 
659  /** \brief Erase a set of points given by a (first, last) iterator pair
660  * \note This breaks the organized structure of the cloud by setting the height to 1!
661  * \param[in] first where to start erasing points from
662  * \param[in] last where to stop erasing points from
663  * \return returns the new position iterator
664  */
665  inline iterator
666  erase (iterator first, iterator last)
667  {
668  iterator it = points.erase (first, last);
669  width = size ();
670  height = 1;
671  return (it);
672  }
673 
674  /** \brief Swap a point cloud with another cloud.
675  * \param[in,out] rhs point cloud to swap this with
676  */
677  inline void
679  {
680  std::swap (header, rhs.header);
681  this->points.swap (rhs.points);
682  std::swap (width, rhs.width);
683  std::swap (height, rhs.height);
684  std::swap (is_dense, rhs.is_dense);
685  std::swap (sensor_origin_, rhs.sensor_origin_);
686  std::swap (sensor_orientation_, rhs.sensor_orientation_);
687  }
688 
689  /** \brief Removes all points in a cloud and sets the width and height to 0. */
690  inline void
691  clear ()
692  {
693  points.clear ();
694  width = 0;
695  height = 0;
696  }
697 
698  /** \brief Copy the cloud to the heap and return a smart pointer
699  * Note that deep copy is performed, so avoid using this function on non-empty clouds.
700  * The changes of the returned cloud are not mirrored back to this one.
701  * \return shared pointer to the copy of the cloud
702  */
703  inline Ptr
704  makeShared () const { return Ptr (new PointCloud<PointT> (*this)); }
705 
706  protected:
707  /** \brief This is motivated by ROS integration. Users should not need to access mapping_.
708  * \todo Once mapping_ is removed, erase the explicitly defined copy constructor in PointCloud.
709  */
710  PCL_DEPRECATED(1, 12, "rewrite your code to avoid using this protected field") shared_ptr<MsgFieldMap> mapping_;
711 
712  friend shared_ptr<MsgFieldMap>& detail::getMapping<PointT>(pcl::PointCloud<PointT> &p);
713 
714  public:
716  };
717 
718  namespace detail
719  {
720  template <typename PointT> shared_ptr<pcl::MsgFieldMap>&
722  {
723  return (p.mapping_);
724  }
725  } // namespace detail
726 
727  template <typename PointT> std::ostream&
728  operator << (std::ostream& s, const pcl::PointCloud<PointT> &p)
729  {
730  s << "header: " << p.header << std::endl;
731  s << "points[]: " << p.size () << std::endl;
732  s << "width: " << p.width << std::endl;
733  s << "height: " << p.height << std::endl;
734  s << "is_dense: " << p.is_dense << std::endl;
735  s << "sensor origin (xyz): [" <<
736  p.sensor_origin_.x () << ", " <<
737  p.sensor_origin_.y () << ", " <<
738  p.sensor_origin_.z () << "] / orientation (xyzw): [" <<
739  p.sensor_orientation_.x () << ", " <<
740  p.sensor_orientation_.y () << ", " <<
741  p.sensor_orientation_.z () << ", " <<
742  p.sensor_orientation_.w () << "]" <<
743  std::endl;
744  return (s);
745  }
746 }
747 
748 #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:678
pcl::PointCloud< pcl::RGB >::const_reverse_iterator
typename VectorType::const_reverse_iterator const_reverse_iterator
Definition: point_cloud.h:444
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:704
pcl::NdCopyEigenPointFunctor
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:76
pcl::PointCloud::assign
void assign(std::initializer_list< PointT > ilist)
Replaces the points with the elements from the initializer list ilist
Definition: point_cloud.h:554
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:416
pcl::PointCloud::resize
void resize(index_t count, const PointT &value)
Resizes the container to contain count elements.
Definition: point_cloud.h:500
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:666
pcl::PointCloud::cbegin
const_iterator cbegin() const noexcept
Definition: point_cloud.h:449
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::PointCloud::rbegin
reverse_iterator rbegin() noexcept
Definition: point_cloud.h:451
types.h
Defines basic non-point types used by PCL.
pcl::NdCopyEigenPointFunctor::Pod
typename traits::POD< PointOutT >::type Pod
Definition: point_cloud.h:78
pcl::NdCopyEigenPointFunctor::operator()
void operator()()
Operator.
Definition: point_cloud.h:91
pcl::PointCloud::begin
iterator begin() noexcept
Definition: point_cloud.h:445
pcl::PointCloud::at
const PointT & at(std::size_t n) const
Definition: point_cloud.h:512
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:462
pcl::PointCloud::cend
const_iterator cend() const noexcept
Definition: point_cloud.h:450
pcl::NdCopyPointEigenFunctor::Pod
typename traits::POD< PointInT >::type Pod
Definition: point_cloud.h:111
pcl::PointCloud::erase
iterator erase(iterator position)
Erase a point in the cloud.
Definition: point_cloud.h:651
pcl::PointCloud::emplace
iterator emplace(iterator position, Args &&...args)
Emplace a new point in the cloud, given an iterator.
Definition: point_cloud.h:637
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PointCloud::at
PointT & at(int column, int row)
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:298
pcl::index_t
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:120
pcl::PointCloud< pcl::RGB >::VectorType
std::vector< pcl::RGB, Eigen::aligned_allocator< pcl::RGB > > VectorType
Definition: point_cloud.h:427
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::NdCopyPointEigenFunctor::operator()
void operator()()
Operator.
Definition: point_cloud.h:122
pcl::PointCloud::at
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:284
pcl::PointCloud::front
PointT & front()
Definition: point_cloud.h:515
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:332
pcl::PointCloud< pcl::RGB >::CloudVectorType
std::vector< PointCloud< pcl::RGB >, Eigen::aligned_allocator< PointCloud< pcl::RGB > > > CloudVectorType
Definition: point_cloud.h:428
pcl::PointCloud::data
const PointT * data() const noexcept
Definition: point_cloud.h:464
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:66
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:147
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::PointCloud< pcl::RGB >::difference_type
typename VectorType::difference_type difference_type
Definition: point_cloud.h:437
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:352
pcl::PointCloud::mapping_
shared_ptr< MsgFieldMap > mapping_
This is motivated by ROS integration.
Definition: point_cloud.h:710
pcl::PointCloud::rend
const_reverse_iterator rend() const noexcept
Definition: point_cloud.h:454
pcl::PointCloud::getMatrixXfMap
const Eigen::Map< const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap() const
Definition: point_cloud.h:402
pcl::PointCloud< pcl::RGB >::size_type
typename VectorType::size_type size_type
Definition: point_cloud.h:438
pcl::PointCloud::end
const_iterator end() const noexcept
Definition: point_cloud.h:448
pcl::PointCloud::back
const PointT & back() const
Definition: point_cloud.h:516
pcl::PointCloud::getMatrixXfMap
Eigen::Map< Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap()
Definition: point_cloud.h:390
pcl::PointCloud::rend
reverse_iterator rend() noexcept
Definition: point_cloud.h:452
pcl::PointCloud::end
iterator end() noexcept
Definition: point_cloud.h:446
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:375
pcl::PointCloud::crend
const_reverse_iterator crend() const noexcept
Definition: point_cloud.h:456
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:345
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition: point_cloud.h:461
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:422
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:424
pcl::detail::getMapping
shared_ptr< pcl::MsgFieldMap > & getMapping(pcl::PointCloud< PointT > &p)
Definition: point_cloud.h:721
pcl::NdCopyEigenPointFunctor::NdCopyEigenPointFunctor
NdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointOutT &p2)
Constructor.
Definition: point_cloud.h:84
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:419
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:478
pcl::PointCloud< pcl::RGB >::reverse_iterator
typename VectorType::reverse_iterator reverse_iterator
Definition: point_cloud.h:443
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:408
pcl::NdCopyPointEigenFunctor
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:109
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:141
pcl::UnorganizedPointCloudException
An exception that is thrown when an organized point cloud is needed but not provided.
Definition: exceptions.h:208
pcl::PointCloud::concatenate
static bool concatenate(pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2)
Definition: point_cloud.h:253
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:459
pcl::PointCloud::begin
const_iterator begin() const noexcept
Definition: point_cloud.h:447
pcl::PointCloud::insert
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
Definition: point_cloud.h:594
pcl::PointCloud< pcl::RGB >::Ptr
shared_ptr< PointCloud< pcl::RGB > > Ptr
Definition: point_cloud.h:429
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:579
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:609
pcl::PointCloud::assign
void assign(index_t count, const PointT &value)
Replaces the points with count copies of value
Definition: point_cloud.h:525
pcl::PointCloud::rbegin
const_reverse_iterator rbegin() const noexcept
Definition: point_cloud.h:453
pcl::PointCloud::front
const PointT & front() const
Definition: point_cloud.h:514
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:623
pcl::PointCloud< pcl::RGB >::const_iterator
typename VectorType::const_iterator const_iterator
Definition: point_cloud.h:442
pcl::PointCloud::assign
void assign(InputIt first, InputIt last)
Replaces the points with copies of those in the range [first, last)
Definition: point_cloud.h:541
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:691
pcl::PointCloud::back
PointT & back()
Definition: point_cloud.h:517
pcl::uint8_t
std::uint8_t uint8_t
Definition: types.h:54
pcl::PointCloud::PointCloud
PointCloud(const PointCloud< PointT > &pc, const Indices &indices)
Copy constructor from point cloud subset.
Definition: point_cloud.h:207
pcl::PointCloud::data
PointT * data() noexcept
Definition: point_cloud.h:463
pcl::PointCloud< pcl::RGB >::ConstPtr
shared_ptr< const PointCloud< pcl::RGB > > ConstPtr
Definition: point_cloud.h:430
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:223
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:270
pcl::PointCloud::max_size
index_t max_size() const noexcept
Definition: point_cloud.h:460
pcl::detail::FieldMapping::serialized_offset
std::size_t serialized_offset
Definition: point_cloud.h:64
pcl::MsgFieldMap
std::vector< detail::FieldMapping > MsgFieldMap
Definition: point_cloud.h:72
pcl::PointCloud< pcl::RGB >::iterator
typename VectorType::iterator iterator
Definition: point_cloud.h:441
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:566
pcl::NdCopyPointEigenFunctor::NdCopyPointEigenFunctor
NdCopyPointEigenFunctor(const PointInT &p1, Eigen::VectorXf &p2)
Constructor.
Definition: point_cloud.h:117
pcl::PointCloud::crbegin
const_reverse_iterator crbegin() const noexcept
Definition: point_cloud.h:455
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::PCLHeader
Definition: PCLHeader.h:10
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:328
pcl::detail::FieldMapping::struct_offset
std::size_t struct_offset
Definition: point_cloud.h:65
pcl::PointCloud::at
PointT & at(std::size_t n)
Definition: point_cloud.h:513