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>
63 template<
typename Po
intT>
71 f.
name = pcl::traits::name<PointT, U>::value;
72 f.
offset = pcl::traits::offset<PointT, U>::value;
73 f.
datatype = pcl::traits::datatype<PointT, U>::value;
74 f.
count = pcl::traits::datatype<PointT, U>::size;
82 template<
typename Po
intT>
86 std::vector<FieldMapping>& map)
91 template<
typename Tag>
void
94 for (
const auto& field :
fields_)
100 mapping.
struct_offset = pcl::traits::offset<PointT, Tag>::value;
101 mapping.
size =
sizeof (
typename pcl::traits::datatype<PointT, Tag>::type);
102 map_.push_back (mapping);
107 PCL_WARN (
"Failed to find match for field '%s'.\n", pcl::traits::name<PointT, Tag>::value);
111 const std::vector<pcl::PCLPointField>&
fields_;
112 std::vector<FieldMapping>&
map_;
123 template<
typename Po
intT>
void
128 for_each_type< typename traits::fieldList<PointT>::type > (mapper);
131 if (field_map.size() > 1)
134 MsgFieldMap::iterator i = field_map.begin(), j = i + 1;
135 while (j != field_map.end())
140 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
142 i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
143 j = field_map.erase(j);
163 template <
typename Po
intT>
void
165 const MsgFieldMap& field_map,
const std::uint8_t* msg_data)
179 PCL_WARN(
"[pcl::fromPCLPointCloud2] No data to copy.\n");
184 std::uint8_t* cloud_data =
reinterpret_cast<std::uint8_t*
>(cloud.
data());
189 if (field_map.size() == 1 &&
190 field_map[0].serialized_offset == 0 &&
191 field_map[0].struct_offset == 0 &&
193 field_map[0].size ==
sizeof(
PointT))
195 const auto cloud_row_step = (
sizeof (
PointT) * cloud.
width);
204 memcpy (cloud_data, msg_data, cloud_row_step);
211 for (std::size_t row = 0; row < msg.
height; ++row)
213 const std::uint8_t* row_data = msg_data + row * msg.
row_step;
214 for (std::size_t col = 0; col < msg.
width; ++col)
216 const std::uint8_t* msg_data = row_data + col * msg.
point_step;
219 std::copy(msg_data + mapping.serialized_offset, msg_data + mapping.serialized_offset + mapping.size,
220 cloud_data + mapping.struct_offset);
222 cloud_data +=
sizeof (
PointT);
241 template <
typename Po
intT>
void
252 template<
typename Po
intT>
void
256 createMapping<PointT> (msg.
fields, field_map);
263 template<
typename Po
intT>
268 memcpy(
msg_data_,
cloud_data_ + pcl::traits::offset<PointT, U>::value,
sizeof(
typename pcl::traits::datatype<PointT, U>::type));
269 msg_data_ +=
sizeof(
typename pcl::traits::datatype<PointT, U>::type);
278 template<
typename Po
intT>
286 f.
name = pcl::traits::name<PointT, U>::value;
287 f.
offset = pcl::traits::offset<PointT, U>::value;
288 f.
datatype = pcl::traits::datatype<PointT, U>::value;
289 f.
count = pcl::traits::datatype<PointT, U>::size;
291 field_sizes_.push_back (
sizeof(
typename pcl::traits::datatype<PointT, U>::type));
304 template<
typename Po
intT>
void
321 std::vector<std::size_t> field_sizes;
324 if (padding || std::accumulate(field_sizes.begin(), field_sizes.end(),
static_cast<std::size_t
>(0)) ==
sizeof (
PointT)) {
326 std::size_t data_size =
sizeof (
PointT) * cloud.
size ();
327 msg.
data.resize (data_size);
330 memcpy(msg.
data.data(), cloud.
data(), data_size);
336 std::size_t point_size = 0;
337 for(std::size_t i=0; i<msg.
fields.size(); ++i) {
338 msg.
fields[i].offset = point_size;
339 point_size += field_sizes[i];
341 msg.
data.resize (point_size * cloud.
size());
342 std::uint8_t* msg_data = &msg.
data[0];
343 const std::uint8_t* cloud_data=
reinterpret_cast<const std::uint8_t*
>(&cloud[0]);
344 const std::uint8_t* end = cloud_data +
sizeof (
PointT) * cloud.
size ();
346 for (; cloud_data<end; cloud_data+=
sizeof(
PointT)) {
347 for_each_type< typename traits::fieldList<PointT>::type > (copier);
362 template<
typename Po
intT>
void
374 template<
typename CloudT>
void
378 if (cloud.width == 0 && cloud.height == 0)
379 throw std::runtime_error(
"Needs to be a dense like cloud!!");
382 if (cloud.size () != cloud.width * cloud.height)
383 throw std::runtime_error(
"The width and height do not match the cloud size!");
384 msg.
height = cloud.height;
385 msg.
width = cloud.width;
389 msg.
header = cloud.header;
391 msg.
step = msg.
width *
sizeof (std::uint8_t) * 3;
393 for (std::size_t y = 0; y < cloud.height; y++)
395 for (std::size_t x = 0; x < cloud.width; x++)
397 std::uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
398 memcpy (pixel, &cloud (x, y).rgb, 3 *
sizeof(std::uint8_t));
411 const auto predicate = [](
const auto& field) {
return field.name ==
"rgb"; };
412 const auto result = std::find_if(cloud.
fields.cbegin (), cloud.
fields.cend (), predicate);
413 if (result == cloud.
fields.end ())
414 throw std::runtime_error (
"No rgb field!!");
418 throw std::runtime_error (
"Needs to be a dense like cloud!!");
424 auto rgb_offset = cloud.
fields[rgb_index].offset;
430 msg.
step = (msg.
width *
sizeof (std::uint8_t) * 3);
433 for (std::size_t y = 0; y < cloud.
height; y++)
435 for (std::size_t x = 0; x < cloud.
width; x++, rgb_offset += point_step)
437 std::uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
438 std::copy(&cloud.
data[rgb_offset], &cloud.
data[rgb_offset] + 3, pixel);
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, false > uindex_t
Type used for an unsigned index in PCL.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg, bool padding)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
void createMapping(const std::vector< pcl::PCLPointField > &msg_fields, MsgFieldMap &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.
Used together with pcl::for_each_type, creates list of all fields, and list of size of each field.
FieldAdderAdvanced(std::vector< pcl::PCLPointField > &fields, std::vector< std::size_t > &field_sizes)
std::vector< std::size_t > & field_sizes_
std::vector< pcl::PCLPointField > & fields_
FieldAdder(std::vector< pcl::PCLPointField > &fields)
std::vector< pcl::PCLPointField > & fields_
Used together with pcl::for_each_type, copies all point fields from cloud_data (respecting each field...
FieldCopier(std::uint8_t *&msg_data, const std::uint8_t *&cloud_data)
const std::uint8_t *& cloud_data_
std::uint8_t *& msg_data_
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