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