Point Cloud Library (PCL)  1.8.1
conversions.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_CONVERSIONS_H_
41 #define PCL_CONVERSIONS_H_
42 
43 #ifdef __GNUC__
44 #pragma GCC system_header
45 #endif
46 
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 #ifndef Q_MOC_RUN
56 #include <boost/foreach.hpp>
57 #endif
58 
59 namespace pcl
60 {
61  namespace detail
62  {
63  // For converting template point cloud to message.
64  template<typename PointT>
65  struct FieldAdder
66  {
67  FieldAdder (std::vector<pcl::PCLPointField>& fields) : fields_ (fields) {};
68 
69  template<typename U> void operator() ()
70  {
76  fields_.push_back (f);
77  }
78 
79  std::vector<pcl::PCLPointField>& fields_;
80  };
81 
82  // For converting message to template point cloud.
83  template<typename PointT>
84  struct FieldMapper
85  {
86  FieldMapper (const std::vector<pcl::PCLPointField>& fields,
87  std::vector<FieldMapping>& map)
88  : fields_ (fields), map_ (map)
89  {
90  }
91 
92  template<typename Tag> void
94  {
95  BOOST_FOREACH (const pcl::PCLPointField& field, fields_)
96  {
97  if (FieldMatches<PointT, Tag>()(field))
98  {
99  FieldMapping mapping;
100  mapping.serialized_offset = field.offset;
102  mapping.size = sizeof (typename traits::datatype<PointT, Tag>::type);
103  map_.push_back (mapping);
104  return;
105  }
106  }
107  // Disable thrown exception per #595: http://dev.pointclouds.org/issues/595
108  PCL_WARN ("Failed to find match for field '%s'.\n", traits::name<PointT, Tag>::value);
109  //throw pcl::InvalidConversionException (ss.str ());
110  }
111 
112  const std::vector<pcl::PCLPointField>& fields_;
113  std::vector<FieldMapping>& map_;
114  };
115 
116  inline bool
118  {
119  return (a.serialized_offset < b.serialized_offset);
120  }
121 
122  } //namespace detail
123 
124  template<typename PointT> void
125  createMapping (const std::vector<pcl::PCLPointField>& msg_fields, MsgFieldMap& field_map)
126  {
127  // Create initial 1-1 mapping between serialized data segments and struct fields
128  detail::FieldMapper<PointT> mapper (msg_fields, field_map);
129  for_each_type< typename traits::fieldList<PointT>::type > (mapper);
130 
131  // Coalesce adjacent fields into single memcpy's where possible
132  if (field_map.size() > 1)
133  {
134  std::sort(field_map.begin(), field_map.end(), detail::fieldOrdering);
135  MsgFieldMap::iterator i = field_map.begin(), j = i + 1;
136  while (j != field_map.end())
137  {
138  // This check is designed to permit padding between adjacent fields.
139  /// @todo One could construct a pathological case where the struct has a
140  /// field where the serialized data has padding
141  if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
142  {
143  i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
144  j = field_map.erase(j);
145  }
146  else
147  {
148  ++i;
149  ++j;
150  }
151  }
152  }
153  }
154 
155  /** \brief Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
156  * \param[in] msg the PCLPointCloud2 binary blob
157  * \param[out] cloud the resultant pcl::PointCloud<T>
158  * \param[in] field_map a MsgFieldMap object
159  *
160  * \note Use fromPCLPointCloud2 (PCLPointCloud2, PointCloud<T>) directly or create you
161  * own MsgFieldMap using:
162  *
163  * \code
164  * MsgFieldMap field_map;
165  * createMapping<PointT> (msg.fields, field_map);
166  * \endcode
167  */
168  template <typename PointT> void
170  const MsgFieldMap& field_map)
171  {
172  // Copy info fields
173  cloud.header = msg.header;
174  cloud.width = msg.width;
175  cloud.height = msg.height;
176  cloud.is_dense = msg.is_dense == 1;
177 
178  // Copy point data
179  uint32_t num_points = msg.width * msg.height;
180  cloud.points.resize (num_points);
181  uint8_t* cloud_data = reinterpret_cast<uint8_t*>(&cloud.points[0]);
182 
183  // Check if we can copy adjacent points in a single memcpy. We can do so if there
184  // is exactly one field to copy and it is the same size as the source and destination
185  // point types.
186  if (field_map.size() == 1 &&
187  field_map[0].serialized_offset == 0 &&
188  field_map[0].struct_offset == 0 &&
189  field_map[0].size == msg.point_step &&
190  field_map[0].size == sizeof(PointT))
191  {
192  uint32_t cloud_row_step = static_cast<uint32_t> (sizeof (PointT) * cloud.width);
193  const uint8_t* msg_data = &msg.data[0];
194  // Should usually be able to copy all rows at once
195  if (msg.row_step == cloud_row_step)
196  {
197  memcpy (cloud_data, msg_data, msg.data.size ());
198  }
199  else
200  {
201  for (uint32_t i = 0; i < msg.height; ++i, cloud_data += cloud_row_step, msg_data += msg.row_step)
202  memcpy (cloud_data, msg_data, cloud_row_step);
203  }
204 
205  }
206  else
207  {
208  // If not, memcpy each group of contiguous fields separately
209  for (uint32_t row = 0; row < msg.height; ++row)
210  {
211  const uint8_t* row_data = &msg.data[row * msg.row_step];
212  for (uint32_t col = 0; col < msg.width; ++col)
213  {
214  const uint8_t* msg_data = row_data + col * msg.point_step;
215  BOOST_FOREACH (const detail::FieldMapping& mapping, field_map)
216  {
217  memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size);
218  }
219  cloud_data += sizeof (PointT);
220  }
221  }
222  }
223  }
224 
225  /** \brief Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object.
226  * \param[in] msg the PCLPointCloud2 binary blob
227  * \param[out] cloud the resultant pcl::PointCloud<T>
228  */
229  template<typename PointT> void
231  {
232  MsgFieldMap field_map;
233  createMapping<PointT> (msg.fields, field_map);
234  fromPCLPointCloud2 (msg, cloud, field_map);
235  }
236 
237  /** \brief Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
238  * \param[in] cloud the input pcl::PointCloud<T>
239  * \param[out] msg the resultant PCLPointCloud2 binary blob
240  */
241  template<typename PointT> void
243  {
244  // Ease the user's burden on specifying width/height for unorganized datasets
245  if (cloud.width == 0 && cloud.height == 0)
246  {
247  msg.width = static_cast<uint32_t>(cloud.points.size ());
248  msg.height = 1;
249  }
250  else
251  {
252  assert (cloud.points.size () == cloud.width * cloud.height);
253  msg.height = cloud.height;
254  msg.width = cloud.width;
255  }
256 
257  // Fill point cloud binary data (padding and all)
258  size_t data_size = sizeof (PointT) * cloud.points.size ();
259  msg.data.resize (data_size);
260  if (data_size)
261  {
262  memcpy(&msg.data[0], &cloud.points[0], data_size);
263  }
264 
265  // Fill fields metadata
266  msg.fields.clear ();
267  for_each_type<typename traits::fieldList<PointT>::type> (detail::FieldAdder<PointT>(msg.fields));
268 
269  msg.header = cloud.header;
270  msg.point_step = sizeof (PointT);
271  msg.row_step = static_cast<uint32_t> (sizeof (PointT) * msg.width);
272  msg.is_dense = cloud.is_dense;
273  /// @todo msg.is_bigendian = ?;
274  }
275 
276  /** \brief Copy the RGB fields of a PointCloud into pcl::PCLImage format
277  * \param[in] cloud the point cloud message
278  * \param[out] msg the resultant pcl::PCLImage
279  * CloudT cloud type, CloudT should be akin to pcl::PointCloud<pcl::PointXYZRGBA>
280  * \note will throw std::runtime_error if there is a problem
281  */
282  template<typename CloudT> void
283  toPCLPointCloud2 (const CloudT& cloud, pcl::PCLImage& msg)
284  {
285  // Ease the user's burden on specifying width/height for unorganized datasets
286  if (cloud.width == 0 && cloud.height == 0)
287  throw std::runtime_error("Needs to be a dense like cloud!!");
288  else
289  {
290  if (cloud.points.size () != cloud.width * cloud.height)
291  throw std::runtime_error("The width and height do not match the cloud size!");
292  msg.height = cloud.height;
293  msg.width = cloud.width;
294  }
295 
296  // ensor_msgs::image_encodings::BGR8;
297  msg.encoding = "bgr8";
298  msg.step = msg.width * sizeof (uint8_t) * 3;
299  msg.data.resize (msg.step * msg.height);
300  for (size_t y = 0; y < cloud.height; y++)
301  {
302  for (size_t x = 0; x < cloud.width; x++)
303  {
304  uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
305  memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t));
306  }
307  }
308  }
309 
310  /** \brief Copy the RGB fields of a PCLPointCloud2 msg into pcl::PCLImage format
311  * \param cloud the point cloud message
312  * \param msg the resultant pcl::PCLImage
313  * will throw std::runtime_error if there is a problem
314  */
315  inline void
317  {
318  int rgb_index = -1;
319  // Get the index we need
320  for (size_t d = 0; d < cloud.fields.size (); ++d)
321  if (cloud.fields[d].name == "rgb")
322  {
323  rgb_index = static_cast<int>(d);
324  break;
325  }
326 
327  if(rgb_index == -1)
328  throw std::runtime_error ("No rgb field!!");
329  if (cloud.width == 0 && cloud.height == 0)
330  throw std::runtime_error ("Needs to be a dense like cloud!!");
331  else
332  {
333  msg.height = cloud.height;
334  msg.width = cloud.width;
335  }
336  int rgb_offset = cloud.fields[rgb_index].offset;
337  int point_step = cloud.point_step;
338 
339  // pcl::image_encodings::BGR8;
340  msg.encoding = "bgr8";
341  msg.step = static_cast<uint32_t>(msg.width * sizeof (uint8_t) * 3);
342  msg.data.resize (msg.step * msg.height);
343 
344  for (size_t y = 0; y < cloud.height; y++)
345  {
346  for (size_t x = 0; x < cloud.width; x++, rgb_offset += point_step)
347  {
348  uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
349  memcpy (pixel, &(cloud.data[rgb_offset]), 3 * sizeof (uint8_t));
350  }
351  }
352  }
353 }
354 
355 #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...
Definition: conversions.h:169
pcl::uint32_t height
Definition: PCLImage.h:24
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
std::vector< pcl::PCLPointField > & fields_
Definition: conversions.h:79
pcl::uint32_t point_step
pcl::uint32_t offset
Definition: PCLPointField.h:23
bool fieldOrdering(const FieldMapping &a, const FieldMapping &b)
Definition: conversions.h:117
::pcl::PCLHeader header
pcl::uint8_t is_dense
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::uint32_t width
Definition: PCLImage.h:25
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::uint32_t count
Definition: PCLPointField.h:25
pcl::uint32_t step
Definition: PCLImage.h:29
pcl::uint32_t row_step
pcl::uint32_t width
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.
Definition: point_cloud.h:407
pcl::uint32_t height
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:242
std::vector< pcl::uint8_t > data
Definition: PCLImage.h:31
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
std::string encoding
Definition: PCLImage.h:26
pcl::uint8_t datatype
Definition: PCLPointField.h:24
std::string name
Definition: PCLPointField.h:21
A point structure representing Euclidean xyz coordinates, and the RGB color.
FieldAdder(std::vector< pcl::PCLPointField > &fields)
Definition: conversions.h:67
std::vector< FieldMapping > & map_
Definition: conversions.h:113
std::vector< detail::FieldMapping > MsgFieldMap
Definition: point_cloud.h:65
std::vector< pcl::uint8_t > data
void createMapping(const std::vector< pcl::PCLPointField > &msg_fields, MsgFieldMap &field_map)
Definition: conversions.h:125
const std::vector< pcl::PCLPointField > & fields_
Definition: conversions.h:112
FieldMapper(const std::vector< pcl::PCLPointField > &fields, std::vector< FieldMapping > &map)
Definition: conversions.h:86