Point Cloud Library (PCL)  1.11.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/common/concatenate.h>
44 #include <pcl/common/copy_point.h>
45 #include <pcl/common/io.h>
46 #include <pcl/point_types.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 
120 template <typename PointInT, typename PointOutT> void
122  pcl::PointCloud<PointOutT> &cloud_out)
123 {
124  // Allocate enough space and copy the basics
125  cloud_out.header = cloud_in.header;
126  cloud_out.width = cloud_in.width;
127  cloud_out.height = cloud_in.height;
128  cloud_out.is_dense = cloud_in.is_dense;
129  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
130  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
131  cloud_out.resize (cloud_in.size ());
132 
133  if (cloud_in.empty ())
134  return;
135 
136  if (isSamePointType<PointInT, PointOutT> ())
137  // Copy the whole memory block
138  memcpy (&cloud_out[0], &cloud_in[0], cloud_in.size () * sizeof (PointInT));
139  else
140  // Iterate over each point
141  for (std::size_t i = 0; i < cloud_in.size (); ++i)
142  copyPoint (cloud_in[i], cloud_out[i]);
143 }
144 
145 
146 template <typename PointT, typename IndicesVectorAllocator> void
149  pcl::PointCloud<PointT> &cloud_out)
150 {
151  // Do we want to copy everything?
152  if (indices.size () == cloud_in.size ())
153  {
154  cloud_out = cloud_in;
155  return;
156  }
157 
158  // Allocate enough space and copy the basics
159  cloud_out.resize (indices.size ());
160  cloud_out.header = cloud_in.header;
161  cloud_out.width = indices.size ();
162  cloud_out.height = 1;
163  cloud_out.is_dense = cloud_in.is_dense;
164  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
165  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
166 
167  // Iterate over each point
168  for (std::size_t i = 0; i < indices.size (); ++i)
169  cloud_out[i] = cloud_in[indices[i]];
170 }
171 
172 
173 template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
176  pcl::PointCloud<PointOutT> &cloud_out)
177 {
178  // Allocate enough space and copy the basics
179  cloud_out.resize (indices.size ());
180  cloud_out.header = cloud_in.header;
181  cloud_out.width = indices.size ();
182  cloud_out.height = 1;
183  cloud_out.is_dense = cloud_in.is_dense;
184  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
185  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
186 
187  // Iterate over each point
188  for (std::size_t i = 0; i < indices.size (); ++i)
189  copyPoint (cloud_in[indices[i]], cloud_out[i]);
190 }
191 
192 
193 template <typename PointT> void
195  const pcl::PointIndices &indices,
196  pcl::PointCloud<PointT> &cloud_out)
197 {
198  // Do we want to copy everything?
199  if (indices.indices.size () == cloud_in.size ())
200  {
201  cloud_out = cloud_in;
202  return;
203  }
204 
205  // Allocate enough space and copy the basics
206  cloud_out.resize (indices.indices.size ());
207  cloud_out.header = cloud_in.header;
208  cloud_out.width = indices.indices.size ();
209  cloud_out.height = 1;
210  cloud_out.is_dense = cloud_in.is_dense;
211  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
212  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
213 
214  // Iterate over each point
215  for (std::size_t i = 0; i < indices.indices.size (); ++i)
216  cloud_out[i] = cloud_in[indices.indices[i]];
217 }
218 
219 
220 template <typename PointInT, typename PointOutT> void
222  const pcl::PointIndices &indices,
223  pcl::PointCloud<PointOutT> &cloud_out)
224 {
225  copyPointCloud (cloud_in, indices.indices, cloud_out);
226 }
227 
228 
229 template <typename PointT> void
231  const std::vector<pcl::PointIndices> &indices,
232  pcl::PointCloud<PointT> &cloud_out)
233 {
234  int nr_p = 0;
235  for (const auto &index : indices)
236  nr_p += index.indices.size ();
237 
238  // Do we want to copy everything? Remember we assume UNIQUE indices
239  if (nr_p == cloud_in.size ())
240  {
241  cloud_out = cloud_in;
242  return;
243  }
244 
245  // Allocate enough space and copy the basics
246  cloud_out.resize (nr_p);
247  cloud_out.header = cloud_in.header;
248  cloud_out.width = nr_p;
249  cloud_out.height = 1;
250  cloud_out.is_dense = cloud_in.is_dense;
251  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
252  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
253 
254  // Iterate over each cluster
255  int cp = 0;
256  for (const auto &cluster_index : indices)
257  {
258  // Iterate over each idx
259  for (const auto &index : cluster_index.indices)
260  {
261  // Iterate over each dimension
262  cloud_out[cp] = cloud_in[index];
263  cp++;
264  }
265  }
266 }
267 
268 
269 template <typename PointInT, typename PointOutT> void
271  const std::vector<pcl::PointIndices> &indices,
272  pcl::PointCloud<PointOutT> &cloud_out)
273 {
274  const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
275  [](const auto& acc, const auto& index) { return index.indices.size() + acc; });
276 
277  // Do we want to copy everything? Remember we assume UNIQUE indices
278  if (nr_p == cloud_in.size ())
279  {
280  copyPointCloud (cloud_in, cloud_out);
281  return;
282  }
283 
284  // Allocate enough space and copy the basics
285  cloud_out.resize (nr_p);
286  cloud_out.header = cloud_in.header;
287  cloud_out.width = nr_p;
288  cloud_out.height = 1;
289  cloud_out.is_dense = cloud_in.is_dense;
290  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
291  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
292 
293  // Iterate over each cluster
294  std::size_t cp = 0;
295  for (const auto &cluster_index : indices)
296  {
297  // Iterate over each idx
298  for (const auto &index : cluster_index.indices)
299  {
300  copyPoint (cloud_in[index], cloud_out[cp]);
301  ++cp;
302  }
303  }
304 }
305 
306 
307 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
309  const pcl::PointCloud<PointIn2T> &cloud2_in,
310  pcl::PointCloud<PointOutT> &cloud_out)
311 {
312  using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
313  using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
314 
315  if (cloud1_in.size () != cloud2_in.size ())
316  {
317  PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
318  return;
319  }
320 
321  // Resize the output dataset
322  cloud_out.resize (cloud1_in.size ());
323  cloud_out.header = cloud1_in.header;
324  cloud_out.width = cloud1_in.width;
325  cloud_out.height = cloud1_in.height;
326  if (!cloud1_in.is_dense || !cloud2_in.is_dense)
327  cloud_out.is_dense = false;
328  else
329  cloud_out.is_dense = true;
330 
331  // Iterate over each point
332  for (std::size_t i = 0; i < cloud_out.size (); ++i)
333  {
334  // Iterate over each dimension
335  pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in[i], cloud_out[i]));
336  pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in[i], cloud_out[i]));
337  }
338 }
339 
340 
341 template <typename PointT> void
343  int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
344 {
345  if (top < 0 || left < 0 || bottom < 0 || right < 0)
346  {
347  std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
348  PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
349  return;
350  }
351 
352  if (top == 0 && left == 0 && bottom == 0 && right == 0)
353  cloud_out = cloud_in;
354  else
355  {
356  // Allocate enough space and copy the basics
357  cloud_out.header = cloud_in.header;
358  cloud_out.width = cloud_in.width + left + right;
359  cloud_out.height = cloud_in.height + top + bottom;
360  if (cloud_out.size () != cloud_out.width * cloud_out.height)
361  cloud_out.resize (cloud_out.width * cloud_out.height);
362  cloud_out.is_dense = cloud_in.is_dense;
363  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
364  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
365 
366  if (border_type == pcl::BORDER_TRANSPARENT)
367  {
368  const PointT* in = &(cloud_in[0]);
369  PointT* out = &(cloud_out[0]);
370  PointT* out_inner = out + cloud_out.width*top + left;
371  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
372  {
373  if (out_inner != in)
374  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
375  }
376  }
377  else
378  {
379  // Copy the data
380  if (border_type != pcl::BORDER_CONSTANT)
381  {
382  try
383  {
384  std::vector<int> padding (cloud_out.width - cloud_in.width);
385  int right = cloud_out.width - cloud_in.width - left;
386  int bottom = cloud_out.height - cloud_in.height - top;
387 
388  for (int i = 0; i < left; i++)
389  padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
390 
391  for (int i = 0; i < right; i++)
392  padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
393 
394  const PointT* in = &(cloud_in[0]);
395  PointT* out = &(cloud_out[0]);
396  PointT* out_inner = out + cloud_out.width*top + left;
397 
398  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
399  {
400  if (out_inner != in)
401  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
402 
403  for (int j = 0; j < left; j++)
404  out_inner[j - left] = in[padding[j]];
405 
406  for (int j = 0; j < right; j++)
407  out_inner[j + cloud_in.width] = in[padding[j + left]];
408  }
409 
410  for (int i = 0; i < top; i++)
411  {
412  int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
413  memcpy (out + i*cloud_out.width,
414  out + (j+top) * cloud_out.width,
415  sizeof (PointT) * cloud_out.width);
416  }
417 
418  for (int i = 0; i < bottom; i++)
419  {
420  int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
421  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
422  out + (j+top)*cloud_out.width,
423  cloud_out.width * sizeof (PointT));
424  }
425  }
427  {
428  PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
429  }
430  }
431  else
432  {
433  int right = cloud_out.width - cloud_in.width - left;
434  int bottom = cloud_out.height - cloud_in.height - top;
435  std::vector<PointT> buff (cloud_out.width, value);
436  PointT* buff_ptr = &(buff[0]);
437  const PointT* in = &(cloud_in[0]);
438  PointT* out = &(cloud_out[0]);
439  PointT* out_inner = out + cloud_out.width*top + left;
440 
441  for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
442  {
443  if (out_inner != in)
444  memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
445 
446  memcpy (out_inner - left, buff_ptr, left * sizeof (PointT));
447  memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT));
448  }
449 
450  for (int i = 0; i < top; i++)
451  {
452  memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT));
453  }
454 
455  for (int i = 0; i < bottom; i++)
456  {
457  memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
458  buff_ptr,
459  cloud_out.width * sizeof (PointT));
460  }
461  }
462  }
463  }
464 }
465 
466 } // namespace pcl
467 
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:308
pcl
Definition: convolution.h:46
point_types.h
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:393
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:439
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
pcl::BORDER_CONSTANT
@ BORDER_CONSTANT
Definition: io.h:224
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:226
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
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:121
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:391
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::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:399
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:401
pcl::InterpolationType
InterpolationType
Definition: io.h:222
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:396
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:455
pcl::PointIndices
Definition: PointIndices.h:11
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:385
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:436
pcl::IndicesAllocator
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
Definition: types.h:126
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:63