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  break;
178 
179  case 2:
180  if (type == 'I')
181  return (pcl::PCLPointField::INT16);
182  if (type == 'U')
184  break;
185 
186  case 4:
187  if (type == 'I')
188  return (pcl::PCLPointField::INT32);
189  if (type == 'U')
191  if (type == 'F')
193  break;
194 
195  case 8:
196  if (type == 'F')
198  break;
199  }
200  return (-1);
201  }
202 
203  /** \brief Obtains the type of the PCLPointField from a specific PCLPointField as a char
204  * \param[in] type the PCLPointField field type
205  * \ingroup common
206  */
207  inline char
208  getFieldType (const int type)
209  {
210  switch (type)
211  {
215  return ('I');
216 
220  return ('U');
221 
224  return ('F');
225  default:
226  return ('?');
227  }
228  }
229 
230  typedef enum
231  {
237 
238  /** \brief \return the right index according to the interpolation type.
239  * \note this is adapted from OpenCV
240  * \param p the index of point to interpolate
241  * \param length the top/bottom row or left/right column index
242  * \param type the requested interpolation
243  * \throws pcl::BadArgumentException if type is unknown
244  */
245  PCL_EXPORTS int
246  interpolatePointIndex (int p, int length, InterpolationType type);
247 
248  /** \brief Concatenate two pcl::PCLPointCloud2.
249  * \param[in] cloud1 the first input point cloud dataset
250  * \param[in] cloud2 the second input point cloud dataset
251  * \param[out] cloud_out the resultant output point cloud dataset
252  * \return true if successful, false if failed (e.g., name/number of fields differs)
253  * \ingroup common
254  */
255  PCL_EXPORTS bool
257  const pcl::PCLPointCloud2 &cloud2,
258  pcl::PCLPointCloud2 &cloud_out);
259 
260  /** \brief Extract the indices of a given point cloud as a new point cloud
261  * \param[in] cloud_in the input point cloud dataset
262  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
263  * \param[out] cloud_out the resultant output point cloud dataset
264  * \note Assumes unique indices.
265  * \ingroup common
266  */
267  PCL_EXPORTS void
268  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
269  const std::vector<int> &indices,
270  pcl::PCLPointCloud2 &cloud_out);
271 
272  /** \brief Extract the indices of a given point cloud as a new point cloud
273  * \param[in] cloud_in the input point cloud dataset
274  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
275  * \param[out] cloud_out the resultant output point cloud dataset
276  * \note Assumes unique indices.
277  * \ingroup common
278  */
279  PCL_EXPORTS void
280  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
281  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
282  pcl::PCLPointCloud2 &cloud_out);
283 
284  /** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out
285  * \param[in] cloud_in the input point cloud dataset
286  * \param[out] cloud_out the resultant output point cloud dataset
287  * \ingroup common
288  */
289  PCL_EXPORTS void
290  copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
291  pcl::PCLPointCloud2 &cloud_out);
292 
293  /** \brief Check if two given point types are the same or not. */
294  template <typename Point1T, typename Point2T> inline bool
296  {
297  return (typeid (Point1T) == typeid (Point2T));
298  }
299 
300  /** \brief Extract the indices of a given point cloud as a new point cloud
301  * \param[in] cloud_in the input point cloud dataset
302  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
303  * \param[out] cloud_out the resultant output point cloud dataset
304  * \note Assumes unique indices.
305  * \ingroup common
306  */
307  template <typename PointT> void
308  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
309  const std::vector<int> &indices,
310  pcl::PointCloud<PointT> &cloud_out);
311 
312  /** \brief Extract the indices of a given point cloud as a new point cloud
313  * \param[in] cloud_in the input point cloud dataset
314  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
315  * \param[out] cloud_out the resultant output point cloud dataset
316  * \note Assumes unique indices.
317  * \ingroup common
318  */
319  template <typename PointT> void
320  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
321  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
322  pcl::PointCloud<PointT> &cloud_out);
323 
324  /** \brief Extract the indices of a given point cloud as a new point cloud
325  * \param[in] cloud_in the input point cloud dataset
326  * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
327  * \param[out] cloud_out the resultant output point cloud dataset
328  * \note Assumes unique indices.
329  * \ingroup common
330  */
331  template <typename PointT> void
332  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
333  const PointIndices &indices,
334  pcl::PointCloud<PointT> &cloud_out);
335 
336  /** \brief Extract the indices of a given point cloud as a new point cloud
337  * \param[in] cloud_in the input point cloud dataset
338  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
339  * \param[out] cloud_out the resultant output point cloud dataset
340  * \note Assumes unique indices.
341  * \ingroup common
342  */
343  template <typename PointT> void
344  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
345  const std::vector<pcl::PointIndices> &indices,
346  pcl::PointCloud<PointT> &cloud_out);
347 
348  /** \brief Copy all the fields from a given point cloud into a new point cloud
349  * \param[in] cloud_in the input point cloud dataset
350  * \param[out] cloud_out the resultant output point cloud dataset
351  * \ingroup common
352  */
353  template <typename PointInT, typename PointOutT> void
354  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
355  pcl::PointCloud<PointOutT> &cloud_out);
356 
357  /** \brief Extract the indices of a given point cloud as a new point cloud
358  * \param[in] cloud_in the input point cloud dataset
359  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
360  * \param[out] cloud_out the resultant output point cloud dataset
361  * \note Assumes unique indices.
362  * \ingroup common
363  */
364  template <typename PointInT, typename PointOutT> void
365  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
366  const std::vector<int> &indices,
367  pcl::PointCloud<PointOutT> &cloud_out);
368 
369  /** \brief Extract the indices of a given point cloud as a new point cloud
370  * \param[in] cloud_in the input point cloud dataset
371  * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
372  * \param[out] cloud_out the resultant output point cloud dataset
373  * \note Assumes unique indices.
374  * \ingroup common
375  */
376  template <typename PointInT, typename PointOutT> void
377  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
378  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
379  pcl::PointCloud<PointOutT> &cloud_out);
380 
381  /** \brief Extract the indices of a given point cloud as a new point cloud
382  * \param[in] cloud_in the input point cloud dataset
383  * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
384  * \param[out] cloud_out the resultant output point cloud dataset
385  * \note Assumes unique indices.
386  * \ingroup common
387  */
388  template <typename PointInT, typename PointOutT> void
389  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
390  const PointIndices &indices,
391  pcl::PointCloud<PointOutT> &cloud_out);
392 
393  /** \brief Extract the indices of a given point cloud as a new point cloud
394  * \param[in] cloud_in the input point cloud dataset
395  * \param[in] indices the vector of indices representing the points to be copied from cloud_in
396  * \param[out] cloud_out the resultant output point cloud dataset
397  * \note Assumes unique indices.
398  * \ingroup common
399  */
400  template <typename PointInT, typename PointOutT> void
401  copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
402  const std::vector<pcl::PointIndices> &indices,
403  pcl::PointCloud<PointOutT> &cloud_out);
404 
405  /** \brief Copy a point cloud inside a larger one interpolating borders.
406  * \param[in] cloud_in the input point cloud dataset
407  * \param[out] cloud_out the resultant output point cloud dataset
408  * \param top
409  * \param bottom
410  * \param left
411  * \param right
412  * Position of cloud_in inside cloud_out is given by \a top, \a left, \a bottom \a right.
413  * \param[in] border_type the interpolating method (pcl::BORDER_XXX)
414  * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh
415  * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb
416  * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba
417  * BORDER_WRAP: cdefgh|abcdefgh|abcdefg
418  * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i'
419  * BORDER_TRANSPARENT: mnopqr|abcdefgh|tuvwxyz where m-r and t-z are orignal values of cloud_out
420  * \param value
421  * \throw pcl::BadArgumentException if any of top, bottom, left or right is negative.
422  * \ingroup common
423  */
424  template <typename PointT> void
425  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
426  pcl::PointCloud<PointT> &cloud_out,
427  int top, int bottom, int left, int right,
428  pcl::InterpolationType border_type, const PointT& value);
429 
430  /** \brief Concatenate two datasets representing different fields.
431  *
432  * \note If the input datasets have overlapping fields (i.e., both contain
433  * the same fields), then the data in the second cloud (cloud2_in) will
434  * overwrite the data in the first (cloud1_in).
435  *
436  * \param[in] cloud1_in the first input dataset
437  * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
438  * \param[out] cloud_out the resultant output dataset created by the concatenation of all the fields in the input datasets
439  * \ingroup common
440  */
441  template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
443  const pcl::PointCloud<PointIn2T> &cloud2_in,
444  pcl::PointCloud<PointOutT> &cloud_out);
445 
446  /** \brief Concatenate two datasets representing different fields.
447  *
448  * \note If the input datasets have overlapping fields (i.e., both contain
449  * the same fields), then the data in the second cloud (cloud2_in) will
450  * overwrite the data in the first (cloud1_in).
451  *
452  * \param[in] cloud1_in the first input dataset
453  * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
454  * \param[out] cloud_out the output dataset created by concatenating all the fields in the input datasets
455  * \ingroup common
456  */
457  PCL_EXPORTS bool
458  concatenateFields (const pcl::PCLPointCloud2 &cloud1_in,
459  const pcl::PCLPointCloud2 &cloud2_in,
460  pcl::PCLPointCloud2 &cloud_out);
461 
462  /** \brief Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format
463  * \param[in] in the point cloud message
464  * \param[out] out the resultant Eigen MatrixXf format containing XYZ0 / point
465  * \ingroup common
466  */
467  PCL_EXPORTS bool
468  getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out);
469 
470  /** \brief Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message
471  * \param[in] in the Eigen MatrixXf format containing XYZ0 / point
472  * \param[out] out the resultant point cloud message
473  * \note the method assumes that the PCLPointCloud2 message already has the fields set up properly !
474  * \ingroup common
475  */
476  PCL_EXPORTS bool
477  getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out);
478 
479  namespace io
480  {
481  /** \brief swap bytes order of a char array of length N
482  * \param bytes char array to swap
483  * \ingroup common
484  */
485  template <std::size_t N> void
486  swapByte (char* bytes);
487 
488  /** \brief specialization of swapByte for dimension 1
489  * \param bytes char array to swap
490  */
491  template <> inline void
492  swapByte<1> (char* bytes) { bytes[0] = bytes[0]; }
493 
494 
495  /** \brief specialization of swapByte for dimension 2
496  * \param bytes char array to swap
497  */
498  template <> inline void
499  swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); }
500 
501  /** \brief specialization of swapByte for dimension 4
502  * \param bytes char array to swap
503  */
504  template <> inline void
505  swapByte<4> (char* bytes)
506  {
507  std::swap (bytes[0], bytes[3]);
508  std::swap (bytes[1], bytes[2]);
509  }
510 
511  /** \brief specialization of swapByte for dimension 8
512  * \param bytes char array to swap
513  */
514  template <> inline void
515  swapByte<8> (char* bytes)
516  {
517  std::swap (bytes[0], bytes[7]);
518  std::swap (bytes[1], bytes[6]);
519  std::swap (bytes[2], bytes[5]);
520  std::swap (bytes[3], bytes[4]);
521  }
522 
523  /** \brief swaps byte of an arbitrary type T casting it to char*
524  * \param value the data you want its bytes swapped
525  */
526  template <typename T> void
527  swapByte (T& value)
528  {
529  pcl::io::swapByte<sizeof(T)> (reinterpret_cast<char*> (&value));
530  }
531  }
532 }
533 
534 #include <pcl/common/impl/io.hpp>
535 
536 #endif //#ifndef PCL_COMMON_IO_H_
537 
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:515
bool isSamePointType()
Check if two given point types are the same or not.
Definition: io.h:295
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:492
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition: io.h:128
InterpolationType
Definition: io.h:230
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:499
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:505
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