1 #pragma once
2 
3 #include <ostream>
4 #include <vector>
5 
6 #include <boost/predef/other/endian.h>
7 
8 #include <pcl/pcl_macros.h>  // for PCL_EXPORTS
9 #include <pcl/PCLHeader.h>
10 #include <pcl/PCLPointField.h>
11 #include <pcl/types.h>
12 
13 namespace pcl
14 {
15 
16   struct PCL_EXPORTS PCLPointCloud2
17   {
18     ::pcl::PCLHeader header;
19 
20     uindex_t height = 0;
21     uindex_t width = 0;
22 
23     std::vector<::pcl::PCLPointField>  fields;
24 
25     static_assert(BOOST_ENDIAN_BIG_BYTE || BOOST_ENDIAN_LITTLE_BYTE, "unable to determine system endianness");
26     std::uint8_t is_bigendian = BOOST_ENDIAN_BIG_BYTE;
27     uindex_t point_step = 0;
28     uindex_t row_step = 0;
29 
30     std::vector<std::uint8_t> data;
31 
32     std::uint8_t is_dense = 0;
33 
34   public:
35     using Ptr = shared_ptr< ::pcl::PCLPointCloud2>;
36     using ConstPtr = shared_ptr<const ::pcl::PCLPointCloud2>;
37 
38     //////////////////////////////////////////////////////////////////////////
39     /** \brief Inplace concatenate two pcl::PCLPointCloud2
40       *
41       * IFF the layout of all the fields in both the clouds is the same, this command
42       * doesn't remove any fields named "_" (aka marked as skip). For comparison of field
43       * names, "rgb" and "rgba" are considered equivalent
44       * However, if the order and/or number of non-skip fields is different, the skip fields
45       * are dropped and non-skip fields copied selectively.
46       * This function returns an error if
47       *   * the total number of non-skip fields is different
48       *   * the non-skip field names are named differently (excluding "rbg{a}") in serial order
49       *   * the endian-ness of both clouds is different
50       * \param[in,out] cloud1 the first input and output point cloud dataset
51       * \param[in] cloud2 the second input point cloud dataset
52       * \return true if successful, false if failed (e.g., name/number of fields differs)
53       */
54     static bool
55     concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2);
56 
57     /** \brief Concatenate two pcl::PCLPointCloud2
58       * \param[in] cloud1 the first input point cloud dataset
59       * \param[in] cloud2 the second input point cloud dataset
60       * \param[out] cloud_out the resultant output point cloud dataset
61       * \return true if successful, false if failed (e.g., name/number of fields differs)
62       */
63     static bool
concatenatePCLPointCloud264     concatenate (const PCLPointCloud2 &cloud1,
65                  const PCLPointCloud2 &cloud2,
66                  PCLPointCloud2 &cloud_out)
67     {
68       cloud_out = cloud1;
69       return concatenate(cloud_out, cloud2);
70     }
71 
72     /** \brief Add a point cloud to the current cloud.
73       * \param[in] rhs the cloud to add to the current cloud
74       * \return the new cloud as a concatenation of the current cloud and the new given cloud
75       */
76     PCLPointCloud2&
77     operator += (const PCLPointCloud2& rhs);
78 
79     /** \brief Add a point cloud to another cloud.
80       * \param[in] rhs the cloud to add to the current cloud
81       * \return the new cloud as a concatenation of the current cloud and the new given cloud
82       */
83     inline PCLPointCloud2
84     operator + (const PCLPointCloud2& rhs)
85     {
86       return (PCLPointCloud2 (*this) += rhs);
87     }
88   }; // struct PCLPointCloud2
89 
90   using PCLPointCloud2Ptr = PCLPointCloud2::Ptr;
91   using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr;
92 
93   inline std::ostream& operator<<(std::ostream& s, const  ::pcl::PCLPointCloud2 &v)
94   {
95     s << "header: " << std::endl;
96     s << v.header;
97     s << "height: ";
98     s << "  " << v.height << std::endl;
99     s << "width: ";
100     s << "  " << v.width << std::endl;
101     s << "fields[]" << std::endl;
102     for (std::size_t i = 0; i < v.fields.size (); ++i)
103     {
104       s << "  fields[" << i << "]: ";
105       s << std::endl;
106       s << "    " << v.fields[i] << std::endl;
107     }
108     s << "is_bigendian: ";
109     s << "  " << v.is_bigendian << std::endl;
110     s << "point_step: ";
111     s << "  " << v.point_step << std::endl;
112     s << "row_step: ";
113     s << "  " << v.row_step << std::endl;
114     s << "data[]" << std::endl;
115     for (std::size_t i = 0; i < v.data.size (); ++i)
116     {
117       s << "  data[" << i << "]: ";
118       s << "  " << v.data[i] << std::endl;
119     }
120     s << "is_dense: ";
121     s << "  " << v.is_dense << std::endl;
122 
123     return (s);
124   }
125 
126 } // namespace pcl
127