43 #pragma GCC system_header
46 #include <pcl/PCLPointField.h>
47 #include <pcl/PCLPointCloud2.h>
48 #include <pcl/PCLImage.h>
49 #include <pcl/point_cloud.h>
50 #include <pcl/type_traits.h>
51 #include <pcl/for_each_type.h>
52 #include <pcl/console/print.h>
54 #include <boost/foreach.hpp>
61 template<
typename Po
intT>
69 f.
name = pcl::traits::name<PointT, U>::value;
70 f.
offset = pcl::traits::offset<PointT, U>::value;
71 f.
datatype = pcl::traits::datatype<PointT, U>::value;
72 f.
count = pcl::traits::datatype<PointT, U>::size;
80 template<
typename Po
intT>
84 std::vector<FieldMapping>& map)
89 template<
typename Tag>
void
92 for (
const auto& field :
fields_)
98 mapping.
struct_offset = pcl::traits::offset<PointT, Tag>::value;
99 mapping.
size =
sizeof (
typename pcl::traits::datatype<PointT, Tag>::type);
100 map_.push_back (mapping);
105 PCL_WARN (
"Failed to find match for field '%s'.\n", pcl::traits::name<PointT, Tag>::value);
109 const std::vector<pcl::PCLPointField>&
fields_;
110 std::vector<FieldMapping>&
map_;
121 template<
typename Po
intT>
void
126 for_each_type< typename traits::fieldList<PointT>::type > (mapper);
129 if (field_map.size() > 1)
132 MsgFieldMap::iterator i = field_map.begin(), j = i + 1;
133 while (j != field_map.end())
138 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
140 i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
141 j = field_map.erase(j);
165 template <
typename Po
intT>
void
177 cloud.
resize (num_points);
178 std::uint8_t* cloud_data =
reinterpret_cast<std::uint8_t*
>(&cloud[0]);
183 if (field_map.size() == 1 &&
184 field_map[0].serialized_offset == 0 &&
185 field_map[0].struct_offset == 0 &&
187 field_map[0].size ==
sizeof(
PointT))
189 std::uint32_t cloud_row_step =
static_cast<std::uint32_t
> (
sizeof (
PointT) * cloud.
width);
190 const std::uint8_t* msg_data = &msg.
data[0];
194 memcpy (cloud_data, msg_data, msg.
data.size ());
198 for (std::uint32_t i = 0; i < msg.
height; ++i, cloud_data += cloud_row_step, msg_data += msg.
row_step)
199 memcpy (cloud_data, msg_data, cloud_row_step);
208 const std::uint8_t* row_data = &msg.
data[row * msg.
row_step];
211 const std::uint8_t* msg_data = row_data + col * msg.
point_step;
214 memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size);
216 cloud_data +=
sizeof (
PointT);
226 template<
typename Po
intT>
void
230 createMapping<PointT> (msg.
fields, field_map);
238 template<
typename Po
intT>
void
255 std::size_t data_size =
sizeof (
PointT) * cloud.
size ();
256 msg.
data.resize (data_size);
259 memcpy(&msg.
data[0], &cloud[0], data_size);
279 template<
typename CloudT>
void
283 if (cloud.width == 0 && cloud.height == 0)
284 throw std::runtime_error(
"Needs to be a dense like cloud!!");
287 if (cloud.size () != cloud.width * cloud.height)
288 throw std::runtime_error(
"The width and height do not match the cloud size!");
289 msg.
height = cloud.height;
290 msg.
width = cloud.width;
294 msg.
header = cloud.header;
296 msg.
step = msg.
width *
sizeof (std::uint8_t) * 3;
298 for (std::size_t y = 0; y < cloud.height; y++)
300 for (std::size_t x = 0; x < cloud.width; x++)
302 std::uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
303 memcpy (pixel, &cloud (x, y).rgb, 3 *
sizeof(std::uint8_t));
316 const auto predicate = [](
const auto& field) {
return field.name ==
"rgb"; };
317 const auto result = std::find_if(cloud.
fields.cbegin (), cloud.
fields.cend (), predicate);
318 if (result == cloud.
fields.end ())
319 throw std::runtime_error (
"No rgb field!!");
323 throw std::runtime_error (
"Needs to be a dense like cloud!!");
329 int rgb_offset = cloud.
fields[rgb_index].offset;
335 msg.
step =
static_cast<std::uint32_t
>(msg.
width *
sizeof (std::uint8_t) * 3);
338 for (std::size_t y = 0; y < cloud.
height; y++)
340 for (std::size_t x = 0; x < cloud.
width; x++, rgb_offset += point_step)
342 std::uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
343 memcpy (pixel, &(cloud.
data[rgb_offset]), 3 * sizeof (std::uint8_t));
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
bool fieldOrdering(const FieldMapping &a, const FieldMapping &b)
float distance(const PointT &p1, const PointT &p2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
void createMapping(const std::vector< pcl::PCLPointField > &msg_fields, MsgFieldMap &field_map)
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
std::vector< detail::FieldMapping > MsgFieldMap
std::vector< std::uint8_t > data
std::vector<::pcl::PCLPointField > fields
std::vector< std::uint8_t > data
A point structure representing Euclidean xyz coordinates, and the RGB color.
FieldAdder(std::vector< pcl::PCLPointField > &fields)
std::vector< pcl::PCLPointField > & fields_
FieldMapper(const std::vector< pcl::PCLPointField > &fields, std::vector< FieldMapping > &map)
const std::vector< pcl::PCLPointField > & fields_
std::vector< FieldMapping > & map_
std::size_t struct_offset
std::size_t serialized_offset