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 namespace pcl
49 {
50 
51 template <typename PointT> int
53  const std::string &field_name,
54  std::vector<pcl::PCLPointField> &fields)
55 {
56  return getFieldIndex<PointT>(field_name, fields);
57 }
58 
59 
60 template <typename PointT> int
61 getFieldIndex (const std::string &field_name,
62  std::vector<pcl::PCLPointField> &fields)
63 {
64  fields = getFields<PointT> ();
65  const auto& ref = fields;
66  return pcl::getFieldIndex<PointT> (field_name, ref);
67 }
68 
69 
70 template <typename PointT> int
71 getFieldIndex (const std::string &field_name,
72  const std::vector<pcl::PCLPointField> &fields)
73 {
74  const auto result = std::find_if(fields.begin (), fields.end (),
75  [&field_name](const auto& field) { return field.name == field_name; });
76  if (result == fields.end ())
77  return -1;
78  return std::distance(fields.begin (), result);
79 }
80 
81 
82 template <typename PointT> void
83 getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
84 {
85  fields = getFields<PointT> ();
86 }
87 
88 
89 template <typename PointT> void
90 getFields (std::vector<pcl::PCLPointField> &fields)
91 {
92  fields = getFields<PointT> ();
93 }
94 
95 
96 template <typename PointT> std::vector<pcl::PCLPointField>
98 {
99  std::vector<pcl::PCLPointField> fields;
100  // Get the fields list
101  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
102  return fields;
103 }
104 
105 
106 template <typename PointT> std::string
108 {
109  // Get the fields list
110  const auto fields = getFields<PointT>();
111  std::string result;
112  for (std::size_t i = 0; i < fields.size () - 1; ++i)
113  result += fields[i].name + " ";
114  result += fields[fields.size () - 1].name;
115  return (result);
116 }
117 
118 namespace detail
119 {
120 
121  template <typename PointInT, typename PointOutT> void
123  pcl::PointCloud<PointOutT> &cloud_out)
124  {
125  // Iterate over each point, if the point types of two clouds are different
126  for (std::size_t i = 0; i < cloud_in.size (); ++i)
127  copyPoint (cloud_in[i], cloud_out[i]);
128  }
129 
130 
131  template <typename PointT> void
133  pcl::PointCloud<PointT> &cloud_out)
134  {
135  // Use std::copy directly, if the point types of two clouds are same
136  std::copy (&cloud_in[0], (&cloud_in[0]) + cloud_in.size (), &cloud_out[0]);
137  }
138 
139 } // namespace detail
140 
141 template <typename PointInT, typename PointOutT> void
143  pcl::PointCloud<PointOutT> &cloud_out)
144 {
145  // Allocate enough space and copy the basics
146  cloud_out.header = cloud_in.header;
147  cloud_out.width = cloud_in.width;
148  cloud_out.height = cloud_in.height;
149  cloud_out.is_dense = cloud_in.is_dense;
150  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
151  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
152  cloud_out.resize (cloud_in.size ());
153 
154  if (!cloud_in.empty ())
155  detail::copyPointCloudMemcpy (cloud_in, cloud_out);
156 }
157 
158 
159 template <typename PointT, typename IndicesVectorAllocator> void
162  pcl::PointCloud<PointT> &cloud_out)
163 {
164  // Do we want to copy everything?
165  if (indices.size () == cloud_in.size ())
166  {
167  cloud_out = cloud_in;
168  return;
169  }
170 
171  // Allocate enough space and copy the basics
172  cloud_out.clear ();
173  cloud_out.reserve (indices.size ());
174  cloud_out.header = cloud_in.header;
175  cloud_out.width = 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 (const auto& index : indices)
183  cloud_out.transient_push_back (cloud_in[index]);
184 }
185 
186 
187 template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
190  pcl::PointCloud<PointOutT> &cloud_out)
191 {
192  // Allocate enough space and copy the basics
193  cloud_out.resize (indices.size ());
194  cloud_out.header = cloud_in.header;
195  cloud_out.width = 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 (std::size_t i = 0; i < indices.size (); ++i)
203  copyPoint (cloud_in[indices[i]], cloud_out[i]);
204 }
205 
206 
207 template <typename PointT> void
209  const pcl::PointIndices &indices,
210  pcl::PointCloud<PointT> &cloud_out)
211 {
212  copyPointCloud (cloud_in, indices.indices, cloud_out);
213 }
214 
215 
216 template <typename PointInT, typename PointOutT> void
218  const pcl::PointIndices &indices,
219  pcl::PointCloud<PointOutT> &cloud_out)
220 {
221  copyPointCloud (cloud_in, indices.indices, cloud_out);
222 }
223 
224 
225 template <typename PointT> void
227  const std::vector<pcl::PointIndices> &indices,
228  pcl::PointCloud<PointT> &cloud_out)
229 {
230  std::size_t nr_p = 0;
231  for (const auto &index : indices)
232  nr_p += index.indices.size ();
233 
234  // Do we want to copy everything? Remember we assume UNIQUE indices
235  if (nr_p == cloud_in.size ())
236  {
237  cloud_out = cloud_in;
238  return;
239  }
240 
241  // Allocate enough space and copy the basics
242  cloud_out.clear ();
243  cloud_out.reserve (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  for (const auto &cluster_index : indices)
253  {
254  // Iterate over each idx
255  for (const auto &index : cluster_index.indices)
256  {
257  // Iterate over each dimension
258  cloud_out.transient_push_back (cloud_in[index]);
259  }
260  }
261 }
262 
263 
264 template <typename PointInT, typename PointOutT> void
266  const std::vector<pcl::PointIndices> &indices,
267  pcl::PointCloud<PointOutT> &cloud_out)
268 {
269  const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
270  [](const auto& acc, const auto& index) { return index.indices.size() + acc; });
271 
272  // Do we want to copy everything? Remember we assume UNIQUE indices
273  if (nr_p == cloud_in.size ())
274  {
275  copyPointCloud (cloud_in, cloud_out);
276  return;
277  }
278 
279  // Allocate enough space and copy the basics
280  cloud_out.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  std::size_t cp = 0;
290  for (const auto &cluster_index : indices)
291  {
292  // Iterate over each idx
293  for (const auto &index : cluster_index.indices)
294  {
295  copyPoint (cloud_in[index], cloud_out[cp]);
296  ++cp;
297  }
298  }
299 }
300 
301 
302 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
304  const pcl::PointCloud<PointIn2T> &cloud2_in,
305  pcl::PointCloud<PointOutT> &cloud_out)
306 {
307  using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
308  using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
309 
310  if (cloud1_in.size () != cloud2_in.size ())
311  {
312  PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
313  return;
314  }
315 
316  // Resize the output dataset
317  cloud_out.resize (cloud1_in.size ());
318  cloud_out.header = cloud1_in.header;
319  cloud_out.width = cloud1_in.width;
320  cloud_out.height = cloud1_in.height;
321  if (!cloud1_in.is_dense || !cloud2_in.is_dense)
322  cloud_out.is_dense = false;
323  else
324  cloud_out.is_dense = true;
325 
326  // Iterate over each point
327  for (std::size_t i = 0; i < cloud_out.size (); ++i)
328  {
329  // Iterate over each dimension
330  pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in[i], cloud_out[i]));
331  pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in[i], cloud_out[i]));
332  }
333 }
334 
335 
336 template <typename PointT> void
338  int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
339 {
340  if (top < 0 || left < 0 || bottom < 0 || right < 0)
341  {
342  std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
343  PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
344  return;
345  }
346 
347  if (top == 0 && left == 0 && bottom == 0 && right == 0)
348  cloud_out = cloud_in;
349  else
350  {
351  // Allocate enough space and copy the basics
352  cloud_out.header = cloud_in.header;
353  cloud_out.width = cloud_in.width + left + right;
354  cloud_out.height = cloud_in.height + top + bottom;
355  if (cloud_out.size () != cloud_out.width * cloud_out.height)
356  cloud_out.resize (cloud_out.width * cloud_out.height);
357  cloud_out.is_dense = cloud_in.is_dense;
358  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
359  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
360 
361  if (border_type == pcl::BORDER_TRANSPARENT)
362  {
363  const PointT* in = &(cloud_in[0]);
364  PointT* out = &(cloud_out[0]);
365  PointT* out_inner = out + cloud_out.width*top + left;
366  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
367  {
368  if (out_inner != in) {
369  std::copy(in, in + cloud_in.width, out_inner);
370  }
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  std::copy(in, in + cloud_in.width, out_inner);
398  }
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  std::copy(out + (j+top) * cloud_out.width, out + (j+top) * cloud_out.width + cloud_out.width,
411  out + i*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  std::copy(out + (j+top)*cloud_out.width, out + (j+top)*cloud_out.width + cloud_out.width,
418  out + (i + cloud_in.height + top)*cloud_out.width);
419  }
420  }
422  {
423  PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
424  }
425  }
426  else
427  {
428  int right = cloud_out.width - cloud_in.width - left;
429  int bottom = cloud_out.height - cloud_in.height - top;
430  std::vector<PointT> buff (cloud_out.width, value);
431  PointT* buff_ptr = &(buff[0]);
432  const PointT* in = &(cloud_in[0]);
433  PointT* out = &(cloud_out[0]);
434  PointT* out_inner = out + cloud_out.width*top + left;
435 
436  for (std::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  std::copy(in, in + cloud_in.width, out_inner);
440  }
441 
442  std::copy(buff_ptr, buff_ptr + left, out_inner - left);
443  std::copy(buff_ptr, buff_ptr + right, out_inner + cloud_in.width);
444  }
445 
446  for (int i = 0; i < top; i++)
447  {
448  std::copy(buff_ptr, buff_ptr + cloud_out.width, out + i*cloud_out.width);
449  }
450 
451  for (int i = 0; i < bottom; i++)
452  {
453  std::copy(buff_ptr, buff_ptr + cloud_out.width,
454  out + (i + cloud_in.height + top)*cloud_out.width);
455  }
456  }
457  }
458  }
459 }
460 
461 } // namespace pcl
462 
An exception that is thrown when the arguments number or type is wrong/unhandled.
Definition: exceptions.h:256
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
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
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
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
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
std::size_t size() const
Definition: point_cloud.h:443
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
void reserve(std::size_t n)
Definition: point_cloud.h:445
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
Definition: io.hpp:107
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
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:303
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:142
void copyPointCloudMemcpy(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Definition: io.hpp:122
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition: io.hpp:52
InterpolationType
Definition: io.h:222
@ BORDER_TRANSPARENT
Definition: io.h:225
@ BORDER_CONSTANT
Definition: io.h:223
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
Definition: types.h:128
void getFields(const pcl::PointCloud< PointT > &, std::vector< pcl::PCLPointField > &fields)
Definition: io.hpp:83
Helper functor structure for concatenate.
Definition: concatenate.h:50
A point structure representing Euclidean xyz coordinates, and the RGB color.