42 #pragma GCC system_header
45 #include <Eigen/StdVector>
46 #include <Eigen/Geometry>
47 #include <pcl/PCLHeader.h>
48 #include <pcl/exceptions.h>
51 #include <pcl/type_traits.h>
53 #include <pcl/console/print.h>
76 template <
typename Po
intOutT>
79 using Pod =
typename traits::POD<PointOutT>::type;
87 p2_ (reinterpret_cast<
Pod&>(p2)),
91 template<
typename Key>
inline void
95 using T =
typename pcl::traits::datatype<PointOutT, Key>::type;
96 std::uint8_t* data_ptr =
reinterpret_cast<std::uint8_t*
>(&p2_) + pcl::traits::offset<PointOutT, Key>::value;
97 *
reinterpret_cast<T*
>(data_ptr) =
static_cast<T
> (p1_[f_idx_++]);
101 const Eigen::VectorXf &p1_;
109 template <
typename Po
intInT>
112 using Pod =
typename traits::POD<PointInT>::type;
119 : p1_ (reinterpret_cast<const
Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
122 template<
typename Key>
inline void
126 using T =
typename pcl::traits::datatype<PointInT, Key>::type;
127 const std::uint8_t* data_ptr =
reinterpret_cast<const std::uint8_t*
>(&p1_) + pcl::traits::offset<PointInT, Key>::value;
128 p2_[f_idx_++] =
static_cast<float> (*
reinterpret_cast<const T*
>(data_ptr));
133 Eigen::VectorXf &p2_;
172 template <
typename Po
intT>
192 assert (indices.size () <= pc.
size ());
193 for (std::size_t i = 0; i < indices.size (); i++)
194 points[i] = pc[indices[i]];
203 :
points (width_ * height_, value_)
263 at (
int column,
int row)
const
266 return (
points.at (row * this->width + column));
277 at (
int column,
int row)
280 return (
points.at (row * this->width + column));
333 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
336 if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
337 return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(
reinterpret_cast<float*
>(&
points[0])+offset,
size (), dim, Eigen::OuterStride<> (stride)));
339 return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(
reinterpret_cast<float*
>(&
points[0])+offset, dim,
size (), Eigen::OuterStride<> (stride)));
359 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
362 if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
363 return (Eigen::Map<
const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(
reinterpret_cast<float*
>(
const_cast<PointT*
>(&
points[0]))+offset,
size (), dim, Eigen::OuterStride<> (stride)));
365 return (Eigen::Map<
const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(
reinterpret_cast<float*
>(
const_cast<PointT*
>(&
points[0]))+offset, dim,
size (), Eigen::OuterStride<> (stride)));
374 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
386 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >
396 std::vector<PointT, Eigen::aligned_allocator<PointT> >
points;
412 using VectorType = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
413 using CloudVectorType = std::vector<PointCloud<PointT>, Eigen::aligned_allocator<PointCloud<PointT> > >;
414 using Ptr = shared_ptr<PointCloud<PointT> >;
415 using ConstPtr = shared_ptr<const PointCloud<PointT> >;
467 width =
static_cast<std::uint32_t
>(count);
485 points.resize(new_width * new_height);
505 points.resize(count, value);
526 points.resize(new_width * new_height, value);
551 points.assign(count, value);
552 width =
static_cast<std::uint32_t
>(
size());
565 points.assign(new_width * new_height, value);
577 template <
class InputIterator>
579 assign(InputIterator first, InputIterator last)
581 points.assign(std::move(first), std::move(last));
582 width =
static_cast<std::uint32_t
>(
size());
595 template <
class InputIterator>
599 if (new_width == 0) {
600 PCL_WARN(
"Assignment with new_width equal to 0,"
601 "setting width to size of the cloud and height to 1\n");
602 return assign(std::move(first), std::move(last));
605 points.assign(std::move(first), std::move(last));
609 PCL_WARN(
"Mismatch in assignment. Requested width (%zu) doesn't divide "
610 "provided size (%zu) cleanly. Setting height to 1\n",
611 static_cast<std::size_t
>(
width),
612 static_cast<std::size_t
>(
size()));
624 inline assign(std::initializer_list<PointT> ilist)
626 points.assign(std::move(ilist));
627 width =
static_cast<std::uint32_t
>(
size());
641 if (new_width == 0) {
642 PCL_WARN(
"Assignment with new_width equal to 0,"
643 "setting width to size of the cloud and height to 1\n");
644 return assign(std::move(ilist));
646 points.assign(std::move(ilist));
650 PCL_WARN(
"Mismatch in assignment. Requested width (%zu) doesn't divide "
651 "provided size (%zu) cleanly. Setting height to 1\n",
652 static_cast<std::size_t
>(
width),
653 static_cast<std::size_t
>(
size()));
686 template <
class... Args>
inline reference
689 points.emplace_back (std::forward<Args> (args)...);
700 template <
class... Args>
inline reference
703 points.emplace_back (std::forward<Args> (args)...);
744 points.insert (std::move(position), n, pt);
758 points.insert (std::move(position), n, pt);
767 template <
class InputIterator>
inline void
770 points.insert (std::move(position), std::move(first), std::move(last));
781 template <
class InputIterator>
inline void
784 points.insert (std::move(position), std::move(first), std::move(last));
793 template <
class... Args>
inline iterator
796 iterator it =
points.emplace (std::move(position), std::forward<Args> (args)...);
808 template <
class... Args>
inline iterator
811 iterator it =
points.emplace (std::move(position), std::forward<Args> (args)...);
876 this->points.swap (rhs.
points);
905 template <
typename Po
intT> std::ostream&
908 s <<
"header: " << p.
header << std::endl;
909 s <<
"points[]: " << p.
size () << std::endl;
910 s <<
"width: " << p.
width << std::endl;
911 s <<
"height: " << p.
height << std::endl;
912 s <<
"is_dense: " << p.
is_dense << std::endl;
913 s <<
"sensor origin (xyz): [" <<
PointCloud represents the base class in PCL for storing collections of 3D points.
typename VectorType::size_type size_type
iterator erase(iterator position)
Erase a point in the cloud.
const_reverse_iterator rend() const noexcept
void resize(uindex_t new_width, uindex_t new_height)
Resizes the container to contain new_width * new_height elements.
const_iterator cbegin() const noexcept
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
reference transient_emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
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.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
reverse_iterator rbegin() noexcept
const_iterator begin() const noexcept
const_iterator cend() const noexcept
const PointT & operator[](std::size_t n) const
void insert(iterator position, std::size_t n, const PointT &pt)
Insert a new point in the cloud N times, given an iterator.
void transient_insert(iterator position, std::size_t n, const PointT &pt)
Insert a new point in the cloud N times, given an iterator.
iterator erase(iterator first, iterator last)
Erase a set of points given by a (first, last) iterator pair.
const_reverse_iterator crend() const noexcept
PointCloud & operator+=(const PointCloud &rhs)
Add a point cloud to the current cloud.
Eigen::Map< Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap()
PointCloud(std::uint32_t width_, std::uint32_t height_, const PointT &value_=PointT())
Allocate constructor from point cloud subset.
void resize(index_t count, const PointT &value)
Resizes the container to contain count elements.
PointT & at(std::size_t n)
const_reverse_iterator crbegin() const noexcept
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void insert(iterator position, InputIterator first, InputIterator last)
Insert a new range of points in the cloud, at a certain position.
iterator transient_erase(iterator first, iterator last)
Erase a set of points given by a (first, last) iterator pair.
PointCloud operator+(const PointCloud &rhs)
Add a point cloud to another cloud.
typename VectorType::reverse_iterator reverse_iterator
const PointT & at(std::size_t n) const
const PointT & front() const
iterator transient_erase(iterator position)
Erase a point in the cloud.
void resize(std::size_t count)
Resizes the container to contain count elements.
void assign(InputIterator first, InputIterator last, index_t new_width)
Replaces the points with copies of those in the range [first, last)
typename VectorType::iterator iterator
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
void transient_push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
const PointT & back() const
std::uint32_t width
The point cloud width (if organized as an image-structure).
void assign(std::initializer_list< PointT > ilist)
Replaces the points with the elements from the initializer list ilist
reference emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
PointCloud(const PointCloud< PointT > &pc, const Indices &indices)
Copy constructor from point cloud subset.
PointCloud()=default
Default constructor.
pcl::PCLHeader header
The point cloud header.
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
iterator emplace(iterator position, Args &&...args)
Emplace a new point in the cloud, given an iterator.
static bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
void assign(InputIterator first, InputIterator last)
Replaces the points with copies of those in the range [first, last)
typename VectorType::const_iterator const_iterator
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
std::uint32_t height
The point cloud height (if organized as an image-structure).
PointT & at(int column, int row)
Obtain the point given by the (column, row) coordinates.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void resize(index_t new_width, index_t new_height, const PointT &value)
Resizes the container to contain count elements.
void transient_insert(iterator position, InputIterator first, InputIterator last)
Insert a new range of points in the cloud, at a certain position.
typename VectorType::const_reverse_iterator const_reverse_iterator
void assign(std::initializer_list< PointT > ilist, index_t new_width)
Replaces the points with the elements from the initializer list ilist
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
void reserve(std::size_t n)
const PointT & operator()(std::size_t column, std::size_t row) const
Obtain the point given by the (column, row) coordinates.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
shared_ptr< PointCloud< PointT > > Ptr
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.
typename VectorType::difference_type difference_type
index_t max_size() const noexcept
const_iterator end() const noexcept
iterator begin() noexcept
void assign(index_t count, const PointT &value)
Replaces the points with count copies of value
const Eigen::Map< const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > getMatrixXfMap() const
const_reverse_iterator rbegin() const noexcept
void assign(index_t new_width, index_t new_height, const PointT &value)
Replaces the points with new_width * new_height copies of value
iterator transient_insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
iterator transient_emplace(iterator position, Args &&...args)
Emplace a new point in the cloud, given an iterator.
static bool concatenate(pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
reverse_iterator rend() noexcept
shared_ptr< const PointCloud< PointT > > ConstPtr
const PointT * data() const noexcept
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
An exception that is thrown when an organized point cloud is needed but not provided.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Defines functions, macros and traits for allocating and using memory.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
IndicesAllocator<> Indices
Type used for indices in PCL.
std::vector< detail::FieldMapping > MsgFieldMap
Defines all the PCL and non-PCL macros used.
Helper functor structure for copying data between an Eigen type and a PointT.
NdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointOutT &p2)
Constructor.
typename traits::POD< PointOutT >::type Pod
void operator()()
Operator.
Helper functor structure for copying data between an Eigen type and a PointT.
typename traits::POD< PointInT >::type Pod
void operator()()
Operator.
NdCopyPointEigenFunctor(const PointInT &p1, Eigen::VectorXf &p2)
Constructor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
std::size_t struct_offset
std::size_t serialized_offset
Defines basic non-point types used by PCL.