41 #ifndef PCL_IO_IMPL_IO_HPP_
42 #define PCL_IO_IMPL_IO_HPP_
44 #include <pcl/common/concatenate.h>
45 #include <pcl/common/copy_point.h>
49 template <
typename Po
intT>
int
51 const std::string &field_name,
52 std::vector<pcl::PCLPointField> &fields)
54 return getFieldIndex<PointT>(field_name, fields);
58 template <
typename Po
intT>
int
60 std::vector<pcl::PCLPointField> &fields)
62 fields = getFields<PointT> ();
63 const auto& ref = fields;
64 return pcl::getFieldIndex<PointT> (field_name, ref);
68 template <
typename Po
intT>
int
70 const std::vector<pcl::PCLPointField> &fields)
72 const auto result = std::find_if(fields.begin (), fields.end (),
73 [&field_name](
const auto& field) { return field.name == field_name; });
74 if (result == fields.end ())
80 template <
typename Po
intT>
void
83 fields = getFields<PointT> ();
87 template <
typename Po
intT>
void
90 fields = getFields<PointT> ();
94 template <
typename Po
intT> std::vector<pcl::PCLPointField>
97 std::vector<pcl::PCLPointField> fields;
104 template <
typename Po
intT> std::string
108 const auto fields = getFields<PointT>();
110 for (std::size_t i = 0; i < fields.size () - 1; ++i)
111 result += fields[i].name +
" ";
112 result += fields[fields.size () - 1].name;
117 template <
typename Po
intInT,
typename Po
intOutT>
void
130 if (cloud_in.
points.empty ())
133 if (isSamePointType<PointInT, PointOutT> ())
135 memcpy (&cloud_out.
points[0], &cloud_in.
points[0], cloud_in.
points.size () * sizeof (PointInT));
138 for (std::size_t i = 0; i < cloud_in.
points.size (); ++i)
143 template <
typename Po
intT,
typename IndicesVectorAllocator>
void
145 const std::vector<int, IndicesVectorAllocator> &indices,
149 if (indices.size () == cloud_in.
points.size ())
151 cloud_out = cloud_in;
156 cloud_out.
points.resize (indices.size ());
158 cloud_out.
width =
static_cast<std::uint32_t
>(indices.size ());
165 for (std::size_t i = 0; i < indices.size (); ++i)
170 template <
typename Po
intInT,
typename Po
intOutT,
typename IndicesVectorAllocator>
void
172 const std::vector<int, IndicesVectorAllocator> &indices,
176 cloud_out.
points.resize (indices.size ());
178 cloud_out.
width = std::uint32_t (indices.size ());
185 for (std::size_t i = 0; i < indices.size (); ++i)
190 template <
typename Po
intT>
void
198 cloud_out = cloud_in;
212 for (std::size_t i = 0; i < indices.
indices.size (); ++i)
217 template <
typename Po
intInT,
typename Po
intOutT>
void
226 template <
typename Po
intT>
void
228 const std::vector<pcl::PointIndices> &indices,
232 for (
const auto &index : indices)
233 nr_p += index.indices.size ();
236 if (nr_p == cloud_in.
points.size ())
238 cloud_out = cloud_in;
243 cloud_out.
points.resize (nr_p);
245 cloud_out.
width = nr_p;
253 for (
const auto &cluster_index : indices)
256 for (
const auto &index : cluster_index.indices)
266 template <
typename Po
intInT,
typename Po
intOutT>
void
268 const std::vector<pcl::PointIndices> &indices,
271 const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
272 [](
const auto& acc,
const auto& index) { return index.indices.size() + acc; });
275 if (nr_p == cloud_in.
points.size ())
282 cloud_out.
points.resize (nr_p);
284 cloud_out.
width = nr_p;
292 for (
const auto &cluster_index : indices)
295 for (
const auto &index : cluster_index.indices)
304 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void
312 if (cloud1_in.
points.size () != cloud2_in.
points.size ())
314 PCL_ERROR (
"[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
329 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
338 template <
typename Po
intT>
void
342 if (top < 0 || left < 0 || bottom < 0 || right < 0)
344 std::string faulty = (top < 0) ?
"top" : (left < 0) ?
"left" : (bottom < 0) ?
"bottom" :
"right";
349 if (top == 0 && left == 0 && bottom == 0 && right == 0)
350 cloud_out = cloud_in;
355 cloud_out.
width = cloud_in.
width + left + right;
367 PointT* out_inner = out + cloud_out.
width*top + left;
368 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
371 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
381 std::vector<int> padding (cloud_out.
width - cloud_in.
width);
382 int right = cloud_out.
width - cloud_in.
width - left;
385 for (
int i = 0; i < left; i++)
388 for (
int i = 0; i < right; i++)
393 PointT* out_inner = out + cloud_out.
width*top + left;
395 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
398 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
400 for (
int j = 0; j < left; j++)
401 out_inner[j - left] = in[padding[j]];
403 for (
int j = 0; j < right; j++)
404 out_inner[j + cloud_in.
width] = in[padding[j + left]];
407 for (
int i = 0; i < top; i++)
410 memcpy (out + i*cloud_out.
width,
411 out + (j+top) * cloud_out.
width,
415 for (
int i = 0; i < bottom; i++)
418 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
419 out + (j+top)*cloud_out.
width,
425 PCL_ERROR (
"[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
430 int right = cloud_out.
width - cloud_in.
width - left;
432 std::vector<PointT> buff (cloud_out.
width, value);
433 PointT* buff_ptr = &(buff[0]);
436 PointT* out_inner = out + cloud_out.
width*top + left;
438 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
441 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
443 memcpy (out_inner - left, buff_ptr, left *
sizeof (
PointT));
444 memcpy (out_inner + cloud_in.
width, buff_ptr, right * sizeof (
PointT));
447 for (
int i = 0; i < top; i++)
449 memcpy (out + i*cloud_out.
width, buff_ptr, cloud_out.
width * sizeof (
PointT));
452 for (
int i = 0; i < bottom; i++)
454 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
463 #endif // PCL_IO_IMPL_IO_H_