Point Cloud Library (PCL)  1.9.0-dev
io.hpp
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_IO_IMPL_IO_HPP_
42 #define PCL_IO_IMPL_IO_HPP_
43 
44 #include <pcl/common/concatenate.h>
45 #include <pcl/common/copy_point.h>
46 #include <pcl/point_types.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT> int
51  const std::string &field_name,
52  std::vector<pcl::PCLPointField> &fields)
53 {
54  fields.clear ();
55  // Get the fields list
56  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
57  for (size_t d = 0; d < fields.size (); ++d)
58  if (fields[d].name == field_name)
59  return (static_cast<int>(d));
60  return (-1);
61 }
62 
63 //////////////////////////////////////////////////////////////////////////////////////////////
64 template <typename PointT> int
65 pcl::getFieldIndex (const std::string &field_name,
66  std::vector<pcl::PCLPointField> &fields)
67 {
68  fields.clear ();
69  // Get the fields list
70  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
71  for (size_t d = 0; d < fields.size (); ++d)
72  if (fields[d].name == field_name)
73  return (static_cast<int>(d));
74  return (-1);
75 }
76 
77 //////////////////////////////////////////////////////////////////////////////////////////////
78 template <typename PointT> void
79 pcl::getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
80 {
81  fields.clear ();
82  // Get the fields list
83  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
84 }
85 
86 //////////////////////////////////////////////////////////////////////////////////////////////
87 template <typename PointT> void
88 pcl::getFields (std::vector<pcl::PCLPointField> &fields)
89 {
90  fields.clear ();
91  // Get the fields list
92  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
93 }
94 
95 //////////////////////////////////////////////////////////////////////////////////////////////
96 template <typename PointT> std::string
98 {
99  // Get the fields list
100  std::vector<pcl::PCLPointField> fields;
101  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
102  std::string result;
103  for (size_t i = 0; i < fields.size () - 1; ++i)
104  result += fields[i].name + " ";
105  result += fields[fields.size () - 1].name;
106  return (result);
107 }
108 
109 //////////////////////////////////////////////////////////////////////////////////////////////
110 template <typename PointInT, typename PointOutT> void
112  pcl::PointCloud<PointOutT> &cloud_out)
113 {
114  // Allocate enough space and copy the basics
115  cloud_out.header = cloud_in.header;
116  cloud_out.width = cloud_in.width;
117  cloud_out.height = cloud_in.height;
118  cloud_out.is_dense = cloud_in.is_dense;
119  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
120  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
121  cloud_out.points.resize (cloud_in.points.size ());
122 
123  if (cloud_in.points.size () == 0)
124  return;
125 
126  if (isSamePointType<PointInT, PointOutT> ())
127  // Copy the whole memory block
128  memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT));
129  else
130  // Iterate over each point
131  for (size_t i = 0; i < cloud_in.points.size (); ++i)
132  copyPoint (cloud_in.points[i], cloud_out.points[i]);
133 }
134 
135 //////////////////////////////////////////////////////////////////////////////////////////////
136 template <typename PointT> void
138  const std::vector<int> &indices,
139  pcl::PointCloud<PointT> &cloud_out)
140 {
141  // Do we want to copy everything?
142  if (indices.size () == cloud_in.points.size ())
143  {
144  cloud_out = cloud_in;
145  return;
146  }
147 
148  // Allocate enough space and copy the basics
149  cloud_out.points.resize (indices.size ());
150  cloud_out.header = cloud_in.header;
151  cloud_out.width = static_cast<uint32_t>(indices.size ());
152  cloud_out.height = 1;
153  cloud_out.is_dense = cloud_in.is_dense;
154  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
155  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
156 
157  // Iterate over each point
158  for (size_t i = 0; i < indices.size (); ++i)
159  cloud_out.points[i] = cloud_in.points[indices[i]];
160 }
161 
162 //////////////////////////////////////////////////////////////////////////////////////////////
163 template <typename PointT> void
165  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
166  pcl::PointCloud<PointT> &cloud_out)
167 {
168  // Do we want to copy everything?
169  if (indices.size () == cloud_in.points.size ())
170  {
171  cloud_out = cloud_in;
172  return;
173  }
174 
175  // Allocate enough space and copy the basics
176  cloud_out.points.resize (indices.size ());
177  cloud_out.header = cloud_in.header;
178  cloud_out.width = static_cast<uint32_t> (indices.size ());
179  cloud_out.height = 1;
180  cloud_out.is_dense = cloud_in.is_dense;
181  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
182  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
183 
184  // Iterate over each point
185  for (size_t i = 0; i < indices.size (); ++i)
186  cloud_out.points[i] = cloud_in.points[indices[i]];
187 }
188 
189 //////////////////////////////////////////////////////////////////////////////////////////////
190 template <typename PointInT, typename PointOutT> void
192  const std::vector<int> &indices,
193  pcl::PointCloud<PointOutT> &cloud_out)
194 {
195  // Allocate enough space and copy the basics
196  cloud_out.points.resize (indices.size ());
197  cloud_out.header = cloud_in.header;
198  cloud_out.width = uint32_t (indices.size ());
199  cloud_out.height = 1;
200  cloud_out.is_dense = cloud_in.is_dense;
201  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
202  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
203 
204  // Iterate over each point
205  for (size_t i = 0; i < indices.size (); ++i)
206  copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]);
207 }
208 
209 //////////////////////////////////////////////////////////////////////////////////////////////
210 template <typename PointInT, typename PointOutT> void
212  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
213  pcl::PointCloud<PointOutT> &cloud_out)
214 {
215  // Allocate enough space and copy the basics
216  cloud_out.points.resize (indices.size ());
217  cloud_out.header = cloud_in.header;
218  cloud_out.width = static_cast<uint32_t> (indices.size ());
219  cloud_out.height = 1;
220  cloud_out.is_dense = cloud_in.is_dense;
221  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
222  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
223 
224  // Iterate over each point
225  for (size_t i = 0; i < indices.size (); ++i)
226  copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]);
227 }
228 
229 //////////////////////////////////////////////////////////////////////////////////////////////
230 template <typename PointT> void
232  const pcl::PointIndices &indices,
233  pcl::PointCloud<PointT> &cloud_out)
234 {
235  // Do we want to copy everything?
236  if (indices.indices.size () == cloud_in.points.size ())
237  {
238  cloud_out = cloud_in;
239  return;
240  }
241 
242  // Allocate enough space and copy the basics
243  cloud_out.points.resize (indices.indices.size ());
244  cloud_out.header = cloud_in.header;
245  cloud_out.width = indices.indices.size ();
246  cloud_out.height = 1;
247  cloud_out.is_dense = cloud_in.is_dense;
248  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
249  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
250 
251  // Iterate over each point
252  for (size_t i = 0; i < indices.indices.size (); ++i)
253  cloud_out.points[i] = cloud_in.points[indices.indices[i]];
254 }
255 
256 ///////////////////////////////////////////////////////////////////////////////////////////////
257 template <typename PointInT, typename PointOutT> void
259  const pcl::PointIndices &indices,
260  pcl::PointCloud<PointOutT> &cloud_out)
261 {
262  copyPointCloud (cloud_in, indices.indices, cloud_out);
263 }
264 
265 //////////////////////////////////////////////////////////////////////////////////////////////
266 template <typename PointT> void
268  const std::vector<pcl::PointIndices> &indices,
269  pcl::PointCloud<PointT> &cloud_out)
270 {
271  int nr_p = 0;
272  for (size_t i = 0; i < indices.size (); ++i)
273  nr_p += indices[i].indices.size ();
274 
275  // Do we want to copy everything? Remember we assume UNIQUE indices
276  if (nr_p == cloud_in.points.size ())
277  {
278  cloud_out = cloud_in;
279  return;
280  }
281 
282  // Allocate enough space and copy the basics
283  cloud_out.points.resize (nr_p);
284  cloud_out.header = cloud_in.header;
285  cloud_out.width = nr_p;
286  cloud_out.height = 1;
287  cloud_out.is_dense = cloud_in.is_dense;
288  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
289  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
290 
291  // Iterate over each cluster
292  int cp = 0;
293  for (size_t cc = 0; cc < indices.size (); ++cc)
294  {
295  // Iterate over each idx
296  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
297  {
298  // Iterate over each dimension
299  cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]];
300  cp++;
301  }
302  }
303 }
304 
305 //////////////////////////////////////////////////////////////////////////////////////////////
306 template <typename PointInT, typename PointOutT> void
308  const std::vector<pcl::PointIndices> &indices,
309  pcl::PointCloud<PointOutT> &cloud_out)
310 {
311  int nr_p = 0;
312  for (size_t i = 0; i < indices.size (); ++i)
313  nr_p += indices[i].indices.size ();
314 
315  // Do we want to copy everything? Remember we assume UNIQUE indices
316  if (nr_p == cloud_in.points.size ())
317  {
318  copyPointCloud<PointInT, PointOutT> (cloud_in, cloud_out);
319  return;
320  }
321 
322  // Allocate enough space and copy the basics
323  cloud_out.points.resize (nr_p);
324  cloud_out.header = cloud_in.header;
325  cloud_out.width = nr_p;
326  cloud_out.height = 1;
327  cloud_out.is_dense = cloud_in.is_dense;
328  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
329  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
330 
331  // Iterate over each cluster
332  int cp = 0;
333  for (size_t cc = 0; cc < indices.size (); ++cc)
334  {
335  // Iterate over each idx
336  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
337  {
338  copyPoint (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp]);
339  ++cp;
340  }
341  }
342 }
343 
344 //////////////////////////////////////////////////////////////////////////////////////////////
345 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
347  const pcl::PointCloud<PointIn2T> &cloud2_in,
348  pcl::PointCloud<PointOutT> &cloud_out)
349 {
350  typedef typename pcl::traits::fieldList<PointIn1T>::type FieldList1;
351  typedef typename pcl::traits::fieldList<PointIn2T>::type FieldList2;
352 
353  if (cloud1_in.points.size () != cloud2_in.points.size ())
354  {
355  PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
356  return;
357  }
358 
359  // Resize the output dataset
360  cloud_out.points.resize (cloud1_in.points.size ());
361  cloud_out.header = cloud1_in.header;
362  cloud_out.width = cloud1_in.width;
363  cloud_out.height = cloud1_in.height;
364  if (!cloud1_in.is_dense || !cloud2_in.is_dense)
365  cloud_out.is_dense = false;
366  else
367  cloud_out.is_dense = true;
368 
369  // Iterate over each point
370  for (size_t i = 0; i < cloud_out.points.size (); ++i)
371  {
372  // Iterate over each dimension
373  pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in.points[i], cloud_out.points[i]));
374  pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in.points[i], cloud_out.points[i]));
375  }
376 }
377 
378 //////////////////////////////////////////////////////////////////////////////////////////////
379 template <typename PointT> void
381  int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
382 {
383  if (top < 0 || left < 0 || bottom < 0 || right < 0)
384  {
385  std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
386  PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
387  return;
388  }
389 
390  if (top == 0 && left == 0 && bottom == 0 && right == 0)
391  cloud_out = cloud_in;
392  else
393  {
394  // Allocate enough space and copy the basics
395  cloud_out.header = cloud_in.header;
396  cloud_out.width = cloud_in.width + left + right;
397  cloud_out.height = cloud_in.height + top + bottom;
398  if (cloud_out.size () != cloud_out.width * cloud_out.height)
399  cloud_out.resize (cloud_out.width * cloud_out.height);
400  cloud_out.is_dense = cloud_in.is_dense;
401  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
402  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
403 
404  if (border_type == pcl::BORDER_TRANSPARENT)
405  {
406  const PointT* in = &(cloud_in.points[0]);
407  PointT* out = &(cloud_out.points[0]);
408  PointT* out_inner = out + cloud_out.width*top + left;
409  for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
410  {
411  if (out_inner != in)
412  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
413  }
414  }
415  else
416  {
417  // Copy the data
418  if (border_type != pcl::BORDER_CONSTANT)
419  {
420  try
421  {
422  std::vector<int> padding (cloud_out.width - cloud_in.width);
423  int right = cloud_out.width - cloud_in.width - left;
424  int bottom = cloud_out.height - cloud_in.height - top;
425 
426  for (int i = 0; i < left; i++)
427  padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
428 
429  for (int i = 0; i < right; i++)
430  padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
431 
432  const PointT* in = &(cloud_in.points[0]);
433  PointT* out = &(cloud_out.points[0]);
434  PointT* out_inner = out + cloud_out.width*top + left;
435 
436  for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
437  {
438  if (out_inner != in)
439  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
440 
441  for (int j = 0; j < left; j++)
442  out_inner[j - left] = in[padding[j]];
443 
444  for (int j = 0; j < right; j++)
445  out_inner[j + cloud_in.width] = in[padding[j + left]];
446  }
447 
448  for (int i = 0; i < top; i++)
449  {
450  int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
451  memcpy (out + i*cloud_out.width,
452  out + (j+top) * cloud_out.width,
453  sizeof (PointT) * cloud_out.width);
454  }
455 
456  for (int i = 0; i < bottom; i++)
457  {
458  int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
459  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
460  out + (j+top)*cloud_out.width,
461  cloud_out.width * sizeof (PointT));
462  }
463  }
464  catch (pcl::BadArgumentException &e)
465  {
466  PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
467  }
468  }
469  else
470  {
471  int right = cloud_out.width - cloud_in.width - left;
472  int bottom = cloud_out.height - cloud_in.height - top;
473  std::vector<PointT> buff (cloud_out.width, value);
474  PointT* buff_ptr = &(buff[0]);
475  const PointT* in = &(cloud_in.points[0]);
476  PointT* out = &(cloud_out.points[0]);
477  PointT* out_inner = out + cloud_out.width*top + left;
478 
479  for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
480  {
481  if (out_inner != in)
482  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
483 
484  memcpy (out_inner - left, buff_ptr, left * sizeof (PointT));
485  memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT));
486  }
487 
488  for (int i = 0; i < top; i++)
489  {
490  memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT));
491  }
492 
493  for (int i = 0; i < bottom; i++)
494  {
495  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
496  buff_ptr,
497  cloud_out.width * sizeof (PointT));
498  }
499  }
500  }
501  }
502 }
503 
504 #endif // PCL_IO_IMPL_IO_H_
505 
size_t size() const
Definition: point_cloud.h:448
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::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
std::string getFieldsList(const pcl::PointCloud< PointT > &cloud)
Get the list of all fields available in a given cloud.
Definition: io.hpp:97
An exception that is thrown when the arguments number or type is wrong/unhandled. ...
Definition: exceptions.h:256
std::vector< int > indices
Definition: PointIndices.h:19
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.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
PointCloud represents the base class in PCL for storing collections of 3D points. ...
InterpolationType
Definition: io.h:230
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
A point structure representing Euclidean xyz coordinates, and the RGB color.
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
Helper functor structure for concatenate.
Definition: concatenate.h:64
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:138
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