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_