Point Cloud Library (PCL)  1.8.1-dev
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 <pcl/exceptions.h>
49 #include <locale>
50 
51 namespace pcl
52 {
53  /** \brief Get the index of a specified field (i.e., dimension/channel)
54  * \param[in] cloud the the point cloud message
55  * \param[in] field_name the string defining the field name
56  * \ingroup common
57  */
58  inline int
59  getFieldIndex (const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
60  {
61  // Get the index we need
62  for (size_t d = 0; d < cloud.fields.size (); ++d)
63  if (cloud.fields[d].name == field_name)
64  return (static_cast<int>(d));
65  return (-1);
66  }
67 
68  /** \brief Get the index of a specified field (i.e., dimension/channel)
69  * \param[in] cloud the the point cloud message
70  * \param[in] field_name the string defining the field name
71  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
72  * \ingroup common
73  */
74  template <typename PointT> inline int
75  getFieldIndex (const pcl::PointCloud<PointT> &cloud, const std::string &field_name,
76  std::vector<pcl::PCLPointField> &fields);
77 
78  /** \brief Get the index of a specified field (i.e., dimension/channel)
79  * \param[in] field_name the string defining the field name
80  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
81  * \ingroup common
82  */
83  template <typename PointT> inline int
84  getFieldIndex (const std::string &field_name,
85  std::vector<pcl::PCLPointField> &fields);
86 
87  /** \brief Get the list of available fields (i.e., dimension/channel)
88  * \param[in] cloud the point cloud message
89  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
90  * \ingroup common
91  */
92  template <typename PointT> inline void
93  getFields (const pcl::PointCloud<PointT> &cloud, std::vector<pcl::PCLPointField> &fields);
94 
95  /** \brief Get the list of available fields (i.e., dimension/channel)
96  * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
97  * \ingroup common
98  */
99  template <typename PointT> inline void
100  getFields (std::vector<pcl::PCLPointField> &fields);
101 
102  /** \brief Get the list of all fields available in a given cloud
103  * \param[in] cloud the the point cloud message
104  * \ingroup common
105  */
106  template <typename PointT> inline std::string
107  getFieldsList (const pcl::PointCloud<PointT> &cloud);
108 
109  /** \brief Get the available point cloud fields as a space separated string
110  * \param[in] cloud a pointer to the PointCloud message
111  * \ingroup common
112  */
113  inline std::string
115  {
116  std::string result;
117  for (size_t i = 0; i < cloud.fields.size () - 1; ++i)
118  result += cloud.fields[i].name + " ";
119  result += cloud.fields[cloud.fields.size () - 1].name;
120  return (result);
121  }
122 
123  /** \brief Obtains the size of a specific field data type in bytes
124  * \param[in] datatype the field data type (see PCLPointField.h)
125  * \ingroup common
126  */
127  inline int
128  getFieldSize (const int datatype)
129  {
130  switch (datatype)
131  {
134  return (1);
135 
138  return (2);
139 
143  return (4);
144 
146  return (8);
147 
148  default:
149  return (0);
150  }
151  }
152 
153  /** \brief Obtain a vector with the sizes of all valid fields (e.g., not "_")
154  * \param[in] fields the input vector containing the fields
155  * \param[out] field_sizes the resultant field sizes in bytes
156  */
157  PCL_EXPORTS void
158  getFieldsSizes (const std::vector<pcl::PCLPointField> &fields,
159  std::vector<int> &field_sizes);
160 
161  /** \brief Obtains the type of the PCLPointField from a specific size and type
162  * \param[in] size the size in bytes of the data field
163  * \param[in] type a char describing the type of the field ('F' = float, 'I' = signed, 'U' = unsigned)
164  * \ingroup common
165  */
166  inline int
167  getFieldType (const int size, char type)
168  {
169  type = std::toupper (type, std::locale::classic ());
170  switch (size)
171  {
172  case 1:
173  if (type == 'I')
174  return (pcl::PCLPointField::INT8);
175  if (type == 'U')
176  return (pcl::PCLPointField::UINT8);
177 
178  case 2:
179  if (type == 'I')
180  return (pcl::PCLPointField::INT16);
181  if (type == 'U')
183 
184  case 4:
185  if (type == 'I')
186  return (pcl::PCLPointField::INT32);
187  if (type == 'U')
189  if (type == 'F')
191 
192  case 8:
194 
195  default:
196  return (-1);
197  }
198  }
199 
200  /** \brief Obtains the type of the PCLPointField from a specific PCLPointField as a char
201  * \param[in] type the PCLPointField field type
202  * \ingroup common
203  */
204  inline char
205  getFieldType (const int type)
206  {
207  switch (type)
208  {
212  return ('I');
213 
217  return ('U');
218 
221  return ('F');
222  default:
223  return ('?');
224  }
225  }
226 
227  typedef enum
228  {
234 
235  /** \brief \return the right index according to the interpolation type.
236  * \note this is adapted from OpenCV
237  * \param p the index of point to interpolate
238  * \param length the top/bottom row or left/right column index
239  * \param type the requested interpolation
240  * \throws pcl::BadArgumentException if type is unknown
241  */
242  PCL_EXPORTS int
243  interpolatePointIndex (int p, int length, InterpolationType type);
244 
245  /** \brief Concatenate two pcl::PCLPointCloud2.
246  * \param[in] cloud1 the first input point cloud dataset
247  * \param[in] cloud2 the second input point cloud dataset
248  * \param[out] cloud_out the resultant output point cloud dataset
249  * \return true if successful, false if failed (e.g., name/number of fields differs)
250  * \ingroup common
251  */
252  PCL_EXPORTS bool
254  const pcl::PCLPointCloud2 &cloud2,
255  pcl::PCLPointCloud2 &cloud_out);
256 
257  /** \brief Extract the indices of a given point cloud as a new point cloud
258  * \param[in] cloud_in the input point cloud dataset
259  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
260  * \param[out] cloud_out the resultant output point cloud dataset
261  * \note Assumes unique indices.
262  * \ingroup common
263  */
264  PCL_EXPORTS void
265  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
266  const std::vector<int> &indices,
267  pcl::PCLPointCloud2 &cloud_out);
268 
269  /** \brief Extract the indices of a given point cloud as a new point cloud
270  * \param[in] cloud_in the input point cloud dataset
271  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
272  * \param[out] cloud_out the resultant output point cloud dataset
273  * \note Assumes unique indices.
274  * \ingroup common
275  */
276  PCL_EXPORTS void
277  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
278  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
279  pcl::PCLPointCloud2 &cloud_out);
280 
281  /** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out
282  * \param[in] cloud_in the input point cloud dataset
283  * \param[out] cloud_out the resultant output point cloud dataset
284  * \ingroup common
285  */
286  PCL_EXPORTS void
287  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
288  pcl::PCLPointCloud2 &cloud_out);
289 
290  /** \brief Check if two given point types are the same or not. */
291  template <typename Point1T, typename Point2T> inline bool
293  {
294  return (typeid (Point1T) == typeid (Point2T));
295  }
296 
297  /** \brief Extract the indices of a given point cloud as a new point cloud
298  * \param[in] cloud_in the input point cloud dataset
299  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
300  * \param[out] cloud_out the resultant output point cloud dataset
301  * \note Assumes unique indices.
302  * \ingroup common
303  */
304  template <typename PointT> void
305  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
306  const std::vector<int> &indices,
307  pcl::PointCloud<PointT> &cloud_out);
308 
309  /** \brief Extract the indices of a given point cloud as a new point cloud
310  * \param[in] cloud_in the input point cloud dataset
311  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
312  * \param[out] cloud_out the resultant output point cloud dataset
313  * \note Assumes unique indices.
314  * \ingroup common
315  */
316  template <typename PointT> void
317  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
318  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
319  pcl::PointCloud<PointT> &cloud_out);
320 
321  /** \brief Extract the indices of a given point cloud as a new point cloud
322  * \param[in] cloud_in the input point cloud dataset
323  * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
324  * \param[out] cloud_out the resultant output point cloud dataset
325  * \note Assumes unique indices.
326  * \ingroup common
327  */
328  template <typename PointT> void
329  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
330  const PointIndices &indices,
331  pcl::PointCloud<PointT> &cloud_out);
332 
333  /** \brief Extract the indices of a given point cloud as a new point cloud
334  * \param[in] cloud_in the input point cloud dataset
335  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
336  * \param[out] cloud_out the resultant output point cloud dataset
337  * \note Assumes unique indices.
338  * \ingroup common
339  */
340  template <typename PointT> void
341  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
342  const std::vector<pcl::PointIndices> &indices,
343  pcl::PointCloud<PointT> &cloud_out);
344 
345  /** \brief Copy all the fields from a given point cloud into a new point cloud
346  * \param[in] cloud_in the input point cloud dataset
347  * \param[out] cloud_out the resultant output point cloud dataset
348  * \ingroup common
349  */
350  template <typename PointInT, typename PointOutT> void
351  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
352  pcl::PointCloud<PointOutT> &cloud_out);
353 
354  /** \brief Extract the indices of a given point cloud as a new point cloud
355  * \param[in] cloud_in the input point cloud dataset
356  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
357  * \param[out] cloud_out the resultant output point cloud dataset
358  * \note Assumes unique indices.
359  * \ingroup common
360  */
361  template <typename PointInT, typename PointOutT> void
362  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
363  const std::vector<int> &indices,
364  pcl::PointCloud<PointOutT> &cloud_out);
365 
366  /** \brief Extract the indices of a given point cloud as a new point cloud
367  * \param[in] cloud_in the input point cloud dataset
368  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
369  * \param[out] cloud_out the resultant output point cloud dataset
370  * \note Assumes unique indices.
371  * \ingroup common
372  */
373  template <typename PointInT, typename PointOutT> void
374  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
375  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
376  pcl::PointCloud<PointOutT> &cloud_out);
377 
378  /** \brief Extract the indices of a given point cloud as a new point cloud
379  * \param[in] cloud_in the input point cloud dataset
380  * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
381  * \param[out] cloud_out the resultant output point cloud dataset
382  * \note Assumes unique indices.
383  * \ingroup common
384  */
385  template <typename PointInT, typename PointOutT> void
386  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
387  const PointIndices &indices,
388  pcl::PointCloud<PointOutT> &cloud_out);
389 
390  /** \brief Extract the indices of a given point cloud as a new point cloud
391  * \param[in] cloud_in the input point cloud dataset
392  * \param[in] indices the vector of indices representing the points to be copied from cloud_in
393  * \param[out] cloud_out the resultant output point cloud dataset
394  * \note Assumes unique indices.
395  * \ingroup common
396  */
397  template <typename PointInT, typename PointOutT> void
398  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
399  const std::vector<pcl::PointIndices> &indices,
400  pcl::PointCloud<PointOutT> &cloud_out);
401 
402  /** \brief Copy a point cloud inside a larger one interpolating borders.
403  * \param[in] cloud_in the input point cloud dataset
404  * \param[out] cloud_out the resultant output point cloud dataset
405  * \param top
406  * \param bottom
407  * \param left
408  * \param right
409  * Position of cloud_in inside cloud_out is given by \a top, \a left, \a bottom \a right.
410  * \param[in] border_type the interpolating method (pcl::BORDER_XXX)
411  * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh
412  * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb
413  * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba
414  * BORDER_WRAP: cdefgh|abcdefgh|abcdefg
415  * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i'
416  * BORDER_TRANSPARENT: mnopqr|abcdefgh|tuvwxyz where m-r and t-z are orignal values of cloud_out
417  * \param value
418  * \throw pcl::BadArgumentException if any of top, bottom, left or right is negative.
419  * \ingroup common
420  */
421  template <typename PointT> void
422  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
423  pcl::PointCloud<PointT> &cloud_out,
424  int top, int bottom, int left, int right,
425  pcl::InterpolationType border_type, const PointT& value);
426 
427  /** \brief Concatenate two datasets representing different fields.
428  *
429  * \note If the input datasets have overlapping fields (i.e., both contain
430  * the same fields), then the data in the second cloud (cloud2_in) will
431  * overwrite the data in the first (cloud1_in).
432  *
433  * \param[in] cloud1_in the first input dataset
434  * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
435  * \param[out] cloud_out the resultant output dataset created by the concatenation of all the fields in the input datasets
436  * \ingroup common
437  */
438  template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
440  const pcl::PointCloud<PointIn2T> &cloud2_in,
441  pcl::PointCloud<PointOutT> &cloud_out);
442 
443  /** \brief Concatenate two datasets representing different fields.
444  *
445  * \note If the input datasets have overlapping fields (i.e., both contain
446  * the same fields), then the data in the second cloud (cloud2_in) will
447  * overwrite the data in the first (cloud1_in).
448  *
449  * \param[in] cloud1_in the first input dataset
450  * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
451  * \param[out] cloud_out the output dataset created by concatenating all the fields in the input datasets
452  * \ingroup common
453  */
454  PCL_EXPORTS bool
455  concatenateFields (const pcl::PCLPointCloud2 &cloud1_in,
456  const pcl::PCLPointCloud2 &cloud2_in,
457  pcl::PCLPointCloud2 &cloud_out);
458 
459  /** \brief Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format
460  * \param[in] in the point cloud message
461  * \param[out] out the resultant Eigen MatrixXf format containing XYZ0 / point
462  * \ingroup common
463  */
464  PCL_EXPORTS bool
465  getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out);
466 
467  /** \brief Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message
468  * \param[in] in the Eigen MatrixXf format containing XYZ0 / point
469  * \param[out] out the resultant point cloud message
470  * \note the method assumes that the PCLPointCloud2 message already has the fields set up properly !
471  * \ingroup common
472  */
473  PCL_EXPORTS bool
474  getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out);
475 
476  namespace io
477  {
478  /** \brief swap bytes order of a char array of length N
479  * \param bytes char array to swap
480  * \ingroup common
481  */
482  template <std::size_t N> void
483  swapByte (char* bytes);
484 
485  /** \brief specialization of swapByte for dimension 1
486  * \param bytes char array to swap
487  */
488  template <> inline void
489  swapByte<1> (char* bytes) { bytes[0] = bytes[0]; }
490 
491 
492  /** \brief specialization of swapByte for dimension 2
493  * \param bytes char array to swap
494  */
495  template <> inline void
496  swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); }
497 
498  /** \brief specialization of swapByte for dimension 4
499  * \param bytes char array to swap
500  */
501  template <> inline void
502  swapByte<4> (char* bytes)
503  {
504  std::swap (bytes[0], bytes[3]);
505  std::swap (bytes[1], bytes[2]);
506  }
507 
508  /** \brief specialization of swapByte for dimension 8
509  * \param bytes char array to swap
510  */
511  template <> inline void
512  swapByte<8> (char* bytes)
513  {
514  std::swap (bytes[0], bytes[7]);
515  std::swap (bytes[1], bytes[6]);
516  std::swap (bytes[2], bytes[5]);
517  std::swap (bytes[3], bytes[4]);
518  }
519 
520  /** \brief swaps byte of an arbitrary type T casting it to char*
521  * \param value the data you want its bytes swapped
522  */
523  template <typename T> void
524  swapByte (T& value)
525  {
526  pcl::io::swapByte<sizeof(T)> (reinterpret_cast<char*> (&value));
527  }
528  }
529 }
530 
531 #include <pcl/common/impl/io.hpp>
532 
533 #endif //#ifndef PCL_COMMON_IO_H_
534 
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:59
std::string getFieldsList(const pcl::PointCloud< PointT > &cloud)
Get the list of all fields available in a given cloud.
Definition: io.hpp:97
void swapByte< 8 >(char *bytes)
specialization of swapByte for dimension 8
Definition: io.h:512
bool isSamePointType()
Check if two given point types are the same or not.
Definition: io.h:292
void swapByte(char *bytes)
swap bytes order of a char array of length N
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
int getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
Definition: io.h:167
PCL_EXPORTS bool getPointCloudAsEigen(const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out)
Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format.
PCL_EXPORTS void getFieldsSizes(const std::vector< pcl::PCLPointField > &fields, std::vector< int > &field_sizes)
Obtain a vector with the sizes of all valid fields (e.g., not "_")
void swapByte< 1 >(char *bytes)
specialization of swapByte for dimension 1
Definition: io.h:489
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition: io.h:128
InterpolationType
Definition: io.h:227
std::vector< ::pcl::PCLPointField > fields
PCL_EXPORTS bool getEigenAsPointCloud(Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out)
Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message.
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
void swapByte< 2 >(char *bytes)
specialization of swapByte for dimension 2
Definition: io.h:496
PCL_EXPORTS bool concatenatePointCloud(const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, pcl::PCLPointCloud2 &cloud_out)
Concatenate two pcl::PCLPointCloud2.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void swapByte< 4 >(char *bytes)
specialization of swapByte for dimension 4
Definition: io.h:502
void concatenateFields(const pcl::PointCloud< PointIn1T > &cloud1_in, const pcl::PointCloud< PointIn2T > &cloud2_in, pcl::PointCloud< PointOutT > &cloud_out)
Concatenate two datasets representing different fields.
Definition: io.hpp:346
void getFields(const pcl::PointCloud< PointT > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
Definition: io.hpp:79