Point Cloud Library (PCL)  1.7.0
io.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_COMMON_IO_H_
42 #define PCL_COMMON_IO_H_
43 
44 #include <string>
45 #include <pcl/pcl_base.h>
46 #include <pcl/PointIndices.h>
47 #include <pcl/conversions.h>
48 #include <locale>
49 
50 namespace pcl
51 {
52  /** \brief Get the index of a specified field (i.e., dimension/channel)
53  * \param[in] cloud the the point cloud message
54  * \param[in] field_name the string defining the field name
55  * \ingroup common
56  */
57  inline int
58  getFieldIndex (const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
59  {
60  // Get the index we need
61  for (size_t d = 0; d < cloud.fields.size (); ++d)
62  if (cloud.fields[d].name == field_name)
63  return (static_cast<int>(d));
64  return (-1);
65  }
66 
67  /** \brief Get the index of a specified field (i.e., dimension/channel)
68  * \param[in] cloud the the point cloud message
69  * \param[in] field_name the string defining the field name
70  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
71  * \ingroup common
72  */
73  template <typename PointT> inline int
74  getFieldIndex (const pcl::PointCloud<PointT> &cloud, const std::string &field_name,
75  std::vector<pcl::PCLPointField> &fields);
76 
77  /** \brief Get the index of a specified field (i.e., dimension/channel)
78  * \param[in] field_name the string defining the field name
79  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
80  * \ingroup common
81  */
82  template <typename PointT> inline int
83  getFieldIndex (const std::string &field_name,
84  std::vector<pcl::PCLPointField> &fields);
85 
86  /** \brief Get the list of available fields (i.e., dimension/channel)
87  * \param[in] cloud the point cloud message
88  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
89  * \ingroup common
90  */
91  template <typename PointT> inline void
92  getFields (const pcl::PointCloud<PointT> &cloud, std::vector<pcl::PCLPointField> &fields);
93 
94  /** \brief Get the list of available fields (i.e., dimension/channel)
95  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
96  * \ingroup common
97  */
98  template <typename PointT> inline void
99  getFields (std::vector<pcl::PCLPointField> &fields);
100 
101  /** \brief Get the list of all fields available in a given cloud
102  * \param[in] cloud the the point cloud message
103  * \ingroup common
104  */
105  template <typename PointT> inline std::string
106  getFieldsList (const pcl::PointCloud<PointT> &cloud);
107 
108  /** \brief Get the available point cloud fields as a space separated string
109  * \param[in] cloud a pointer to the PointCloud message
110  * \ingroup common
111  */
112  inline std::string
114  {
115  std::string result;
116  for (size_t i = 0; i < cloud.fields.size () - 1; ++i)
117  result += cloud.fields[i].name + " ";
118  result += cloud.fields[cloud.fields.size () - 1].name;
119  return (result);
120  }
121 
122  /** \brief Obtains the size of a specific field data type in bytes
123  * \param[in] datatype the field data type (see PCLPointField.h)
124  * \ingroup common
125  */
126  inline int
127  getFieldSize (const int datatype)
128  {
129  switch (datatype)
130  {
133  return (1);
134 
137  return (2);
138 
142  return (4);
143 
145  return (8);
146 
147  default:
148  return (0);
149  }
150  }
151 
152  /** \brief Obtain a vector with the sizes of all valid fields (e.g., not "_")
153  * \param[in] fields the input vector containing the fields
154  * \param[out] field_sizes the resultant field sizes in bytes
155  */
156  PCL_EXPORTS void
157  getFieldsSizes (const std::vector<pcl::PCLPointField> &fields,
158  std::vector<int> &field_sizes);
159 
160  /** \brief Obtains the type of the PCLPointField from a specific size and type
161  * \param[in] size the size in bytes of the data field
162  * \param[in] type a char describing the type of the field ('F' = float, 'I' = signed, 'U' = unsigned)
163  * \ingroup common
164  */
165  inline int
166  getFieldType (const int size, char type)
167  {
168  type = std::toupper (type, std::locale::classic ());
169  switch (size)
170  {
171  case 1:
172  if (type == 'I')
173  return (pcl::PCLPointField::INT8);
174  if (type == 'U')
175  return (pcl::PCLPointField::UINT8);
176 
177  case 2:
178  if (type == 'I')
179  return (pcl::PCLPointField::INT16);
180  if (type == 'U')
182 
183  case 4:
184  if (type == 'I')
185  return (pcl::PCLPointField::INT32);
186  if (type == 'U')
188  if (type == 'F')
190 
191  case 8:
193 
194  default:
195  return (-1);
196  }
197  }
198 
199  /** \brief Obtains the type of the PCLPointField from a specific PCLPointField as a char
200  * \param[in] type the PCLPointField field type
201  * \ingroup common
202  */
203  inline char
204  getFieldType (const int type)
205  {
206  switch (type)
207  {
211  return ('I');
212 
216  return ('U');
217 
220  return ('F');
221  default:
222  return ('?');
223  }
224  }
225 
226  /** \brief Concatenate two pcl::PCLPointCloud2.
227  * \param[in] cloud1 the first input point cloud dataset
228  * \param[in] cloud2 the second input point cloud dataset
229  * \param[out] cloud_out the resultant output point cloud dataset
230  * \return true if successful, false if failed (e.g., name/number of fields differs)
231  * \ingroup common
232  */
233  PCL_EXPORTS bool
235  const pcl::PCLPointCloud2 &cloud2,
236  pcl::PCLPointCloud2 &cloud_out);
237 
238  /** \brief Extract the indices of a given point cloud as a new point cloud
239  * \param[in] cloud_in the input point cloud dataset
240  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
241  * \param[out] cloud_out the resultant output point cloud dataset
242  * \note Assumes unique indices.
243  * \ingroup common
244  */
245  PCL_EXPORTS void
246  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
247  const std::vector<int> &indices,
248  pcl::PCLPointCloud2 &cloud_out);
249 
250  /** \brief Extract the indices of a given point cloud as a new point cloud
251  * \param[in] cloud_in the input point cloud dataset
252  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
253  * \param[out] cloud_out the resultant output point cloud dataset
254  * \note Assumes unique indices.
255  * \ingroup common
256  */
257  PCL_EXPORTS void
258  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
259  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
260  pcl::PCLPointCloud2 &cloud_out);
261 
262  /** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out
263  * \param[in] cloud_in the input point cloud dataset
264  * \param[out] cloud_out the resultant output point cloud dataset
265  * \ingroup common
266  */
267  PCL_EXPORTS void
268  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
269  pcl::PCLPointCloud2 &cloud_out);
270 
271  /** \brief Check if two given point types are the same or not. */
272  template <typename Point1T, typename Point2T> inline bool
274  {
275  return (typeid (Point1T) == typeid (Point2T));
276  }
277 
278  /** \brief Extract the indices of a given point cloud as a new point cloud
279  * \param[in] cloud_in the input point cloud dataset
280  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
281  * \param[out] cloud_out the resultant output point cloud dataset
282  * \note Assumes unique indices.
283  * \ingroup common
284  */
285  template <typename PointT> void
286  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
287  const std::vector<int> &indices,
288  pcl::PointCloud<PointT> &cloud_out);
289 
290  /** \brief Extract the indices of a given point cloud as a new point cloud
291  * \param[in] cloud_in the input point cloud dataset
292  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
293  * \param[out] cloud_out the resultant output point cloud dataset
294  * \note Assumes unique indices.
295  * \ingroup common
296  */
297  template <typename PointT> void
298  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
299  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
300  pcl::PointCloud<PointT> &cloud_out);
301 
302  /** \brief Extract the indices of a given point cloud as a new point cloud
303  * \param[in] cloud_in the input point cloud dataset
304  * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
305  * \param[out] cloud_out the resultant output point cloud dataset
306  * \note Assumes unique indices.
307  * \ingroup common
308  */
309  template <typename PointT> void
310  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
311  const PointIndices &indices,
312  pcl::PointCloud<PointT> &cloud_out);
313 
314  /** \brief Extract the indices of a given point cloud as a new point cloud
315  * \param[in] cloud_in the input point cloud dataset
316  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
317  * \param[out] cloud_out the resultant output point cloud dataset
318  * \note Assumes unique indices.
319  * \ingroup common
320  */
321  template <typename PointT> void
322  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
323  const std::vector<pcl::PointIndices> &indices,
324  pcl::PointCloud<PointT> &cloud_out);
325 
326  /** \brief Copy all the fields from a given point cloud into a new point cloud
327  * \param[in] cloud_in the input point cloud dataset
328  * \param[out] cloud_out the resultant output point cloud dataset
329  * \ingroup common
330  */
331  template <typename PointInT, typename PointOutT> void
332  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
333  pcl::PointCloud<PointOutT> &cloud_out);
334 
335  /** \brief Extract the indices of a given point cloud as a new point cloud
336  * \param[in] cloud_in the input point cloud dataset
337  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
338  * \param[out] cloud_out the resultant output point cloud dataset
339  * \note Assumes unique indices.
340  * \ingroup common
341  */
342  template <typename PointInT, typename PointOutT> void
343  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
344  const std::vector<int> &indices,
345  pcl::PointCloud<PointOutT> &cloud_out);
346 
347  /** \brief Extract the indices of a given point cloud as a new point cloud
348  * \param[in] cloud_in the input point cloud dataset
349  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
350  * \param[out] cloud_out the resultant output point cloud dataset
351  * \note Assumes unique indices.
352  * \ingroup common
353  */
354  template <typename PointInT, typename PointOutT> void
355  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
356  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
357  pcl::PointCloud<PointOutT> &cloud_out);
358 
359  /** \brief Extract the indices of a given point cloud as a new point cloud
360  * \param[in] cloud_in the input point cloud dataset
361  * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
362  * \param[out] cloud_out the resultant output point cloud dataset
363  * \note Assumes unique indices.
364  * \ingroup common
365  */
366  template <typename PointInT, typename PointOutT> void
367  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
368  const PointIndices &indices,
369  pcl::PointCloud<PointOutT> &cloud_out);
370 
371  /** \brief Extract the indices of a given point cloud as a new point cloud
372  * \param[in] cloud_in the input point cloud dataset
373  * \param[in] indices the vector of indices representing the points to be copied from cloud_in
374  * \param[out] cloud_out the resultant output point cloud dataset
375  * \note Assumes unique indices.
376  * \ingroup common
377  */
378  template <typename PointInT, typename PointOutT> void
379  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
380  const std::vector<pcl::PointIndices> &indices,
381  pcl::PointCloud<PointOutT> &cloud_out);
382 
383  /** \brief Concatenate two datasets representing different fields.
384  *
385  * \note If the input datasets have overlapping fields (i.e., both contain
386  * the same fields), then the data in the second cloud (cloud2_in) will
387  * overwrite the data in the first (cloud1_in).
388  *
389  * \param[in] cloud1_in the first input dataset
390  * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
391  * \param[out] cloud_out the resultant output dataset created by the concatenation of all the fields in the input datasets
392  * \ingroup common
393  */
394  template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
396  const pcl::PointCloud<PointIn2T> &cloud2_in,
397  pcl::PointCloud<PointOutT> &cloud_out);
398 
399  /** \brief Concatenate two datasets representing different fields.
400  *
401  * \note If the input datasets have overlapping fields (i.e., both contain
402  * the same fields), then the data in the second cloud (cloud2_in) will
403  * overwrite the data in the first (cloud1_in).
404  *
405  * \param[in] cloud1_in the first input dataset
406  * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
407  * \param[out] cloud_out the output dataset created by concatenating all the fields in the input datasets
408  * \ingroup common
409  */
410  PCL_EXPORTS bool
411  concatenateFields (const pcl::PCLPointCloud2 &cloud1_in,
412  const pcl::PCLPointCloud2 &cloud2_in,
413  pcl::PCLPointCloud2 &cloud_out);
414 
415  /** \brief Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format
416  * \param[in] in the point cloud message
417  * \param[out] out the resultant Eigen MatrixXf format containing XYZ0 / point
418  * \ingroup common
419  */
420  PCL_EXPORTS bool
421  getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out);
422 
423  /** \brief Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message
424  * \param[in] in the Eigen MatrixXf format containing XYZ0 / point
425  * \param[out] out the resultant point cloud message
426  * \note the method assumes that the PCLPointCloud2 message already has the fields set up properly !
427  * \ingroup common
428  */
429  PCL_EXPORTS bool
430  getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out);
431 
432  namespace io
433  {
434  /** \brief swap bytes order of a char array of length N
435  * \param bytes char array to swap
436  * \ingroup common
437  */
438  template <std::size_t N> void
439  swapByte (char* bytes);
440 
441  /** \brief specialization of swapByte for dimension 1
442  * \param bytes char array to swap
443  */
444  template <> inline void
445  swapByte<1> (char* bytes) { bytes[0] = bytes[0]; }
446 
447 
448  /** \brief specialization of swapByte for dimension 2
449  * \param bytes char array to swap
450  */
451  template <> inline void
452  swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); }
453 
454  /** \brief specialization of swapByte for dimension 4
455  * \param bytes char array to swap
456  */
457  template <> inline void
458  swapByte<4> (char* bytes)
459  {
460  std::swap (bytes[0], bytes[3]);
461  std::swap (bytes[1], bytes[2]);
462  }
463 
464  /** \brief specialization of swapByte for dimension 8
465  * \param bytes char array to swap
466  */
467  template <> inline void
468  swapByte<8> (char* bytes)
469  {
470  std::swap (bytes[0], bytes[7]);
471  std::swap (bytes[1], bytes[6]);
472  std::swap (bytes[2], bytes[5]);
473  std::swap (bytes[3], bytes[4]);
474  }
475 
476  /** \brief swaps byte of an arbitrary type T casting it to char*
477  * \param value the data you want its bytes swapped
478  */
479  template <typename T> void
480  swapByte (T& value)
481  {
482  pcl::io::swapByte<sizeof(T)> (reinterpret_cast<char*> (&value));
483  }
484  }
485 }
486 
487 #include <pcl/common/impl/io.hpp>
488 
489 #endif //#ifndef PCL_COMMON_IO_H_
490