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>
212 const std::vector<int> &indices) :
218 assert (indices.size () <= pc.
size ());
219 for (
size_t i = 0; i < indices.size (); i++)
230 ,
points (width_ * height_, value_)
253 size_t nr_points =
points.size ();
255 for (
size_t i = nr_points; i <
points.size (); ++i)
283 at (
int column,
int row)
const
286 return (
points.at (row * this->width + column));
297 at (
int column,
int row)
300 return (
points.at (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<> >
400 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
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;
452 width =
static_cast<uint32_t
> (n);
503 points.insert (position, n, pt);
514 template <
class InputIterator>
inline void
517 points.insert (position, first, last);
589 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
594 template <
typename Po
intT> boost::shared_ptr<pcl::MsgFieldMap>&
601 template <
typename Po
intT> std::ostream&
602 operator << (std::ostream& s, const pcl::PointCloud<PointT> &p)
604 s <<
"points[]: " << p.points.size () << std::endl;
605 s <<
"width: " << p.width << std::endl;
606 s <<
"height: " << p.height << std::endl;
607 s <<
"is_dense: " << p.is_dense << std::endl;
608 s <<
"sensor origin (xyz): [" <<
609 p.sensor_origin_.x () <<
", " <<
610 p.sensor_origin_.y () <<
", " <<
611 p.sensor_origin_.z () <<
"] / orientation (xyzw): [" <<
612 p.sensor_orientation_.x () <<
", " <<
613 p.sensor_orientation_.y () <<
", " <<
614 p.sensor_orientation_.z () <<
", " <<
615 p.sensor_orientation_.w () <<
"]" <<
621 #define PCL_INSTANTIATE_PointCloud(T) template class PCL_EXPORTS pcl::PointCloud<T>;
623 #endif //#ifndef PCL_POINT_CLOUD_H_
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...
boost::shared_ptr< pcl::MsgFieldMap > & getMapping(pcl::PointCloud< PointT > &p)
void resize(size_t n)
Resize the cloud.
PointCloud(uint32_t width_, uint32_t height_, const PointT &value_=PointT())
Allocate constructor from point cloud subset.
PointCloud & operator+=(const PointCloud &rhs)
Add a point cloud to the current cloud.
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
void insert(iterator position, size_t n, const PointT &pt)
Insert a new point in the cloud N times, given an iterator.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
PointCloud(const PointCloud< PointT > &pc)
Copy constructor (needed by compilers such as Intel C++)
const PointT & operator[](size_t n) const
VectorType::const_iterator const_iterator
virtual ~PointCloud()
Destructor.
const PointT & at(size_t n) const
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Eigen::Map< Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap()
Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
std::vector< detail::FieldMapping > MsgFieldMap
pcl::PCLHeader header
The point cloud header.
VectorType::iterator iterator
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
PointCloud()
Default constructor.
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...
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
PointCloud(const PointCloud< PointT > &pc, const std::vector< int > &indices)
Copy constructor from point cloud subset.
uint32_t width
The point cloud width (if organized as an image-structure).
boost::shared_ptr< MsgFieldMap > mapping_
iterator erase(iterator position)
Erase a point in the cloud.
void operator()()
Operator.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
PointCloud(PointCloud< PointT > &pc)
Copy constructor (needed by compilers such as Intel C++)
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.
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.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
An exception that is thrown when a PointCloud is not dense but is attemped to be used as dense...
PointT & at(int column, int row)
Obtain the point given by the (column, row) coordinates.
Helper functor structure for copying data between an Eigen type and a PointT.
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t height
The point cloud height (if organized as an image-structure).
NdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointOutT &p2)
Constructor.
void operator()()
Operator.
NdCopyPointEigenFunctor(const PointInT &p1, Eigen::VectorXf &p2)
Constructor.
const PointT & operator()(size_t column, size_t row) const
Obtain the point given by the (column, row) coordinates.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
traits::POD< PointOutT >::type Pod
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
iterator erase(iterator first, iterator last)
Erase a set of points given by a (first, last) iterator pair.
const_iterator begin() const
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
const PointT & front() const
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
const PointCloud operator+(const PointCloud &rhs)
Add a point cloud to another cloud.
traits::POD< PointInT >::type Pod
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
void clear()
Removes all points in a cloud and sets the width and height to 0.
Helper functor structure for copying data between an Eigen type and a PointT.
A point structure representing Euclidean xyz coordinates, and the RGB color.