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