40 #ifndef PCL_CONVERSIONS_H_
41 #define PCL_CONVERSIONS_H_
44 #pragma GCC system_header
47 #include <pcl/PCLPointField.h>
48 #include <pcl/PCLPointCloud2.h>
49 #include <pcl/PCLImage.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/point_traits.h>
52 #include <pcl/for_each_type.h>
53 #include <pcl/exceptions.h>
54 #include <pcl/console/print.h>
55 #include <boost/foreach.hpp>
62 template<
typename Po
intT>
81 template<
typename Po
intT>
85 std::vector<FieldMapping>& map)
90 template<
typename Tag>
void
101 map_.push_back (mapping);
110 const std::vector<pcl::PCLPointField>&
fields_;
111 std::vector<FieldMapping>&
map_;
122 template<
typename Po
intT>
void
127 for_each_type< typename traits::fieldList<PointT>::type > (mapper);
130 if (field_map.size() > 1)
133 MsgFieldMap::iterator i = field_map.begin(), j = i + 1;
134 while (j != field_map.end())
139 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
141 i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
142 j = field_map.erase(j);
166 template <
typename Po
intT>
void
178 cloud.
points.resize (num_points);
179 uint8_t* cloud_data =
reinterpret_cast<uint8_t*
>(&cloud.
points[0]);
182 if (field_map.size() == 1 &&
183 field_map[0].serialized_offset == 0 &&
184 field_map[0].struct_offset == 0 &&
187 uint32_t cloud_row_step =
static_cast<uint32_t
> (
sizeof (
PointT) * cloud.
width);
188 const uint8_t* msg_data = &msg.
data[0];
192 memcpy (cloud_data, msg_data, msg.
data.size ());
196 for (uint32_t i = 0; i < msg.
height; ++i, cloud_data += cloud_row_step, msg_data += msg.
row_step)
197 memcpy (cloud_data, msg_data, cloud_row_step);
204 for (uint32_t row = 0; row < msg.
height; ++row)
206 const uint8_t* row_data = &msg.
data[row * msg.
row_step];
207 for (uint32_t col = 0; col < msg.
width; ++col)
209 const uint8_t* msg_data = row_data + col * msg.
point_step;
214 cloud_data +=
sizeof (
PointT);
224 template<
typename Po
intT>
void
228 createMapping<PointT> (msg.
fields, field_map);
236 template<
typename Po
intT>
void
242 msg.
width =
static_cast<uint32_t
>(cloud.
points.size ());
253 size_t data_size =
sizeof (
PointT) * cloud.
points.size ();
254 msg.
data.resize (data_size);
255 memcpy (&msg.
data[0], &cloud.
points[0], data_size);
274 template<
typename CloudT>
void
278 if (cloud.width == 0 && cloud.height == 0)
279 throw std::runtime_error(
"Needs to be a dense like cloud!!");
282 if (cloud.points.size () != cloud.width * cloud.height)
283 throw std::runtime_error(
"The width and height do not match the cloud size!");
284 msg.
height = cloud.height;
285 msg.
width = cloud.width;
290 msg.
step = msg.
width *
sizeof (uint8_t) * 3;
292 for (
size_t y = 0; y < cloud.height; y++)
294 for (
size_t x = 0; x < cloud.width; x++)
296 uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
297 memcpy (pixel, &cloud (x, y).rgb, 3 *
sizeof(uint8_t));
312 for (
size_t d = 0; d < cloud.
fields.size (); ++d)
313 if (cloud.
fields[d].name ==
"rgb")
315 rgb_index =
static_cast<int>(d);
320 throw std::runtime_error (
"No rgb field!!");
322 throw std::runtime_error (
"Needs to be a dense like cloud!!");
328 int rgb_offset = cloud.
fields[rgb_index].offset;
333 msg.
step =
static_cast<uint32_t
>(msg.
width *
sizeof (uint8_t) * 3);
336 for (
size_t y = 0; y < cloud.
height; y++)
338 for (
size_t x = 0; x < cloud.
width; x++, rgb_offset += point_step)
340 uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
341 memcpy (pixel, &(cloud.
data[rgb_offset]), 3 * sizeof (uint8_t));
347 #endif //#ifndef PCL_CONVERSIONS_H_
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< pcl::PCLPointField > & fields_
bool fieldOrdering(const FieldMapping &a, const FieldMapping &b)
uint32_t height
The point cloud height (if organized as an image-structure).
uint32_t width
The point cloud width (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::vector< ::pcl::PCLPointField > fields
pcl::PCLHeader header
The point cloud header.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob.
std::vector< pcl::uint8_t > data
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
A point structure representing Euclidean xyz coordinates, and the RGB color.
FieldAdder(std::vector< pcl::PCLPointField > &fields)
std::vector< FieldMapping > & map_
std::vector< detail::FieldMapping > MsgFieldMap
std::vector< pcl::uint8_t > data
void createMapping(const std::vector< pcl::PCLPointField > &msg_fields, MsgFieldMap &field_map)
const std::vector< pcl::PCLPointField > & fields_
FieldMapper(const std::vector< pcl::PCLPointField > &fields, std::vector< FieldMapping > &map)