42 #pragma GCC system_header
45 #include <Eigen/StdVector>
46 #include <Eigen/Geometry>
47 #include <pcl/PCLHeader.h>
48 #include <pcl/exceptions.h>
49 #include <pcl/memory.h>
50 #include <pcl/pcl_macros.h>
51 #include <pcl/type_traits.h>
74 template <
typename Po
intOutT>
85 p2_ (reinterpret_cast<
Pod&>(p2)),
89 template<
typename Key>
inline void
95 *
reinterpret_cast<T*
>(data_ptr) = static_cast<T> (p1_[f_idx_++]);
99 const Eigen::VectorXf &p1_;
107 template <
typename Po
intInT>
117 : p1_ (reinterpret_cast<const
Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
120 template<
typename Key>
inline void
126 p2_[f_idx_++] =
static_cast<float> (*
reinterpret_cast<const T*
>(data_ptr));
131 Eigen::VectorXf &p2_;
139 template <
typename Po
intT>
140 PCL_DEPRECATED(1, 12,
"use createMapping() instead")
178 template <typename
PointT>
186 PointCloud () =
default;
193 #pragma warning(push)
194 #pragma warning(disable: 4068)
196 #pragma clang diagnostic push
197 #pragma clang diagnostic ignored "-Wdeprecated-declarations"
199 #pragma clang diagnostic pop
207 const std::vector<int> &indices) :
208 header (pc.header), points (indices.size ()), width (indices.size ()), height (1), is_dense (pc.is_dense),
209 sensor_origin_ (pc.sensor_origin_), sensor_orientation_ (pc.sensor_orientation_)
212 assert (indices.size () <= pc.
size ());
213 for (std::size_t i = 0; i < indices.size (); i++)
214 points[i] = pc.
points[indices[i]];
223 : points (width_ * height_, value_)
235 operator += (
const PointCloud& rhs)
246 operator + (
const PointCloud& rhs)
248 return (PointCloud (*
this) += rhs);
262 cloud1.
width =
static_cast<std::uint32_t
>(cloud1.
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 () (std::size_t column, std::size_t row)
const
313 return (points[row * this->width + column]);
322 operator () (std::size_t column, std::size_t row)
324 return (points[row * this->width + column]);
350 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
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)));
356 return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, dim, points.size (), Eigen::OuterStride<> (stride)));
373 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
376 if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
377 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 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)));
388 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
391 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;
413 std::uint32_t width = 0;
415 std::uint32_t height = 0;
418 bool is_dense =
true;
421 Eigen::Vector4f sensor_origin_ = Eigen::Vector4f::Zero ();
423 Eigen::Quaternionf sensor_orientation_ = Eigen::Quaternionf::Identity ();
426 using VectorType = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
427 using CloudVectorType = std::vector<PointCloud<PointT>, Eigen::aligned_allocator<PointCloud<PointT> > >;
428 using Ptr = shared_ptr<PointCloud<PointT> >;
429 using ConstPtr = shared_ptr<const PointCloud<PointT> >;
448 inline std::size_t
size ()
const {
return (points.size ()); }
449 inline void reserve (std::size_t n) { points.reserve (n); }
450 inline bool empty ()
const {
return points.empty (); }
458 if (width * height != n)
460 width =
static_cast<std::uint32_t
> (n);
466 inline const PointT& operator[] (std::size_t n)
const {
return (points[n]); }
467 inline PointT& operator[] (std::size_t n) {
return (points[n]); }
468 inline const PointT&
at (std::size_t n)
const {
return (points.at (n)); }
469 inline PointT&
at (std::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<std::uint32_t
> (points.size ());
492 template <
class... Args>
inline reference
495 points.emplace_back (std::forward<Args> (args)...);
496 width =
static_cast<std::uint32_t
> (points.size ());
498 return points.back();
510 iterator it = points.insert (position, pt);
511 width =
static_cast<std::uint32_t
> (points.size ());
525 points.insert (position, n, pt);
526 width =
static_cast<std::uint32_t
> (points.size ());
536 template <
class InputIterator>
inline void
539 points.insert (position, first, last);
540 width =
static_cast<std::uint32_t
> (points.size ());
550 template <
class... Args>
inline iterator
553 iterator it = points.emplace (position, std::forward<Args> (args)...);
554 width =
static_cast<std::uint32_t
> (points.size ());
567 iterator it = points.erase (position);
568 width =
static_cast<std::uint32_t
> (points.size ());
582 iterator it = points.erase (first, last);
583 width =
static_cast<std::uint32_t
> (points.size ());
594 std::swap (header, rhs.
header);
595 this->points.swap (rhs.
points);
596 std::swap (width, rhs.
width);
597 std::swap (height, rhs.
height);
624 PCL_DEPRECATED(1, 12,
"rewrite your code to avoid using this protected field") shared_ptr<
MsgFieldMap> mapping_;
634 template <
typename Po
intT> shared_ptr<pcl::MsgFieldMap>&
641 template <
typename Po
intT> std::ostream&
642 operator << (std::ostream& s, const pcl::PointCloud<PointT> &p)
644 s <<
"header: " << p.header << std::endl;
645 s <<
"points[]: " << p.points.size () << std::endl;
646 s <<
"width: " << p.width << std::endl;
647 s <<
"height: " << p.height << std::endl;
648 s <<
"is_dense: " << p.is_dense << std::endl;
649 s <<
"sensor origin (xyz): [" <<
650 p.sensor_origin_.x () <<
", " <<
651 p.sensor_origin_.y () <<
", " <<
652 p.sensor_origin_.z () <<
"] / orientation (xyzw): [" <<
653 p.sensor_orientation_.x () <<
", " <<
654 p.sensor_orientation_.y () <<
", " <<
655 p.sensor_orientation_.z () <<
", " <<
656 p.sensor_orientation_.w () <<
"]" <<
662 #define PCL_INSTANTIATE_PointCloud(T) template class PCL_EXPORTS pcl::PointCloud<T>;
PointCloud(std::uint32_t width_, std::uint32_t height_, const PointT &value_=PointT())
Allocate constructor from point cloud subset.
Helper functor structure for copying data between an Eigen type and a PointT.
static bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
std::size_t struct_offset
NdCopyPointEigenFunctor(const PointInT &p1, Eigen::VectorXf &p2)
Constructor.
std::vector< detail::FieldMapping > MsgFieldMap
const PointT & front() const
typename VectorType::const_iterator const_iterator
Helper functor structure for copying data between an Eigen type and a PointT.
const Eigen::Map< const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap() const
NdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointOutT &p2)
Constructor.
pcl::pcl::PointXYZI value_type
void insert(iterator position, std::size_t n, const PointT &pt)
Insert a new point in the cloud N times, given an iterator.
PointCloud(const PointCloud< PointT > &pc, const std::vector< int > &indices)
Copy constructor from point cloud subset.
iterator erase(iterator position)
Erase a point in the cloud.
reference emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
typename VectorType::size_type size_type
std::uint32_t width
The point cloud width (if organized as an image-structure).
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
iterator erase(iterator first, iterator last)
Erase a set of points given by a (first, last) iterator pair.
const PointT & at(std::size_t n) const
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
std::size_t serialized_offset
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
shared_ptr< MsgFieldMap > mapping_
This is motivated by ROS integration.
const_iterator end() const
shared_ptr< PointCloud< pcl::pcl::PointXYZI > > Ptr
pcl::pcl::PointXYZI PointType
PointT & at(std::size_t n)
void insert(iterator position, InputIterator first, InputIterator last)
Insert a new range of points in the cloud, at a certain position.
PointT & at(int column, int row)
Obtain the point given by the (column, row) coordinates.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
const pcl::pcl::PointXYZI & const_reference
const_iterator begin() const
std::vector< pcl::pcl::PointXYZI, Eigen::aligned_allocator< pcl::pcl::PointXYZI > > VectorType
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...
shared_ptr< pcl::MsgFieldMap > & getMapping(pcl::PointCloud< PointT > &p)
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
void operator()()
Operator.
static bool concatenate(pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2)
const PointT & back() const
PointCloud represents the base class in PCL for storing collections of 3D points. ...
iterator emplace(iterator position, Args &&...args)
Emplace a new point in the cloud, given an iterator.
shared_ptr< const PointCloud< pcl::pcl::PointXYZI > > ConstPtr
typename VectorType::difference_type difference_type
typename traits::POD< PointOutT >::type Pod
An exception that is thrown when an organized point cloud is needed but not provided.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
std::uint32_t height
The point cloud height (if organized as an image-structure).
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
void reserve(std::size_t n)
Eigen::Map< Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap()
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...
void operator()()
Operator.
void resize(std::size_t n)
Resize the cloud.
std::vector< PointCloud< pcl::pcl::PointXYZI >, Eigen::aligned_allocator< PointCloud< pcl::pcl::PointXYZI > > > CloudVectorType
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
typename VectorType::iterator iterator
void clear()
Removes all points in a cloud and sets the width and height to 0.
pcl::pcl::PointXYZI & reference
typename traits::POD< PointInT >::type Pod
pcl::PCLHeader header
The point cloud header.