Point Cloud Library (PCL)  1.7.1
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/point_types.h>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT> int
50  const std::string &field_name,
51  std::vector<pcl::PCLPointField> &fields)
52 {
53  fields.clear ();
54  // Get the fields list
55  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
56  for (size_t d = 0; d < fields.size (); ++d)
57  if (fields[d].name == field_name)
58  return (static_cast<int>(d));
59  return (-1);
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointT> int
64 pcl::getFieldIndex (const std::string &field_name,
65  std::vector<pcl::PCLPointField> &fields)
66 {
67  fields.clear ();
68  // Get the fields list
69  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
70  for (size_t d = 0; d < fields.size (); ++d)
71  if (fields[d].name == field_name)
72  return (static_cast<int>(d));
73  return (-1);
74 }
75 
76 //////////////////////////////////////////////////////////////////////////////////////////////
77 template <typename PointT> void
78 pcl::getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
79 {
80  fields.clear ();
81  // Get the fields list
82  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointT> void
87 pcl::getFields (std::vector<pcl::PCLPointField> &fields)
88 {
89  fields.clear ();
90  // Get the fields list
91  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointT> std::string
97 {
98  // Get the fields list
99  std::vector<pcl::PCLPointField> fields;
100  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
101  std::string result;
102  for (size_t i = 0; i < fields.size () - 1; ++i)
103  result += fields[i].name + " ";
104  result += fields[fields.size () - 1].name;
105  return (result);
106 }
107 
108 //////////////////////////////////////////////////////////////////////////////////////////////
109 template <typename PointInT, typename PointOutT> void
111  pcl::PointCloud<PointOutT> &cloud_out)
112 {
113  // Copy all the data fields from the input cloud to the output one
114  typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT;
115  typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT;
116  typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
117 
118  cloud_out.header = cloud_in.header;
119  cloud_out.width = cloud_in.width;
120  cloud_out.height = cloud_in.height;
121  cloud_out.is_dense = cloud_in.is_dense;
122  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
123  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
124  cloud_out.points.resize (cloud_in.points.size ());
125 
126  // If the point types are the same, don't copy one by one
127  if (isSamePointType<PointInT, PointOutT> ())
128  {
129  memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT));
130  return;
131  }
132 
133  std::vector<pcl::PCLPointField> fields_in, fields_out;
134  pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in));
135  pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out));
136 
137  // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and
138  // fix it manually
139  int rgb_idx_in = -1, rgb_idx_out = -1;
140  for (size_t i = 0; i < fields_in.size (); ++i)
141  if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba")
142  {
143  rgb_idx_in = int (i);
144  break;
145  }
146  for (size_t i = 0; i < fields_out.size (); ++i)
147  if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba")
148  {
149  rgb_idx_out = int (i);
150  break;
151  }
152 
153  // We have one of the two cases: RGB vs RGBA or RGBA vs RGB
154  if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
155  fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
156  {
157  size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype),
158  field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype);
159 
160  if (field_size_in == field_size_out)
161  {
162  for (size_t i = 0; i < cloud_in.points.size (); ++i)
163  {
164  // Copy the rest
165  pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[i], cloud_out.points[i]));
166  // Copy RGB<->RGBA
167  memcpy (reinterpret_cast<char*> (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in);
168  }
169  return;
170  }
171  }
172 
173  // Iterate over each point if no RGB/RGBA or if their size is different
174  for (size_t i = 0; i < cloud_in.points.size (); ++i)
175  // Iterate over each dimension
176  pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[i], cloud_out.points[i]));
177 }
178 
179 //////////////////////////////////////////////////////////////////////////////////////////////
180 template <typename PointT> void
182  const std::vector<int> &indices,
183  pcl::PointCloud<PointT> &cloud_out)
184 {
185  // Do we want to copy everything?
186  if (indices.size () == cloud_in.points.size ())
187  {
188  cloud_out = cloud_in;
189  return;
190  }
191 
192  // Allocate enough space and copy the basics
193  cloud_out.points.resize (indices.size ());
194  cloud_out.header = cloud_in.header;
195  cloud_out.width = static_cast<uint32_t>(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 (size_t i = 0; i < indices.size (); ++i)
203  cloud_out.points[i] = cloud_in.points[indices[i]];
204 }
205 
206 //////////////////////////////////////////////////////////////////////////////////////////////
207 template <typename PointT> void
209  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
210  pcl::PointCloud<PointT> &cloud_out)
211 {
212  // Do we want to copy everything?
213  if (indices.size () == cloud_in.points.size ())
214  {
215  cloud_out = cloud_in;
216  return;
217  }
218 
219  // Allocate enough space and copy the basics
220  cloud_out.points.resize (indices.size ());
221  cloud_out.header = cloud_in.header;
222  cloud_out.width = static_cast<uint32_t> (indices.size ());
223  cloud_out.height = 1;
224  cloud_out.is_dense = cloud_in.is_dense;
225  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
226  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
227 
228  // Iterate over each point
229  for (size_t i = 0; i < indices.size (); ++i)
230  cloud_out.points[i] = cloud_in.points[indices[i]];
231 }
232 
233 //////////////////////////////////////////////////////////////////////////////////////////////
234 template <typename PointInT, typename PointOutT> void
236  const std::vector<int> &indices,
237  pcl::PointCloud<PointOutT> &cloud_out)
238 {
239  // Allocate enough space and copy the basics
240  cloud_out.points.resize (indices.size ());
241  cloud_out.header = cloud_in.header;
242  cloud_out.width = uint32_t (indices.size ());
243  cloud_out.height = 1;
244  cloud_out.is_dense = cloud_in.is_dense;
245  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
246  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
247 
248  // Copy all the data fields from the input cloud to the output one
249  typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT;
250  typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT;
251  typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
252 
253  // If the point types are the same, don't copy one by one
254  if (isSamePointType<PointInT, PointOutT> ())
255  {
256  // Iterate over each point
257  for (size_t i = 0; i < indices.size (); ++i)
258  memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT));
259  return;
260  }
261 
262  std::vector<pcl::PCLPointField> fields_in, fields_out;
263  pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in));
264  pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out));
265 
266  // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and
267  // fix it manually
268  int rgb_idx_in = -1, rgb_idx_out = -1;
269  for (size_t i = 0; i < fields_in.size (); ++i)
270  if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba")
271  {
272  rgb_idx_in = int (i);
273  break;
274  }
275  for (size_t i = 0; int (i) < fields_out.size (); ++i)
276  if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba")
277  {
278  rgb_idx_out = int (i);
279  break;
280  }
281 
282  // We have one of the two cases: RGB vs RGBA or RGBA vs RGB
283  if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
284  fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
285  {
286  size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype),
287  field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype);
288 
289  if (field_size_in == field_size_out)
290  {
291  for (size_t i = 0; i < indices.size (); ++i)
292  {
293  // Copy the rest
294  pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i]));
295  // Copy RGB<->RGBA
296  memcpy (reinterpret_cast<char*> (&cloud_out.points[indices[i]]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in);
297  }
298  return;
299  }
300  }
301 
302  // Iterate over each point if no RGB/RGBA or if their size is different
303  for (size_t i = 0; i < indices.size (); ++i)
304  // Iterate over each dimension
305  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i]));
306 }
307 
308 //////////////////////////////////////////////////////////////////////////////////////////////
309 template <typename PointInT, typename PointOutT> void
311  const std::vector<int, Eigen::aligned_allocator<int> > &indices,
312  pcl::PointCloud<PointOutT> &cloud_out)
313 {
314  // Allocate enough space and copy the basics
315  cloud_out.points.resize (indices.size ());
316  cloud_out.header = cloud_in.header;
317  cloud_out.width = static_cast<uint32_t> (indices.size ());
318  cloud_out.height = 1;
319  cloud_out.is_dense = cloud_in.is_dense;
320  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
321  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
322 
323  // Copy all the data fields from the input cloud to the output one
324  typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT;
325  typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT;
326  typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
327 
328  // If the point types are the same, don't copy one by one
329  if (isSamePointType<PointInT, PointOutT> ())
330  {
331  // Iterate over each point
332  for (size_t i = 0; i < indices.size (); ++i)
333  memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT));
334  return;
335  }
336 
337  std::vector<pcl::PCLPointField> fields_in, fields_out;
338  pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in));
339  pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out));
340 
341  // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and
342  // fix it manually
343  int rgb_idx_in = -1, rgb_idx_out = -1;
344  for (size_t i = 0; i < fields_in.size (); ++i)
345  if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba")
346  {
347  rgb_idx_in = int (i);
348  break;
349  }
350  for (size_t i = 0; i < fields_out.size (); ++i)
351  if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba")
352  {
353  rgb_idx_out = int (i);
354  break;
355  }
356 
357  // We have one of the two cases: RGB vs RGBA or RGBA vs RGB
358  if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
359  fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
360  {
361  size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype),
362  field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype);
363 
364  if (field_size_in == field_size_out)
365  {
366  for (size_t i = 0; i < indices.size (); ++i)
367  {
368  // Copy the rest
369  pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i]));
370  // Copy RGB<->RGBA
371  memcpy (reinterpret_cast<char*> (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in);
372  }
373  return;
374  }
375  }
376 
377  // Iterate over each point if no RGB/RGBA or if their size is different
378  for (size_t i = 0; i < indices.size (); ++i)
379  // Iterate over each dimension
380  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i]));
381 }
382 
383 //////////////////////////////////////////////////////////////////////////////////////////////
384 template <typename PointT> void
386  const pcl::PointIndices &indices,
387  pcl::PointCloud<PointT> &cloud_out)
388 {
389  // Do we want to copy everything?
390  if (indices.indices.size () == cloud_in.points.size ())
391  {
392  cloud_out = cloud_in;
393  return;
394  }
395 
396  // Allocate enough space and copy the basics
397  cloud_out.points.resize (indices.indices.size ());
398  cloud_out.header = cloud_in.header;
399  cloud_out.width = indices.indices.size ();
400  cloud_out.height = 1;
401  cloud_out.is_dense = cloud_in.is_dense;
402  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
403  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
404 
405  // Iterate over each point
406  for (size_t i = 0; i < indices.indices.size (); ++i)
407  cloud_out.points[i] = cloud_in.points[indices.indices[i]];
408 }
409 
410 ///////////////////////////////////////////////////////////////////////////////////////////////
411 template <typename PointInT, typename PointOutT> void
413  const pcl::PointIndices &indices,
414  pcl::PointCloud<PointOutT> &cloud_out)
415 {
416  // Allocate enough space and copy the basics
417  cloud_out.points.resize (indices.indices.size ());
418  cloud_out.header = cloud_in.header;
419  cloud_out.width = indices.indices.size ();
420  cloud_out.height = 1;
421  cloud_out.is_dense = cloud_in.is_dense;
422  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
423  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
424 
425  // Copy all the data fields from the input cloud to the output one
426  typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT;
427  typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT;
428  typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
429 
430  // If the point types are the same, don't copy one by one
431  if (isSamePointType<PointInT, PointOutT> ())
432  {
433  // Iterate over each point
434  for (size_t i = 0; i < indices.indices.size (); ++i)
435  memcpy (&cloud_out.points[i], &cloud_in.points[indices.indices[i]], sizeof (PointInT));
436  return;
437  }
438 
439  std::vector<pcl::PCLPointField> fields_in, fields_out;
440  pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in));
441  pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out));
442 
443  // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and
444  // fix it manually
445  int rgb_idx_in = -1, rgb_idx_out = -1;
446  for (size_t i = 0; i < fields_in.size (); ++i)
447  if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba")
448  {
449  rgb_idx_in = int (i);
450  break;
451  }
452  for (size_t i = 0; i < fields_out.size (); ++i)
453  if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba")
454  {
455  rgb_idx_out = int (i);
456  break;
457  }
458 
459  // We have one of the two cases: RGB vs RGBA or RGBA vs RGB
460  if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
461  fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
462  {
463  size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype),
464  field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype);
465 
466  if (field_size_in == field_size_out)
467  {
468  for (size_t i = 0; i < indices.indices.size (); ++i)
469  {
470  // Copy the rest
471  pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices.indices[i]], cloud_out.points[i]));
472  // Copy RGB<->RGBA
473  memcpy (reinterpret_cast<char*> (&cloud_out.points[indices.indices[i]]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in);
474  }
475  return;
476  }
477  }
478 
479  // Iterate over each point if no RGB/RGBA or if their size is different
480  for (size_t i = 0; i < indices.indices.size (); ++i)
481  // Iterate over each dimension
482  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices.indices[i]], cloud_out.points[i]));
483 }
484 
485 //////////////////////////////////////////////////////////////////////////////////////////////
486 template <typename PointT> void
488  const std::vector<pcl::PointIndices> &indices,
489  pcl::PointCloud<PointT> &cloud_out)
490 {
491  int nr_p = 0;
492  for (size_t i = 0; i < indices.size (); ++i)
493  nr_p += indices[i].indices.size ();
494 
495  // Do we want to copy everything? Remember we assume UNIQUE indices
496  if (nr_p == cloud_in.points.size ())
497  {
498  cloud_out = cloud_in;
499  return;
500  }
501 
502  // Allocate enough space and copy the basics
503  cloud_out.points.resize (nr_p);
504  cloud_out.header = cloud_in.header;
505  cloud_out.width = nr_p;
506  cloud_out.height = 1;
507  cloud_out.is_dense = cloud_in.is_dense;
508  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
509  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
510 
511  // Iterate over each cluster
512  int cp = 0;
513  for (size_t cc = 0; cc < indices.size (); ++cc)
514  {
515  // Iterate over each idx
516  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
517  {
518  // Iterate over each dimension
519  cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]];
520  cp++;
521  }
522  }
523 }
524 
525 //////////////////////////////////////////////////////////////////////////////////////////////
526 template <typename PointInT, typename PointOutT> void
528  const std::vector<pcl::PointIndices> &indices,
529  pcl::PointCloud<PointOutT> &cloud_out)
530 {
531  int nr_p = 0;
532  for (size_t i = 0; i < indices.size (); ++i)
533  nr_p += indices[i].indices.size ();
534 
535  // Do we want to copy everything? Remember we assume UNIQUE indices
536  if (nr_p == cloud_in.points.size ())
537  {
538  copyPointCloud<PointInT, PointOutT> (cloud_in, cloud_out);
539  return;
540  }
541 
542  // Allocate enough space and copy the basics
543  cloud_out.points.resize (nr_p);
544  cloud_out.header = cloud_in.header;
545  cloud_out.width = nr_p;
546  cloud_out.height = 1;
547  cloud_out.is_dense = cloud_in.is_dense;
548  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
549  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
550 
551  // Copy all the data fields from the input cloud to the output one
552  typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT;
553  typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT;
554  typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
555 
556  // If the point types are the same, don't copy one by one
557  if (isSamePointType<PointInT, PointOutT> ())
558  {
559  // Iterate over each cluster
560  int cp = 0;
561  for (size_t cc = 0; cc < indices.size (); ++cc)
562  {
563  // Iterate over each idx
564  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
565  {
566  cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]];
567  ++cp;
568  }
569  }
570  return;
571  }
572 
573  std::vector<pcl::PCLPointField> fields_in, fields_out;
574  pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in));
575  pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out));
576 
577  // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and
578  // fix it manually
579  int rgb_idx_in = -1, rgb_idx_out = -1;
580  for (size_t i = 0; i < fields_in.size (); ++i)
581  if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba")
582  {
583  rgb_idx_in = int (i);
584  break;
585  }
586  for (size_t i = 0; i < fields_out.size (); ++i)
587  if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba")
588  {
589  rgb_idx_out = int (i);
590  break;
591  }
592 
593  // We have one of the two cases: RGB vs RGBA or RGBA vs RGB
594  if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
595  fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
596  {
597  size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype),
598  field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype);
599 
600  if (field_size_in == field_size_out)
601  {
602  // Iterate over each cluster
603  int cp = 0;
604  for (size_t cc = 0; cc < indices.size (); ++cc)
605  {
606  // Iterate over each idx
607  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
608  {
609  // Iterate over each dimension
610  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp]));
611  // Copy RGB<->RGBA
612  memcpy (reinterpret_cast<char*> (&cloud_out.points[cp]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[indices[cp].indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in);
613  ++cp;
614  }
615  }
616  return;
617  }
618  }
619 
620  // Iterate over each cluster
621  int cp = 0;
622  for (size_t cc = 0; cc < indices.size (); ++cc)
623  {
624  // Iterate over each idx
625  for (size_t i = 0; i < indices[cc].indices.size (); ++i)
626  {
627  // Iterate over each dimension
628  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp]));
629  ++cp;
630  }
631  }
632 }
633 
634 //////////////////////////////////////////////////////////////////////////////////////////////
635 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
637  const pcl::PointCloud<PointIn2T> &cloud2_in,
638  pcl::PointCloud<PointOutT> &cloud_out)
639 {
640  typedef typename pcl::traits::fieldList<PointIn1T>::type FieldList1;
641  typedef typename pcl::traits::fieldList<PointIn2T>::type FieldList2;
642 
643  if (cloud1_in.points.size () != cloud2_in.points.size ())
644  {
645  PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
646  return;
647  }
648 
649  // Resize the output dataset
650  cloud_out.points.resize (cloud1_in.points.size ());
651  cloud_out.header = cloud1_in.header;
652  cloud_out.width = cloud1_in.width;
653  cloud_out.height = cloud1_in.height;
654  if (!cloud1_in.is_dense || !cloud2_in.is_dense)
655  cloud_out.is_dense = false;
656  else
657  cloud_out.is_dense = true;
658 
659  // Iterate over each point
660  for (size_t i = 0; i < cloud_out.points.size (); ++i)
661  {
662  // Iterate over each dimension
663  pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in.points[i], cloud_out.points[i]));
664  pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in.points[i], cloud_out.points[i]));
665  }
666 }
667 
668 #endif // PCL_IO_IMPL_IO_H_
669