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