Point Cloud Library (PCL)  1.9.1-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  return getFieldIndex<PointT>(field_name, fields);
55 }
56 
57 //////////////////////////////////////////////////////////////////////////////////////////////
58 template <typename PointT> int
59 pcl::getFieldIndex (const std::string &field_name,
60  std::vector<pcl::PCLPointField> &fields)
61 {
62  fields = getFields<PointT> ();
63  const auto& ref = fields;
64  return pcl::getFieldIndex<PointT> (field_name, ref);
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointT> int
69 pcl::getFieldIndex (const std::string &field_name,
70  const std::vector<pcl::PCLPointField> &fields)
71 {
72  const auto result = std::find_if(fields.begin (), fields.end (),
73  [&field_name](const auto& field) { return field.name == field_name; });
74  if (result == fields.end ())
75  return -1;
76  return std::distance(fields.begin (), result);
77 }
78 
79 //////////////////////////////////////////////////////////////////////////////////////////////
80 template <typename PointT> void
81 pcl::getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
82 {
83  fields = getFields<PointT> ();
84 }
85 
86 //////////////////////////////////////////////////////////////////////////////////////////////
87 template <typename PointT> void
88 pcl::getFields (std::vector<pcl::PCLPointField> &fields)
89 {
90  fields = getFields<PointT> ();
91 }
92 
93 //////////////////////////////////////////////////////////////////////////////////////////////
94 template <typename PointT> std::vector<pcl::PCLPointField>
96 {
97  std::vector<pcl::PCLPointField> fields;
98  // Get the fields list
99  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
100  return fields;
101 }
102 
103 //////////////////////////////////////////////////////////////////////////////////////////////
104 template <typename PointT> std::string
106 {
107  // Get the fields list
108  const auto fields = getFields<PointT>();
109  std::string result;
110  for (std::size_t i = 0; i < fields.size () - 1; ++i)
111  result += fields[i].name + " ";
112  result += fields[fields.size () - 1].name;
113  return (result);
114 }
115 
116 //////////////////////////////////////////////////////////////////////////////////////////////
117 template <typename PointInT, typename PointOutT> void
119  pcl::PointCloud<PointOutT> &cloud_out)
120 {
121  // Allocate enough space and copy the basics
122  cloud_out.header = cloud_in.header;
123  cloud_out.width = cloud_in.width;
124  cloud_out.height = cloud_in.height;
125  cloud_out.is_dense = cloud_in.is_dense;
126  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
127  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
128  cloud_out.points.resize (cloud_in.points.size ());
129 
130  if (cloud_in.points.empty ())
131  return;
132 
133  if (isSamePointType<PointInT, PointOutT> ())
134  // Copy the whole memory block
135  memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT));
136  else
137  // Iterate over each point
138  for (std::size_t i = 0; i < cloud_in.points.size (); ++i)
139  copyPoint (cloud_in.points[i], cloud_out.points[i]);
140 }
141 
142 //////////////////////////////////////////////////////////////////////////////////////////////
143 template <typename PointT, typename IndicesVectorAllocator> void
145  const std::vector<int, IndicesVectorAllocator> &indices,
146  pcl::PointCloud<PointT> &cloud_out)
147 {
148  // Do we want to copy everything?
149  if (indices.size () == cloud_in.points.size ())
150  {
151  cloud_out = cloud_in;
152  return;
153  }
154 
155  // Allocate enough space and copy the basics
156  cloud_out.points.resize (indices.size ());
157  cloud_out.header = cloud_in.header;
158  cloud_out.width = static_cast<std::uint32_t>(indices.size ());
159  cloud_out.height = 1;
160  cloud_out.is_dense = cloud_in.is_dense;
161  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
162  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
163 
164  // Iterate over each point
165  for (std::size_t i = 0; i < indices.size (); ++i)
166  cloud_out.points[i] = cloud_in.points[indices[i]];
167 }
168 
169 //////////////////////////////////////////////////////////////////////////////////////////////
170 template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
172  const std::vector<int, IndicesVectorAllocator> &indices,
173  pcl::PointCloud<PointOutT> &cloud_out)
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 = std::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 (std::size_t i = 0; i < indices.size (); ++i)
186  copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]);
187 }
188 
189 //////////////////////////////////////////////////////////////////////////////////////////////
190 template <typename PointT> void
192  const pcl::PointIndices &indices,
193  pcl::PointCloud<PointT> &cloud_out)
194 {
195  // Do we want to copy everything?
196  if (indices.indices.size () == cloud_in.points.size ())
197  {
198  cloud_out = cloud_in;
199  return;
200  }
201 
202  // Allocate enough space and copy the basics
203  cloud_out.points.resize (indices.indices.size ());
204  cloud_out.header = cloud_in.header;
205  cloud_out.width = indices.indices.size ();
206  cloud_out.height = 1;
207  cloud_out.is_dense = cloud_in.is_dense;
208  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
209  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
210 
211  // Iterate over each point
212  for (std::size_t i = 0; i < indices.indices.size (); ++i)
213  cloud_out.points[i] = cloud_in.points[indices.indices[i]];
214 }
215 
216 ///////////////////////////////////////////////////////////////////////////////////////////////
217 template <typename PointInT, typename PointOutT> void
219  const pcl::PointIndices &indices,
220  pcl::PointCloud<PointOutT> &cloud_out)
221 {
222  copyPointCloud (cloud_in, indices.indices, cloud_out);
223 }
224 
225 //////////////////////////////////////////////////////////////////////////////////////////////
226 template <typename PointT> void
228  const std::vector<pcl::PointIndices> &indices,
229  pcl::PointCloud<PointT> &cloud_out)
230 {
231  int nr_p = 0;
232  for (const auto &index : indices)
233  nr_p += index.indices.size ();
234 
235  // Do we want to copy everything? Remember we assume UNIQUE indices
236  if (nr_p == 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 (nr_p);
244  cloud_out.header = cloud_in.header;
245  cloud_out.width = nr_p;
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 cluster
252  int cp = 0;
253  for (const auto &cluster_index : indices)
254  {
255  // Iterate over each idx
256  for (const auto &index : cluster_index.indices)
257  {
258  // Iterate over each dimension
259  cloud_out.points[cp] = cloud_in.points[index];
260  cp++;
261  }
262  }
263 }
264 
265 //////////////////////////////////////////////////////////////////////////////////////////////
266 template <typename PointInT, typename PointOutT> void
268  const std::vector<pcl::PointIndices> &indices,
269  pcl::PointCloud<PointOutT> &cloud_out)
270 {
271  const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
272  [](const auto& acc, const auto& index) { return index.indices.size() + acc; });
273 
274  // Do we want to copy everything? Remember we assume UNIQUE indices
275  if (nr_p == cloud_in.points.size ())
276  {
277  copyPointCloud (cloud_in, cloud_out);
278  return;
279  }
280 
281  // Allocate enough space and copy the basics
282  cloud_out.points.resize (nr_p);
283  cloud_out.header = cloud_in.header;
284  cloud_out.width = nr_p;
285  cloud_out.height = 1;
286  cloud_out.is_dense = cloud_in.is_dense;
287  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
288  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
289 
290  // Iterate over each cluster
291  std::size_t cp = 0;
292  for (const auto &cluster_index : indices)
293  {
294  // Iterate over each idx
295  for (const auto &index : cluster_index.indices)
296  {
297  copyPoint (cloud_in.points[index], cloud_out.points[cp]);
298  ++cp;
299  }
300  }
301 }
302 
303 //////////////////////////////////////////////////////////////////////////////////////////////
304 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
306  const pcl::PointCloud<PointIn2T> &cloud2_in,
307  pcl::PointCloud<PointOutT> &cloud_out)
308 {
309  using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
310  using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
311 
312  if (cloud1_in.points.size () != cloud2_in.points.size ())
313  {
314  PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
315  return;
316  }
317 
318  // Resize the output dataset
319  cloud_out.points.resize (cloud1_in.points.size ());
320  cloud_out.header = cloud1_in.header;
321  cloud_out.width = cloud1_in.width;
322  cloud_out.height = cloud1_in.height;
323  if (!cloud1_in.is_dense || !cloud2_in.is_dense)
324  cloud_out.is_dense = false;
325  else
326  cloud_out.is_dense = true;
327 
328  // Iterate over each point
329  for (std::size_t i = 0; i < cloud_out.points.size (); ++i)
330  {
331  // Iterate over each dimension
332  pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in.points[i], cloud_out.points[i]));
333  pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in.points[i], cloud_out.points[i]));
334  }
335 }
336 
337 //////////////////////////////////////////////////////////////////////////////////////////////
338 template <typename PointT> void
340  int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
341 {
342  if (top < 0 || left < 0 || bottom < 0 || right < 0)
343  {
344  std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
345  PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
346  return;
347  }
348 
349  if (top == 0 && left == 0 && bottom == 0 && right == 0)
350  cloud_out = cloud_in;
351  else
352  {
353  // Allocate enough space and copy the basics
354  cloud_out.header = cloud_in.header;
355  cloud_out.width = cloud_in.width + left + right;
356  cloud_out.height = cloud_in.height + top + bottom;
357  if (cloud_out.size () != cloud_out.width * cloud_out.height)
358  cloud_out.resize (cloud_out.width * cloud_out.height);
359  cloud_out.is_dense = cloud_in.is_dense;
360  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
361  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
362 
363  if (border_type == pcl::BORDER_TRANSPARENT)
364  {
365  const PointT* in = &(cloud_in.points[0]);
366  PointT* out = &(cloud_out.points[0]);
367  PointT* out_inner = out + cloud_out.width*top + left;
368  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
369  {
370  if (out_inner != in)
371  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
372  }
373  }
374  else
375  {
376  // Copy the data
377  if (border_type != pcl::BORDER_CONSTANT)
378  {
379  try
380  {
381  std::vector<int> padding (cloud_out.width - cloud_in.width);
382  int right = cloud_out.width - cloud_in.width - left;
383  int bottom = cloud_out.height - cloud_in.height - top;
384 
385  for (int i = 0; i < left; i++)
386  padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
387 
388  for (int i = 0; i < right; i++)
389  padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
390 
391  const PointT* in = &(cloud_in.points[0]);
392  PointT* out = &(cloud_out.points[0]);
393  PointT* out_inner = out + cloud_out.width*top + left;
394 
395  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
396  {
397  if (out_inner != in)
398  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
399 
400  for (int j = 0; j < left; j++)
401  out_inner[j - left] = in[padding[j]];
402 
403  for (int j = 0; j < right; j++)
404  out_inner[j + cloud_in.width] = in[padding[j + left]];
405  }
406 
407  for (int i = 0; i < top; i++)
408  {
409  int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
410  memcpy (out + i*cloud_out.width,
411  out + (j+top) * cloud_out.width,
412  sizeof (PointT) * cloud_out.width);
413  }
414 
415  for (int i = 0; i < bottom; i++)
416  {
417  int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
418  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
419  out + (j+top)*cloud_out.width,
420  cloud_out.width * sizeof (PointT));
421  }
422  }
423  catch (pcl::BadArgumentException &e)
424  {
425  PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
426  }
427  }
428  else
429  {
430  int right = cloud_out.width - cloud_in.width - left;
431  int bottom = cloud_out.height - cloud_in.height - top;
432  std::vector<PointT> buff (cloud_out.width, value);
433  PointT* buff_ptr = &(buff[0]);
434  const PointT* in = &(cloud_in.points[0]);
435  PointT* out = &(cloud_out.points[0]);
436  PointT* out_inner = out + cloud_out.width*top + left;
437 
438  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
439  {
440  if (out_inner != in)
441  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
442 
443  memcpy (out_inner - left, buff_ptr, left * sizeof (PointT));
444  memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT));
445  }
446 
447  for (int i = 0; i < top; i++)
448  {
449  memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT));
450  }
451 
452  for (int i = 0; i < bottom; i++)
453  {
454  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
455  buff_ptr,
456  cloud_out.width * sizeof (PointT));
457  }
458  }
459  }
460  }
461 }
462 
463 #endif // PCL_IO_IMPL_IO_H_
464 
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:61
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:471
std::string getFieldsList(const pcl::PointCloud< PointT > &cloud)
Get the list of all fields available in a given cloud.
Definition: io.hpp:105
An exception that is thrown when the arguments number or type is wrong/unhandled. ...
Definition: exceptions.h:256
std::size_t size() const
Definition: point_cloud.h:464
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:429
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:437
Defines all the PCL implemented PointT point type structures.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:439
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:431
PointCloud represents the base class in PCL for storing collections of 3D points. ...
InterpolationType
Definition: io.h:254
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:423
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:434
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:305
Helper functor structure for concatenate.
Definition: concatenate.h:51
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:81