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>
172 class PCL_EXPORTS PointCloud
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 ())
247 operator += (
const PointCloud& rhs)
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 ());
271 inline const PointCloud
272 operator + (
const PointCloud& rhs)
274 return (PointCloud (*
this) += rhs);
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;
434 inline iterator
begin () {
return (points.begin ()); }
435 inline iterator
end () {
return (points.end ()); }
436 inline const_iterator
begin ()
const {
return (points.begin ()); }
437 inline const_iterator
end ()
const {
return (points.end ()); }
440 inline size_t size ()
const {
return (points.size ()); }
441 inline void reserve (
size_t n) { points.reserve (n); }
442 inline bool empty ()
const {
return points.empty (); }
450 if (width * height != n)
452 width =
static_cast<uint32_t
> (n);
458 inline const PointT& operator[] (
size_t n)
const {
return (points[n]); }
459 inline PointT& operator[] (
size_t n) {
return (points[n]); }
460 inline const PointT&
at (
size_t n)
const {
return (points.at (n)); }
461 inline PointT&
at (
size_t n) {
return (points.at (n)); }
464 inline const PointT&
back ()
const {
return (points.back ()); }
474 points.push_back (pt);
475 width =
static_cast<uint32_t
> (points.size ());
488 iterator it = points.insert (position, pt);
489 width =
static_cast<uint32_t
> (points.size ());
503 points.insert (position, n, pt);
504 width =
static_cast<uint32_t
> (points.size ());
514 template <
class InputIterator>
inline void 515 insert (iterator position, InputIterator first, InputIterator last)
517 points.insert (position, first, last);
518 width =
static_cast<uint32_t
> (points.size ());
530 iterator it = points.erase (position);
531 width =
static_cast<uint32_t
> (points.size ());
543 erase (iterator first, iterator last)
545 iterator it = points.erase (first, last);
546 width =
static_cast<uint32_t
> (points.size ());
557 this->points.swap (rhs.
points);
558 std::swap (width, rhs.
width);
559 std::swap (height, rhs.
height);
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_ 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.
An exception that is thrown when a PointCloud is not dense but is attemped to be used as dense...
NdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointOutT &p2)
Constructor.
boost::shared_ptr< pcl::MsgFieldMap > & getMapping(pcl::PointCloud< PointT > &p)
PointCloud(const PointCloud< PointT > &pc)
Copy constructor (needed by compilers such as Intel C++)
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.
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...
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.
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).
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.
void resize(size_t n)
Resize the cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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