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> 56 #include <boost/foreach.hpp> 64 template<
typename Po
intT>
83 template<
typename Po
intT>
87 std::vector<FieldMapping>& map)
92 template<
typename Tag>
void 103 map_.push_back (mapping);
112 const std::vector<pcl::PCLPointField>&
fields_;
113 std::vector<FieldMapping>&
map_;
124 template<
typename Po
intT>
void 129 for_each_type< typename traits::fieldList<PointT>::type > (mapper);
132 if (field_map.size() > 1)
135 MsgFieldMap::iterator i = field_map.begin(), j = i + 1;
136 while (j != field_map.end())
141 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
143 i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
144 j = field_map.erase(j);
168 template <
typename Po
intT>
void 180 cloud.
points.resize (num_points);
181 uint8_t* cloud_data =
reinterpret_cast<uint8_t*
>(&cloud.
points[0]);
184 if (field_map.size() == 1 &&
185 field_map[0].serialized_offset == 0 &&
186 field_map[0].struct_offset == 0 &&
189 uint32_t cloud_row_step =
static_cast<uint32_t
> (
sizeof (
PointT) * cloud.
width);
190 const uint8_t* msg_data = &msg.
data[0];
194 memcpy (cloud_data, msg_data, msg.
data.size ());
198 for (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);
206 for (uint32_t row = 0; row < msg.
height; ++row)
208 const uint8_t* row_data = &msg.
data[row * msg.
row_step];
209 for (uint32_t col = 0; col < msg.
width; ++col)
211 const uint8_t* msg_data = row_data + col * msg.
point_step;
216 cloud_data +=
sizeof (
PointT);
226 template<
typename Po
intT>
void 230 createMapping<PointT> (msg.
fields, field_map);
238 template<
typename Po
intT>
void 244 msg.
width =
static_cast<uint32_t
>(cloud.
points.size ());
255 size_t data_size =
sizeof (
PointT) * cloud.
points.size ();
256 msg.
data.resize (data_size);
257 memcpy (&msg.
data[0], &cloud.
points[0], data_size);
276 template<
typename CloudT>
void 280 if (cloud.width == 0 && cloud.height == 0)
281 throw std::runtime_error(
"Needs to be a dense like cloud!!");
284 if (cloud.points.size () != cloud.width * cloud.height)
285 throw std::runtime_error(
"The width and height do not match the cloud size!");
286 msg.
height = cloud.height;
287 msg.
width = cloud.width;
292 msg.
step = msg.
width *
sizeof (uint8_t) * 3;
294 for (
size_t y = 0; y < cloud.height; y++)
296 for (
size_t x = 0; x < cloud.width; x++)
298 uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
299 memcpy (pixel, &cloud (x, y).rgb, 3 *
sizeof(uint8_t));
314 for (
size_t d = 0; d < cloud.
fields.size (); ++d)
315 if (cloud.
fields[d].name ==
"rgb")
317 rgb_index =
static_cast<int>(d);
322 throw std::runtime_error (
"No rgb field!!");
324 throw std::runtime_error (
"Needs to be a dense like cloud!!");
330 int rgb_offset = cloud.
fields[rgb_index].offset;
335 msg.
step =
static_cast<uint32_t
>(msg.
width *
sizeof (uint8_t) * 3);
338 for (
size_t y = 0; y < cloud.
height; y++)
340 for (
size_t x = 0; x < cloud.
width; x++, rgb_offset += point_step)
342 uint8_t * pixel = &(msg.
data[y * msg.
step + x * 3]);
343 memcpy (pixel, &(cloud.
data[rgb_offset]), 3 * sizeof (uint8_t));
349 #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<T> 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<T> 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)