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