39 #ifndef PCL_POINT_CLOUD_H_ 40 #define PCL_POINT_CLOUD_H_ 43 #pragma GCC system_header 46 #include <Eigen/StdVector> 47 #include <Eigen/Geometry> 48 #include <pcl/PCLHeader.h> 49 #include <pcl/exceptions.h> 50 #include <pcl/point_traits.h> 66 typedef std::vector<detail::FieldMapping>
MsgFieldMap;
69 template <
typename Po
intOutT>
80 p2_ (reinterpret_cast<
Pod&>(p2)),
84 template<
typename Key>
inline void 90 *reinterpret_cast<T*>(data_ptr) = static_cast<T> (p1_[f_idx_++]);
94 const Eigen::VectorXf &p1_;
98 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
102 template <
typename Po
intInT>
112 : p1_ (reinterpret_cast<const
Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
115 template<
typename Key>
inline void 121 p2_[f_idx_++] = static_cast<float> (*reinterpret_cast<const T*>(data_ptr));
126 Eigen::VectorXf &p2_;
129 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
134 template <
typename Po
intT> boost::shared_ptr<pcl::MsgFieldMap>&
171 template <
typename Po
intT>
180 header (), points (), width (0), height (0), is_dense (true),
181 sensor_origin_ (
Eigen::Vector4f::Zero ()), sensor_orientation_ (
Eigen::Quaternionf::Identity ()),
189 header (), points (), width (0), height (0), is_dense (true),
190 sensor_origin_ (
Eigen::Vector4f::Zero ()), sensor_orientation_ (
Eigen::Quaternionf::Identity ()),
200 header (), points (), width (0), height (0), is_dense (true),
201 sensor_origin_ (
Eigen::Vector4f::Zero ()), sensor_orientation_ (
Eigen::Quaternionf::Identity ()),
212 const std::vector<int> &indices) :
213 header (pc.header), points (indices.size ()), width (indices.size ()), height (1), is_dense (pc.is_dense),
214 sensor_origin_ (pc.sensor_origin_), sensor_orientation_ (pc.sensor_orientation_),
218 assert (indices.size () <= pc.
size ());
219 for (
size_t i = 0; i < indices.size (); i++)
220 points[i] = pc.
points[indices[i]];
230 , points (width_ * height_, value_)
234 , sensor_origin_ (
Eigen::Vector4f::Zero ())
235 , sensor_orientation_ (
Eigen::Quaternionf::Identity ())
253 size_t nr_points = points.size ();
254 points.resize (nr_points + rhs.
points.size ());
255 for (
size_t i = nr_points; i < points.size (); ++i)
256 points[i] = rhs.
points[i - nr_points];
258 width = static_cast<uint32_t>(points.size ());
283 at (
int column,
int row)
const 285 if (this->height > 1)
286 return (points.at (row * this->width + column));
297 at (
int column,
int row)
299 if (this->height > 1)
300 return (points.at (row * this->width + column));
311 operator () (
size_t column,
size_t row)
const 313 return (points[row * this->width + column]);
322 operator () (
size_t column,
size_t row)
324 return (points[row * this->width + column]);
351 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
354 if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
355 return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, points.size (), dim, Eigen::OuterStride<> (stride)));
357 return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, dim, points.size (), Eigen::OuterStride<> (stride)));
375 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
378 if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
379 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)));
381 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)));
389 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
392 return (getMatrixXfMap (
sizeof (
PointT) /
sizeof (
float),
sizeof (
PointT) /
sizeof (
float), 0));
400 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
403 return (getMatrixXfMap (
sizeof (
PointT) /
sizeof (
float),
sizeof (
PointT) /
sizeof (
float), 0));
410 std::vector<PointT, Eigen::aligned_allocator<PointT> >
points;
426 typedef std::vector<PointT, Eigen::aligned_allocator<PointT> >
VectorType;
427 typedef std::vector<PointCloud<PointT>, Eigen::aligned_allocator<PointCloud<PointT> > >
CloudVectorType;
428 typedef boost::shared_ptr<PointCloud<PointT> >
Ptr;
429 typedef boost::shared_ptr<const PointCloud<PointT> >
ConstPtr;
448 inline size_t size ()
const {
return (points.size ()); }
449 inline void reserve (
size_t n) { points.reserve (n); }
450 inline bool empty ()
const {
return points.empty (); }
458 if (width * height != n)
460 width = static_cast<uint32_t> (n);
466 inline const PointT& operator[] (
size_t n)
const {
return (points[n]); }
467 inline PointT& operator[] (
size_t n) {
return (points[n]); }
468 inline const PointT&
at (
size_t n)
const {
return (points.at (n)); }
469 inline PointT&
at (
size_t n) {
return (points.at (n)); }
472 inline const PointT&
back ()
const {
return (points.back ()); }
482 points.push_back (pt);
483 width = static_cast<uint32_t> (points.size ());
496 iterator it = points.insert (position, pt);
497 width = static_cast<uint32_t> (points.size ());
511 points.insert (position, n, pt);
512 width = static_cast<uint32_t> (points.size ());
522 template <
class InputIterator>
inline void 525 points.insert (position, first, last);
526 width = static_cast<uint32_t> (points.size ());
538 iterator it = points.erase (position);
539 width = static_cast<uint32_t> (points.size ());
553 iterator it = points.erase (first, last);
554 width = static_cast<uint32_t> (points.size ());
565 std::swap (header, rhs.
header);
566 this->points.swap (rhs.
points);
567 std::swap (width, rhs.
width);
568 std::swap (height, rhs.
height);
598 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
603 template <
typename Po
intT> boost::shared_ptr<pcl::MsgFieldMap>&
610 template <
typename Po
intT> std::ostream&
613 s <<
"header: " << p.
header << std::endl;
614 s <<
"points[]: " << p.
points.size () << std::endl;
615 s <<
"width: " << p.
width << std::endl;
616 s <<
"height: " << p.
height << std::endl;
617 s <<
"is_dense: " << p.
is_dense << std::endl;
618 s <<
"sensor origin (xyz): [" <<
631 #define PCL_INSTANTIATE_PointCloud(T) template class PCL_EXPORTS pcl::PointCloud<T>; 633 #endif //#ifndef PCL_POINT_CLOUD_H_ Helper functor structure for copying data between an Eigen type and a PointT.
iterator erase(iterator position)
Erase a point in the cloud.
NdCopyPointEigenFunctor(const PointInT &p1, Eigen::VectorXf &p2)
Constructor.
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
PointT & at(int column, int row)
Obtain the point given by the (column, row) coordinates.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
const_iterator begin() const
Helper functor structure for copying data between an Eigen type and a PointT.
NdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointOutT &p2)
Constructor.
boost::shared_ptr< pcl::MsgFieldMap > & getMapping(pcl::PointCloud< PointT > &p)
This file defines compatibility wrappers for low level I/O functions.
PointCloud(const PointCloud< PointT > &pc)
Copy constructor (needed by compilers such as Intel C++)
const PointT & const_reference
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
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.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
PointCloud(const PointCloud< PointT > &pc, const std::vector< int > &indices)
Copy constructor from point cloud subset.
PointCloud(PointCloud< PointT > &pc)
Copy constructor (needed by compilers such as Intel C++)
const PointT & at(size_t n) const
boost::shared_ptr< MsgFieldMap > mapping_
VectorType::iterator iterator
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
VectorType::difference_type difference_type
uint32_t height
The point cloud height (if organized as an image-structure).
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
boost::shared_ptr< PointCloud< PointT > > Ptr
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
void insert(iterator position, size_t n, const PointT &pt)
Insert a new point in the cloud N times, given an iterator.
uint32_t width
The point cloud width (if organized as an image-structure).
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
traits::POD< PointOutT >::type Pod
const PointT & front() const
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void clear()
Removes all points in a cloud and sets the width and height to 0.
Eigen::Map< Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap()
Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
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.
void operator()()
Operator.
iterator erase(iterator first, iterator last)
Erase a set of points given by a (first, last) iterator pair.
PointCloud()
Default constructor.
PointCloud(uint32_t width_, uint32_t height_, const PointT &value_=PointT())
Allocate constructor from point cloud subset.
virtual ~PointCloud()
Destructor.
PointCloud represents the base class in PCL for storing collections of 3D points.
pcl::PCLHeader header
The point cloud header.
const_iterator end() const
const Eigen::Map< const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap() const
Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
An exception that is thrown when an organized point cloud is needed but not provided.
void insert(iterator position, InputIterator first, InputIterator last)
Insert a new range of points in the cloud, at a certain position.
const PointT & back() const
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
void resize(size_t n)
Resize the cloud.
VectorType::size_type size_type
A point structure representing Euclidean xyz coordinates, and the RGB color.
void operator()()
Operator.
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
std::vector< detail::FieldMapping > MsgFieldMap
traits::POD< PointInT >::type Pod
VectorType::const_iterator const_iterator