41 #ifndef PCL_IO_IMPL_IO_HPP_
42 #define PCL_IO_IMPL_IO_HPP_
44 #include <pcl/common/concatenate.h>
45 #include <pcl/point_types.h>
48 template <
typename Po
intT>
int
50 const std::string &field_name,
51 std::vector<pcl::PCLPointField> &fields)
56 for (
size_t d = 0; d < fields.size (); ++d)
57 if (fields[d].name == field_name)
58 return (static_cast<int>(d));
63 template <
typename Po
intT>
int
65 std::vector<pcl::PCLPointField> &fields)
70 for (
size_t d = 0; d < fields.size (); ++d)
71 if (fields[d].name == field_name)
72 return (static_cast<int>(d));
77 template <
typename Po
intT>
void
86 template <
typename Po
intT>
void
95 template <
typename Po
intT> std::string
99 std::vector<pcl::PCLPointField> fields;
102 for (
size_t i = 0; i < fields.size () - 1; ++i)
103 result += fields[i].name +
" ";
104 result += fields[fields.size () - 1].name;
109 template <
typename Po
intInT,
typename Po
intOutT>
void
127 if (isSamePointType<PointInT, PointOutT> ())
129 memcpy (&cloud_out.
points[0], &cloud_in.
points[0], cloud_in.
points.size () *
sizeof (PointInT));
133 std::vector<pcl::PCLPointField> fields_in, fields_out;
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")
143 rgb_idx_in = int (i);
146 for (
size_t i = 0; i < fields_out.size (); ++i)
147 if (fields_out[i].name ==
"rgb" || fields_out[i].name ==
"rgba")
149 rgb_idx_out = int (i);
154 if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
155 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
157 size_t field_size_in =
getFieldSize (fields_in[rgb_idx_in].datatype),
158 field_size_out =
getFieldSize (fields_out[rgb_idx_out].datatype);
160 if (field_size_in == field_size_out)
162 for (
size_t i = 0; i < cloud_in.
points.size (); ++i)
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);
174 for (
size_t i = 0; i < cloud_in.
points.size (); ++i)
180 template <
typename Po
intT>
void
182 const std::vector<int> &indices,
186 if (indices.size () == cloud_in.
points.size ())
188 cloud_out = cloud_in;
193 cloud_out.
points.resize (indices.size ());
195 cloud_out.
width =
static_cast<uint32_t
>(indices.size ());
202 for (
size_t i = 0; i < indices.size (); ++i)
207 template <
typename Po
intT>
void
209 const std::vector<
int, Eigen::aligned_allocator<int> > &indices,
213 if (indices.size () == cloud_in.
points.size ())
215 cloud_out = cloud_in;
220 cloud_out.
points.resize (indices.size ());
222 cloud_out.
width =
static_cast<uint32_t
> (indices.size ());
229 for (
size_t i = 0; i < indices.size (); ++i)
234 template <
typename Po
intInT,
typename Po
intOutT>
void
236 const std::vector<int> &indices,
240 cloud_out.
points.resize (indices.size ());
242 cloud_out.
width = uint32_t (indices.size ());
254 if (isSamePointType<PointInT, PointOutT> ())
257 for (
size_t i = 0; i < indices.size (); ++i)
258 memcpy (&cloud_out.
points[i], &cloud_in.
points[indices[i]], sizeof (PointInT));
262 std::vector<pcl::PCLPointField> fields_in, fields_out;
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")
272 rgb_idx_in = int (i);
275 for (
size_t i = 0; int (i) < fields_out.size (); ++i)
276 if (fields_out[i].name ==
"rgb" || fields_out[i].name ==
"rgba")
278 rgb_idx_out = int (i);
283 if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
284 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
286 size_t field_size_in =
getFieldSize (fields_in[rgb_idx_in].datatype),
287 field_size_out =
getFieldSize (fields_out[rgb_idx_out].datatype);
289 if (field_size_in == field_size_out)
291 for (
size_t i = 0; i < indices.size (); ++i)
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);
303 for (
size_t i = 0; i < indices.size (); ++i)
309 template <
typename Po
intInT,
typename Po
intOutT>
void
311 const std::vector<
int, Eigen::aligned_allocator<int> > &indices,
315 cloud_out.
points.resize (indices.size ());
317 cloud_out.
width =
static_cast<uint32_t
> (indices.size ());
329 if (isSamePointType<PointInT, PointOutT> ())
332 for (
size_t i = 0; i < indices.size (); ++i)
333 memcpy (&cloud_out.
points[i], &cloud_in.
points[indices[i]], sizeof (PointInT));
337 std::vector<pcl::PCLPointField> fields_in, fields_out;
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")
347 rgb_idx_in = int (i);
350 for (
size_t i = 0; i < fields_out.size (); ++i)
351 if (fields_out[i].name ==
"rgb" || fields_out[i].name ==
"rgba")
353 rgb_idx_out = int (i);
358 if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
359 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
361 size_t field_size_in =
getFieldSize (fields_in[rgb_idx_in].datatype),
362 field_size_out =
getFieldSize (fields_out[rgb_idx_out].datatype);
364 if (field_size_in == field_size_out)
366 for (
size_t i = 0; i < indices.size (); ++i)
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);
378 for (
size_t i = 0; i < indices.size (); ++i)
384 template <
typename Po
intT>
void
392 cloud_out = cloud_in;
406 for (
size_t i = 0; i < indices.
indices.size (); ++i)
411 template <
typename Po
intInT,
typename Po
intOutT>
void
431 if (isSamePointType<PointInT, PointOutT> ())
434 for (
size_t i = 0; i < indices.
indices.size (); ++i)
439 std::vector<pcl::PCLPointField> fields_in, fields_out;
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")
449 rgb_idx_in = int (i);
452 for (
size_t i = 0; i < fields_out.size (); ++i)
453 if (fields_out[i].name ==
"rgb" || fields_out[i].name ==
"rgba")
455 rgb_idx_out = int (i);
460 if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
461 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
463 size_t field_size_in =
getFieldSize (fields_in[rgb_idx_in].datatype),
464 field_size_out =
getFieldSize (fields_out[rgb_idx_out].datatype);
466 if (field_size_in == field_size_out)
468 for (
size_t i = 0; i < indices.
indices.size (); ++i)
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);
480 for (
size_t i = 0; i < indices.
indices.size (); ++i)
486 template <
typename Po
intT>
void
488 const std::vector<pcl::PointIndices> &indices,
492 for (
size_t i = 0; i < indices.size (); ++i)
493 nr_p += indices[i].indices.size ();
496 if (nr_p == cloud_in.
points.size ())
498 cloud_out = cloud_in;
503 cloud_out.
points.resize (nr_p);
505 cloud_out.
width = nr_p;
513 for (
size_t cc = 0; cc < indices.size (); ++cc)
516 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
519 cloud_out.
points[cp] = cloud_in.
points[indices[cc].indices[i]];
526 template <
typename Po
intInT,
typename Po
intOutT>
void
528 const std::vector<pcl::PointIndices> &indices,
532 for (
size_t i = 0; i < indices.size (); ++i)
533 nr_p += indices[i].indices.size ();
536 if (nr_p == cloud_in.
points.size ())
538 copyPointCloud<PointInT, PointOutT> (cloud_in, cloud_out);
543 cloud_out.
points.resize (nr_p);
545 cloud_out.
width = nr_p;
557 if (isSamePointType<PointInT, PointOutT> ())
561 for (
size_t cc = 0; cc < indices.size (); ++cc)
564 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
566 cloud_out.
points[cp] = cloud_in.
points[indices[cc].indices[i]];
573 std::vector<pcl::PCLPointField> fields_in, fields_out;
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")
583 rgb_idx_in = int (i);
586 for (
size_t i = 0; i < fields_out.size (); ++i)
587 if (fields_out[i].name ==
"rgb" || fields_out[i].name ==
"rgba")
589 rgb_idx_out = int (i);
594 if (rgb_idx_in != -1 && rgb_idx_out != -1 &&
595 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name)
597 size_t field_size_in =
getFieldSize (fields_in[rgb_idx_in].datatype),
598 field_size_out =
getFieldSize (fields_out[rgb_idx_out].datatype);
600 if (field_size_in == field_size_out)
604 for (
size_t cc = 0; cc < indices.size (); ++cc)
607 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
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);
622 for (
size_t cc = 0; cc < indices.size (); ++cc)
625 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
635 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void
643 if (cloud1_in.
points.size () != cloud2_in.
points.size ())
645 PCL_ERROR (
"[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
660 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
668 #endif // PCL_IO_IMPL_IO_H_