Point Cloud Library (PCL)  1.12.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 #pragma once
42 
43 #include <pcl/conversions.h> // for FieldAdder
44 #include <pcl/common/concatenate.h>
45 #include <pcl/common/copy_point.h>
46 #include <pcl/common/io.h>
47 
48 
49 namespace pcl
50 {
51 
52 template <typename PointT> int
54  const std::string &field_name,
55  std::vector<pcl::PCLPointField> &fields)
56 {
57  return getFieldIndex<PointT>(field_name, fields);
58 }
59 
60 
61 template <typename PointT> int
62 getFieldIndex (const std::string &field_name,
63  std::vector<pcl::PCLPointField> &fields)
64 {
65  fields = getFields<PointT> ();
66  const auto& ref = fields;
67  return pcl::getFieldIndex<PointT> (field_name, ref);
68 }
69 
70 
71 template <typename PointT> int
72 getFieldIndex (const std::string &field_name,
73  const std::vector<pcl::PCLPointField> &fields)
74 {
75  const auto result = std::find_if(fields.begin (), fields.end (),
76  [&field_name](const auto& field) { return field.name == field_name; });
77  if (result == fields.end ())
78  return -1;
79  return std::distance(fields.begin (), result);
80 }
81 
82 
83 template <typename PointT> void
84 getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
85 {
86  fields = getFields<PointT> ();
87 }
88 
89 
90 template <typename PointT> void
91 getFields (std::vector<pcl::PCLPointField> &fields)
92 {
93  fields = getFields<PointT> ();
94 }
95 
96 
97 template <typename PointT> std::vector<pcl::PCLPointField>
99 {
100  std::vector<pcl::PCLPointField> fields;
101  // Get the fields list
102  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
103  return fields;
104 }
105 
106 
107 template <typename PointT> std::string
109 {
110  // Get the fields list
111  const auto fields = getFields<PointT>();
112  std::string result;
113  for (std::size_t i = 0; i < fields.size () - 1; ++i)
114  result += fields[i].name + " ";
115  result += fields[fields.size () - 1].name;
116  return (result);
117 }
118 
119 namespace detail
120 {
121 
122  template <typename PointInT, typename PointOutT> void
124  pcl::PointCloud<PointOutT> &cloud_out)
125  {
126  // Iterate over each point, if the point types of two clouds are different
127  for (std::size_t i = 0; i < cloud_in.size (); ++i)
128  copyPoint (cloud_in[i], cloud_out[i]);
129  }
130 
131 
132  template <typename PointT> void
134  pcl::PointCloud<PointT> &cloud_out)
135  {
136  // Use std::copy directly, if the point types of two clouds are same
137  std::copy (&cloud_in[0], (&cloud_in[0]) + cloud_in.size (), &cloud_out[0]);
138  }
139 
140 } // namespace detail
141 
142 template <typename PointInT, typename PointOutT> void
144  pcl::PointCloud<PointOutT> &cloud_out)
145 {
146  // Allocate enough space and copy the basics
147  cloud_out.header = cloud_in.header;
148  cloud_out.width = cloud_in.width;
149  cloud_out.height = cloud_in.height;
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  cloud_out.resize (cloud_in.size ());
154 
155  if (!cloud_in.empty ())
156  detail::copyPointCloudMemcpy (cloud_in, cloud_out);
157 }
158 
159 
160 template <typename PointT, typename IndicesVectorAllocator> void
163  pcl::PointCloud<PointT> &cloud_out)
164 {
165  // Do we want to copy everything?
166  if (indices.size () == cloud_in.size ())
167  {
168  cloud_out = cloud_in;
169  return;
170  }
171 
172  // Allocate enough space and copy the basics
173  cloud_out.clear ();
174  cloud_out.reserve (indices.size ());
175  cloud_out.header = cloud_in.header;
176  cloud_out.width = indices.size ();
177  cloud_out.height = 1;
178  cloud_out.is_dense = cloud_in.is_dense;
179  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
180  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
181 
182  // Iterate over each point
183  for (const auto& index : indices)
184  cloud_out.transient_push_back (cloud_in[index]);
185 }
186 
187 
188 template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
191  pcl::PointCloud<PointOutT> &cloud_out)
192 {
193  // Allocate enough space and copy the basics
194  cloud_out.resize (indices.size ());
195  cloud_out.header = cloud_in.header;
196  cloud_out.width = indices.size ();
197  cloud_out.height = 1;
198  cloud_out.is_dense = cloud_in.is_dense;
199  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
200  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
201 
202  // Iterate over each point
203  for (std::size_t i = 0; i < indices.size (); ++i)
204  copyPoint (cloud_in[indices[i]], cloud_out[i]);
205 }
206 
207 
208 template <typename PointT> void
210  const pcl::PointIndices &indices,
211  pcl::PointCloud<PointT> &cloud_out)
212 {
213  copyPointCloud (cloud_in, indices.indices, cloud_out);
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  std::size_t 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.size ())
237  {
238  cloud_out = cloud_in;
239  return;
240  }
241 
242  // Allocate enough space and copy the basics
243  cloud_out.clear ();
244  cloud_out.reserve (nr_p);
245  cloud_out.header = cloud_in.header;
246  cloud_out.width = nr_p;
247  cloud_out.height = 1;
248  cloud_out.is_dense = cloud_in.is_dense;
249  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
250  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
251 
252  // Iterate over each cluster
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.transient_push_back (cloud_in[index]);
260  }
261  }
262 }
263 
264 
265 template <typename PointInT, typename PointOutT> void
267  const std::vector<pcl::PointIndices> &indices,
268  pcl::PointCloud<PointOutT> &cloud_out)
269 {
270  const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
271  [](const auto& acc, const auto& index) { return index.indices.size() + acc; });
272 
273  // Do we want to copy everything? Remember we assume UNIQUE indices
274  if (nr_p == cloud_in.size ())
275  {
276  copyPointCloud (cloud_in, cloud_out);
277  return;
278  }
279 
280  // Allocate enough space and copy the basics
281  cloud_out.resize (nr_p);
282  cloud_out.header = cloud_in.header;
283  cloud_out.width = nr_p;
284  cloud_out.height = 1;
285  cloud_out.is_dense = cloud_in.is_dense;
286  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
287  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
288 
289  // Iterate over each cluster
290  std::size_t cp = 0;
291  for (const auto &cluster_index : indices)
292  {
293  // Iterate over each idx
294  for (const auto &index : cluster_index.indices)
295  {
296  copyPoint (cloud_in[index], cloud_out[cp]);
297  ++cp;
298  }
299  }
300 }
301 
302 
303 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
305  const pcl::PointCloud<PointIn2T> &cloud2_in,
306  pcl::PointCloud<PointOutT> &cloud_out)
307 {
308  using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
309  using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
310 
311  if (cloud1_in.size () != cloud2_in.size ())
312  {
313  PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
314  return;
315  }
316 
317  // Resize the output dataset
318  cloud_out.resize (cloud1_in.size ());
319  cloud_out.header = cloud1_in.header;
320  cloud_out.width = cloud1_in.width;
321  cloud_out.height = cloud1_in.height;
322  if (!cloud1_in.is_dense || !cloud2_in.is_dense)
323  cloud_out.is_dense = false;
324  else
325  cloud_out.is_dense = true;
326 
327  // Iterate over each point
328  for (std::size_t i = 0; i < cloud_out.size (); ++i)
329  {
330  // Iterate over each dimension
331  pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in[i], cloud_out[i]));
332  pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in[i], cloud_out[i]));
333  }
334 }
335 
336 
337 template <typename PointT> void
339  int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
340 {
341  if (top < 0 || left < 0 || bottom < 0 || right < 0)
342  {
343  std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
344  PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
345  return;
346  }
347 
348  if (top == 0 && left == 0 && bottom == 0 && right == 0)
349  cloud_out = cloud_in;
350  else
351  {
352  // Allocate enough space and copy the basics
353  cloud_out.header = cloud_in.header;
354  cloud_out.width = cloud_in.width + left + right;
355  cloud_out.height = cloud_in.height + top + bottom;
356  if (cloud_out.size () != cloud_out.width * cloud_out.height)
357  cloud_out.resize (cloud_out.width * cloud_out.height);
358  cloud_out.is_dense = cloud_in.is_dense;
359  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
360  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
361 
362  if (border_type == pcl::BORDER_TRANSPARENT)
363  {
364  const PointT* in = &(cloud_in[0]);
365  PointT* out = &(cloud_out[0]);
366  PointT* out_inner = out + cloud_out.width*top + left;
367  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
368  {
369  if (out_inner != in)
370  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
371  }
372  }
373  else
374  {
375  // Copy the data
376  if (border_type != pcl::BORDER_CONSTANT)
377  {
378  try
379  {
380  std::vector<int> padding (cloud_out.width - cloud_in.width);
381  int right = cloud_out.width - cloud_in.width - left;
382  int bottom = cloud_out.height - cloud_in.height - top;
383 
384  for (int i = 0; i < left; i++)
385  padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
386 
387  for (int i = 0; i < right; i++)
388  padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
389 
390  const PointT* in = &(cloud_in[0]);
391  PointT* out = &(cloud_out[0]);
392  PointT* out_inner = out + cloud_out.width*top + left;
393 
394  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
395  {
396  if (out_inner != in)
397  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
398 
399  for (int j = 0; j < left; j++)
400  out_inner[j - left] = in[padding[j]];
401 
402  for (int j = 0; j < right; j++)
403  out_inner[j + cloud_in.width] = in[padding[j + left]];
404  }
405 
406  for (int i = 0; i < top; i++)
407  {
408  int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
409  memcpy (out + i*cloud_out.width,
410  out + (j+top) * cloud_out.width,
411  sizeof (PointT) * cloud_out.width);
412  }
413 
414  for (int i = 0; i < bottom; i++)
415  {
416  int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
417  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
418  out + (j+top)*cloud_out.width,
419  cloud_out.width * sizeof (PointT));
420  }
421  }
423  {
424  PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
425  }
426  }
427  else
428  {
429  int right = cloud_out.width - cloud_in.width - left;
430  int bottom = cloud_out.height - cloud_in.height - top;
431  std::vector<PointT> buff (cloud_out.width, value);
432  PointT* buff_ptr = &(buff[0]);
433  const PointT* in = &(cloud_in[0]);
434  PointT* out = &(cloud_out[0]);
435  PointT* out_inner = out + cloud_out.width*top + left;
436 
437  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
438  {
439  if (out_inner != in)
440  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
441 
442  memcpy (out_inner - left, buff_ptr, left * sizeof (PointT));
443  memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT));
444  }
445 
446  for (int i = 0; i < top; i++)
447  {
448  memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT));
449  }
450 
451  for (int i = 0; i < bottom; i++)
452  {
453  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
454  buff_ptr,
455  cloud_out.width * sizeof (PointT));
456  }
457  }
458  }
459  }
460 }
461 
462 } // namespace pcl
463 
pcl::concatenateFields
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:304
pcl
Definition: convolution.h:46
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:446
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
pcl::PointCloud::transient_push_back
void transient_push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:675
pcl::BORDER_CONSTANT
@ BORDER_CONSTANT
Definition: io.h:223
pcl::getFields
void getFields(const pcl::PointCloud< PointT > &, std::vector< pcl::PCLPointField > &fields)
Definition: io.hpp:84
pcl::NdConcatenateFunctor
Helper functor structure for concatenate.
Definition: concatenate.h:49
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::BORDER_TRANSPARENT
@ BORDER_TRANSPARENT
Definition: io.h:225
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:678
pcl::copyPointCloud
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:143
pcl::detail::copyPointCloudMemcpy
void copyPointCloudMemcpy(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Definition: io.hpp:123
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::copyPoint
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:137
pcl::BadArgumentException
An exception that is thrown when the arguments number or type is wrong/unhandled.
Definition: exceptions.h:255
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition: point_cloud.h:445
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
pcl::InterpolationType
InterpolationType
Definition: io.h:221
pcl::PointCloud::is_dense
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:403
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
pcl::PointIndices
Definition: PointIndices.h:11
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::IndicesAllocator
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
Definition: types.h:128
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
pcl::interpolatePointIndex
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
pcl::getFieldIndex
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition: io.hpp:53
pcl::getFieldsList
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
Definition: io.hpp:108
pcl::detail::FieldAdder
Definition: conversions.h:60