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