43 #include <pcl/common/concatenate.h>
44 #include <pcl/common/copy_point.h>
45 #include <pcl/common/io.h>
52 template <
typename Po
intT>
int
54 const std::string &field_name,
55 std::vector<pcl::PCLPointField> &fields)
57 return getFieldIndex<PointT>(field_name, fields);
61 template <
typename Po
intT>
int
63 std::vector<pcl::PCLPointField> &fields)
65 fields = getFields<PointT> ();
66 const auto& ref = fields;
67 return pcl::getFieldIndex<PointT> (field_name, ref);
71 template <
typename Po
intT>
int
73 const std::vector<pcl::PCLPointField> &fields)
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 ())
83 template <
typename Po
intT>
void
86 fields = getFields<PointT> ();
90 template <
typename Po
intT>
void
93 fields = getFields<PointT> ();
97 template <
typename Po
intT> std::vector<pcl::PCLPointField>
100 std::vector<pcl::PCLPointField> fields;
107 template <
typename Po
intT> std::string
111 const auto fields = getFields<PointT>();
113 for (std::size_t i = 0; i < fields.size () - 1; ++i)
114 result += fields[i].name +
" ";
115 result += fields[fields.size () - 1].name;
120 template <
typename Po
intInT,
typename Po
intOutT>
void
133 if (cloud_in.
empty ())
136 if (isSamePointType<PointInT, PointOutT> ())
138 memcpy (&cloud_out[0], &cloud_in[0], cloud_in.
size () * sizeof (PointInT));
141 for (std::size_t i = 0; i < cloud_in.
size (); ++i)
146 template <
typename Po
intT,
typename IndicesVectorAllocator>
void
152 if (indices.size () == cloud_in.
size ())
154 cloud_out = cloud_in;
159 cloud_out.
resize (indices.size ());
161 cloud_out.
width = indices.size ();
168 for (std::size_t i = 0; i < indices.size (); ++i)
169 cloud_out[i] = cloud_in[indices[i]];
173 template <
typename Po
intInT,
typename Po
intOutT,
typename IndicesVectorAllocator>
void
179 cloud_out.
resize (indices.size ());
181 cloud_out.
width = indices.size ();
188 for (std::size_t i = 0; i < indices.size (); ++i)
189 copyPoint (cloud_in[indices[i]], cloud_out[i]);
193 template <
typename Po
intT>
void
201 cloud_out = cloud_in;
215 for (std::size_t i = 0; i < indices.
indices.size (); ++i)
216 cloud_out[i] = cloud_in[indices.
indices[i]];
220 template <
typename Po
intInT,
typename Po
intOutT>
void
229 template <
typename Po
intT>
void
231 const std::vector<pcl::PointIndices> &indices,
235 for (
const auto &index : indices)
236 nr_p += index.indices.size ();
239 if (nr_p == cloud_in.
size ())
241 cloud_out = cloud_in;
248 cloud_out.
width = nr_p;
256 for (
const auto &cluster_index : indices)
259 for (
const auto &index : cluster_index.indices)
262 cloud_out[cp] = cloud_in[index];
269 template <
typename Po
intInT,
typename Po
intOutT>
void
271 const std::vector<pcl::PointIndices> &indices,
274 const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
275 [](
const auto& acc,
const auto& index) { return index.indices.size() + acc; });
278 if (nr_p == cloud_in.
size ())
287 cloud_out.
width = nr_p;
295 for (
const auto &cluster_index : indices)
298 for (
const auto &index : cluster_index.indices)
300 copyPoint (cloud_in[index], cloud_out[cp]);
307 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void
312 using FieldList1 =
typename pcl::traits::fieldList<PointIn1T>::type;
313 using FieldList2 =
typename pcl::traits::fieldList<PointIn2T>::type;
315 if (cloud1_in.
size () != cloud2_in.
size ())
317 PCL_ERROR (
"[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
332 for (std::size_t i = 0; i < cloud_out.
size (); ++i)
341 template <
typename Po
intT>
void
345 if (top < 0 || left < 0 || bottom < 0 || right < 0)
347 std::string faulty = (top < 0) ?
"top" : (left < 0) ?
"left" : (bottom < 0) ?
"bottom" :
"right";
352 if (top == 0 && left == 0 && bottom == 0 && right == 0)
353 cloud_out = cloud_in;
358 cloud_out.
width = cloud_in.
width + left + right;
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)
374 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
384 std::vector<int> padding (cloud_out.
width - cloud_in.
width);
385 int right = cloud_out.
width - cloud_in.
width - left;
388 for (
int i = 0; i < left; i++)
391 for (
int i = 0; i < right; i++)
394 const PointT* in = &(cloud_in[0]);
395 PointT* out = &(cloud_out[0]);
396 PointT* out_inner = out + cloud_out.
width*top + left;
398 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
401 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
403 for (
int j = 0; j < left; j++)
404 out_inner[j - left] = in[padding[j]];
406 for (
int j = 0; j < right; j++)
407 out_inner[j + cloud_in.
width] = in[padding[j + left]];
410 for (
int i = 0; i < top; i++)
413 memcpy (out + i*cloud_out.
width,
414 out + (j+top) * cloud_out.
width,
418 for (
int i = 0; i < bottom; i++)
421 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
422 out + (j+top)*cloud_out.
width,
428 PCL_ERROR (
"[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
433 int right = cloud_out.
width - cloud_in.
width - left;
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;
441 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
444 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
446 memcpy (out_inner - left, buff_ptr, left *
sizeof (
PointT));
447 memcpy (out_inner + cloud_in.
width, buff_ptr, right * sizeof (
PointT));
450 for (
int i = 0; i < top; i++)
452 memcpy (out + i*cloud_out.
width, buff_ptr, cloud_out.
width * sizeof (
PointT));
455 for (
int i = 0; i < bottom; i++)
457 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,