Point Cloud Library (PCL)  1.7.2
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 (isSamePointType<PointInT, PointOutT> ())
124  // Copy the whole memory block
125  memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT));
126  else
127  // Iterate over each point
128  for (size_t i = 0; i < cloud_in.points.size (); ++i)
129  copyPoint (cloud_in.points[i], cloud_out.points[i]);
130 }
131 
132 //////////////////////////////////////////////////////////////////////////////////////////////
133 template <typename PointT> void
135  const std::vector<int> &indices,
136  pcl::PointCloud<PointT> &cloud_out)
137 {
138  // Do we want to copy everything?
139  if (indices.size () == cloud_in.points.size ())
140  {
141  cloud_out = cloud_in;
142  return;
143  }
144 
145  // Allocate enough space and copy the basics
146  cloud_out.points.resize (indices.size ());
147  cloud_out.header = cloud_in.header;
148  cloud_out.width = static_cast<uint32_t>(indices.size ());
149  cloud_out.height = 1;
150  cloud_out.is_dense = cloud_in.is_dense;
151  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
152  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
153 
154  // Iterate over each point
155  for (size_t i = 0; i < indices.size (); ++i)
156  cloud_out.points[i] = cloud_in.points[indices[i]];
157 }
158 
159 //////////////////////////////////////////////////////////////////////////////////////////////
160 template <typename PointT> void
162  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
163  pcl::PointCloud<PointT> &cloud_out)
164 {
165  // Do we want to copy everything?
166  if (indices.size () == cloud_in.points.size ())
167  {
168  cloud_out = cloud_in;
169  return;
170  }
171 
172  // Allocate enough space and copy the basics
173  cloud_out.points.resize (indices.size ());
174  cloud_out.header = cloud_in.header;
175  cloud_out.width = static_cast<uint32_t> (indices.size ());
176  cloud_out.height = 1;
177  cloud_out.is_dense = cloud_in.is_dense;
178  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
179  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
180 
181  // Iterate over each point
182  for (size_t i = 0; i < indices.size (); ++i)
183  cloud_out.points[i] = cloud_in.points[indices[i]];
184 }
185 
186 //////////////////////////////////////////////////////////////////////////////////////////////
187 template <typename PointInT, typename PointOutT> void
189  const std::vector<int> &indices,
190  pcl::PointCloud<PointOutT> &cloud_out)
191 {
192  // Allocate enough space and copy the basics
193  cloud_out.points.resize (indices.size ());
194  cloud_out.header = cloud_in.header;
195  cloud_out.width = uint32_t (indices.size ());
196  cloud_out.height = 1;
197  cloud_out.is_dense = cloud_in.is_dense;
198  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
199  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
200 
201  // Iterate over each point
202  for (size_t i = 0; i < indices.size (); ++i)
203  copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]);
204 }
205 
206 //////////////////////////////////////////////////////////////////////////////////////////////
207 template <typename PointInT, typename PointOutT> void
209  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
210  pcl::PointCloud<PointOutT> &cloud_out)
211 {
212  // Allocate enough space and copy the basics
213  cloud_out.points.resize (indices.size ());
214  cloud_out.header = cloud_in.header;
215  cloud_out.width = static_cast<uint32_t> (indices.size ());
216  cloud_out.height = 1;
217  cloud_out.is_dense = cloud_in.is_dense;
218  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
219  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
220 
221  // Iterate over each point
222  for (size_t i = 0; i < indices.size (); ++i)
223  copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]);
224 }
225 
226 //////////////////////////////////////////////////////////////////////////////////////////////
227 template <typename PointT> void
229  const pcl::PointIndices &indices,
230  pcl::PointCloud<PointT> &cloud_out)
231 {
232  // Do we want to copy everything?
233  if (indices.indices.size () == cloud_in.points.size ())
234  {
235  cloud_out = cloud_in;
236  return;
237  }
238 
239  // Allocate enough space and copy the basics
240  cloud_out.points.resize (indices.indices.size ());
241  cloud_out.header = cloud_in.header;
242  cloud_out.width = indices.indices.size ();
243  cloud_out.height = 1;
244  cloud_out.is_dense = cloud_in.is_dense;
245  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
246  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
247 
248  // Iterate over each point
249  for (size_t i = 0; i < indices.indices.size (); ++i)
250  cloud_out.points[i] = cloud_in.points[indices.indices[i]];
251 }
252 
253 ///////////////////////////////////////////////////////////////////////////////////////////////
254 template <typename PointInT, typename PointOutT> void
256  const pcl::PointIndices &indices,
257  pcl::PointCloud<PointOutT> &cloud_out)
258 {
259  copyPointCloud (cloud_in, indices.indices, cloud_out);
260 }
261 
262 //////////////////////////////////////////////////////////////////////////////////////////////
263 template <typename PointT> void
265  const std::vector<pcl::PointIndices> &indices,
266  pcl::PointCloud<PointT> &cloud_out)
267 {
268  int nr_p = 0;
269  for (size_t i = 0; i < indices.size (); ++i)
270  nr_p += indices[i].indices.size ();
271 
272  // Do we want to copy everything? Remember we assume UNIQUE indices
273  if (nr_p == cloud_in.points.size ())
274  {
275  cloud_out = cloud_in;
276  return;
277  }
278 
279  // Allocate enough space and copy the basics
280  cloud_out.points.resize (nr_p);
281  cloud_out.header = cloud_in.header;
282  cloud_out.width = nr_p;
283  cloud_out.height = 1;
284  cloud_out.is_dense = cloud_in.is_dense;
285  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
286  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
287 
288  // Iterate over each cluster
289  int cp = 0;
290  for (size_t cc = 0; cc < indices.size (); ++cc)
291  {
292  // Iterate over each idx
293  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
294  {
295  // Iterate over each dimension
296  cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]];
297  cp++;
298  }
299  }
300 }
301 
302 //////////////////////////////////////////////////////////////////////////////////////////////
303 template <typename PointInT, typename PointOutT> void
305  const std::vector<pcl::PointIndices> &indices,
306  pcl::PointCloud<PointOutT> &cloud_out)
307 {
308  int nr_p = 0;
309  for (size_t i = 0; i < indices.size (); ++i)
310  nr_p += indices[i].indices.size ();
311 
312  // Do we want to copy everything? Remember we assume UNIQUE indices
313  if (nr_p == cloud_in.points.size ())
314  {
315  copyPointCloud<PointInT, PointOutT> (cloud_in, cloud_out);
316  return;
317  }
318 
319  // Allocate enough space and copy the basics
320  cloud_out.points.resize (nr_p);
321  cloud_out.header = cloud_in.header;
322  cloud_out.width = nr_p;
323  cloud_out.height = 1;
324  cloud_out.is_dense = cloud_in.is_dense;
325  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
326  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
327 
328  // Iterate over each cluster
329  int cp = 0;
330  for (size_t cc = 0; cc < indices.size (); ++cc)
331  {
332  // Iterate over each idx
333  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
334  {
335  copyPoint (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp]);
336  ++cp;
337  }
338  }
339 }
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////
342 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
344  const pcl::PointCloud<PointIn2T> &cloud2_in,
345  pcl::PointCloud<PointOutT> &cloud_out)
346 {
347  typedef typename pcl::traits::fieldList<PointIn1T>::type FieldList1;
348  typedef typename pcl::traits::fieldList<PointIn2T>::type FieldList2;
349 
350  if (cloud1_in.points.size () != cloud2_in.points.size ())
351  {
352  PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
353  return;
354  }
355 
356  // Resize the output dataset
357  cloud_out.points.resize (cloud1_in.points.size ());
358  cloud_out.header = cloud1_in.header;
359  cloud_out.width = cloud1_in.width;
360  cloud_out.height = cloud1_in.height;
361  if (!cloud1_in.is_dense || !cloud2_in.is_dense)
362  cloud_out.is_dense = false;
363  else
364  cloud_out.is_dense = true;
365 
366  // Iterate over each point
367  for (size_t i = 0; i < cloud_out.points.size (); ++i)
368  {
369  // Iterate over each dimension
370  pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in.points[i], cloud_out.points[i]));
371  pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in.points[i], cloud_out.points[i]));
372  }
373 }
374 
375 //////////////////////////////////////////////////////////////////////////////////////////////
376 template <typename PointT> void
378  int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
379 {
380  if (top < 0 || left < 0 || bottom < 0 || right < 0)
381  {
382  std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
383  PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
384  return;
385  }
386 
387  if (top == 0 && left == 0 && bottom == 0 && right == 0)
388  cloud_out = cloud_in;
389  else
390  {
391  // Allocate enough space and copy the basics
392  cloud_out.header = cloud_in.header;
393  cloud_out.width = cloud_in.width + left + right;
394  cloud_out.height = cloud_in.height + top + bottom;
395  if (cloud_out.size () != cloud_out.width * cloud_out.height)
396  cloud_out.resize (cloud_out.width * cloud_out.height);
397  cloud_out.is_dense = cloud_in.is_dense;
398  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
399  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
400 
401  if (border_type == pcl::BORDER_TRANSPARENT)
402  {
403  const PointT* in = &(cloud_in.points[0]);
404  PointT* out = &(cloud_out.points[0]);
405  PointT* out_inner = out + cloud_out.width*top + left;
406  for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
407  {
408  if (out_inner != in)
409  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
410  }
411  }
412  else
413  {
414  // Copy the data
415  if (border_type != pcl::BORDER_CONSTANT)
416  {
417  try
418  {
419  std::vector<int> padding (cloud_out.width - cloud_in.width);
420  int right = cloud_out.width - cloud_in.width - left;
421  int bottom = cloud_out.height - cloud_in.height - top;
422 
423  for (int i = 0; i < left; i++)
424  padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
425 
426  for (int i = 0; i < right; i++)
427  padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
428 
429  const PointT* in = &(cloud_in.points[0]);
430  PointT* out = &(cloud_out.points[0]);
431  PointT* out_inner = out + cloud_out.width*top + left;
432 
433  for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
434  {
435  if (out_inner != in)
436  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
437 
438  for (int j = 0; j < left; j++)
439  out_inner[j - left] = in[padding[j]];
440 
441  for (int j = 0; j < right; j++)
442  out_inner[j + cloud_in.width] = in[padding[j + left]];
443  }
444 
445  for (int i = 0; i < top; i++)
446  {
447  int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
448  memcpy (out + i*cloud_out.width,
449  out + (j+top) * cloud_out.width,
450  sizeof (PointT) * cloud_out.width);
451  }
452 
453  for (int i = 0; i < bottom; i++)
454  {
455  int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
456  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
457  out + (j+top)*cloud_out.width,
458  cloud_out.width * sizeof (PointT));
459  }
460  }
461  catch (pcl::BadArgumentException &e)
462  {
463  PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
464  }
465  }
466  else
467  {
468  int right = cloud_out.width - cloud_in.width - left;
469  int bottom = cloud_out.height - cloud_in.height - top;
470  std::vector<PointT> buff (cloud_out.width, value);
471  PointT* buff_ptr = &(buff[0]);
472  const PointT* in = &(cloud_in.points[0]);
473  PointT* out = &(cloud_out.points[0]);
474  PointT* out_inner = out + cloud_out.width*top + left;
475 
476  for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
477  {
478  if (out_inner != in)
479  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
480 
481  memcpy (out_inner - left, buff_ptr, left * sizeof (PointT));
482  memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT));
483  }
484 
485  for (int i = 0; i < top; i++)
486  {
487  memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT));
488  }
489 
490  for (int i = 0; i < bottom; i++)
491  {
492  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
493  buff_ptr,
494  cloud_out.width * sizeof (PointT));
495  }
496  }
497  }
498  }
499 }
500 
501 #endif // PCL_IO_IMPL_IO_H_
502 
size_t size() const
Definition: point_cloud.h:440
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 argments number or type is wrong/unhandled.
Definition: exceptions.h:257
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:227
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).
Definition: point_cloud.h:418
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
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:343
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