43 #include <pcl/conversions.h>
44 #include <pcl/common/concatenate.h>
45 #include <pcl/common/copy_point.h>
46 #include <pcl/common/io.h>
53 template <
typename Po
intT>
int
55 const std::string &field_name,
56 std::vector<pcl::PCLPointField> &fields)
58 return getFieldIndex<PointT>(field_name, fields);
62 template <
typename Po
intT>
int
64 std::vector<pcl::PCLPointField> &fields)
66 fields = getFields<PointT> ();
67 const auto& ref = fields;
68 return pcl::getFieldIndex<PointT> (field_name, ref);
72 template <
typename Po
intT>
int
74 const std::vector<pcl::PCLPointField> &fields)
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 ())
84 template <
typename Po
intT>
void
87 fields = getFields<PointT> ();
91 template <
typename Po
intT>
void
94 fields = getFields<PointT> ();
98 template <
typename Po
intT> std::vector<pcl::PCLPointField>
101 std::vector<pcl::PCLPointField> fields;
108 template <
typename Po
intT> std::string
112 const auto fields = getFields<PointT>();
114 for (std::size_t i = 0; i < fields.size () - 1; ++i)
115 result += fields[i].name +
" ";
116 result += fields[fields.size () - 1].name;
121 template <
typename Po
intInT,
typename Po
intOutT>
void
134 if (cloud_in.
empty ())
137 if (isSamePointType<PointInT, PointOutT> ())
139 memcpy (&cloud_out[0], &cloud_in[0], cloud_in.
size () * sizeof (PointInT));
142 for (std::size_t i = 0; i < cloud_in.
size (); ++i)
147 template <
typename Po
intT,
typename IndicesVectorAllocator>
void
153 if (indices.size () == cloud_in.
size ())
155 cloud_out = cloud_in;
160 cloud_out.
resize (indices.size ());
162 cloud_out.
width = indices.size ();
169 for (std::size_t i = 0; i < indices.size (); ++i)
170 cloud_out[i] = cloud_in[indices[i]];
174 template <
typename Po
intInT,
typename Po
intOutT,
typename IndicesVectorAllocator>
void
180 cloud_out.
resize (indices.size ());
182 cloud_out.
width = indices.size ();
189 for (std::size_t i = 0; i < indices.size (); ++i)
190 copyPoint (cloud_in[indices[i]], cloud_out[i]);
194 template <
typename Po
intT>
void
202 cloud_out = cloud_in;
216 for (std::size_t i = 0; i < indices.
indices.size (); ++i)
217 cloud_out[i] = cloud_in[indices.
indices[i]];
221 template <
typename Po
intInT,
typename Po
intOutT>
void
230 template <
typename Po
intT>
void
232 const std::vector<pcl::PointIndices> &indices,
236 for (
const auto &index : indices)
237 nr_p += index.indices.size ();
240 if (nr_p == cloud_in.
size ())
242 cloud_out = cloud_in;
249 cloud_out.
width = nr_p;
257 for (
const auto &cluster_index : indices)
260 for (
const auto &index : cluster_index.indices)
263 cloud_out[cp] = cloud_in[index];
270 template <
typename Po
intInT,
typename Po
intOutT>
void
272 const std::vector<pcl::PointIndices> &indices,
275 const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
276 [](
const auto& acc,
const auto& index) { return index.indices.size() + acc; });
279 if (nr_p == cloud_in.
size ())
288 cloud_out.
width = nr_p;
296 for (
const auto &cluster_index : indices)
299 for (
const auto &index : cluster_index.indices)
301 copyPoint (cloud_in[index], cloud_out[cp]);
308 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void
313 using FieldList1 =
typename pcl::traits::fieldList<PointIn1T>::type;
314 using FieldList2 =
typename pcl::traits::fieldList<PointIn2T>::type;
316 if (cloud1_in.
size () != cloud2_in.
size ())
318 PCL_ERROR (
"[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
333 for (std::size_t i = 0; i < cloud_out.
size (); ++i)
342 template <
typename Po
intT>
void
346 if (top < 0 || left < 0 || bottom < 0 || right < 0)
348 std::string faulty = (top < 0) ?
"top" : (left < 0) ?
"left" : (bottom < 0) ?
"bottom" :
"right";
353 if (top == 0 && left == 0 && bottom == 0 && right == 0)
354 cloud_out = cloud_in;
359 cloud_out.
width = cloud_in.
width + left + right;
369 const PointT* in = &(cloud_in[0]);
370 PointT* out = &(cloud_out[0]);
371 PointT* out_inner = out + cloud_out.
width*top + left;
372 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
375 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
385 std::vector<int> padding (cloud_out.
width - cloud_in.
width);
386 int right = cloud_out.
width - cloud_in.
width - left;
389 for (
int i = 0; i < left; i++)
392 for (
int i = 0; i < right; i++)
395 const PointT* in = &(cloud_in[0]);
396 PointT* out = &(cloud_out[0]);
397 PointT* out_inner = out + cloud_out.
width*top + left;
399 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
402 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
404 for (
int j = 0; j < left; j++)
405 out_inner[j - left] = in[padding[j]];
407 for (
int j = 0; j < right; j++)
408 out_inner[j + cloud_in.
width] = in[padding[j + left]];
411 for (
int i = 0; i < top; i++)
414 memcpy (out + i*cloud_out.
width,
415 out + (j+top) * cloud_out.
width,
419 for (
int i = 0; i < bottom; i++)
422 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
423 out + (j+top)*cloud_out.
width,
429 PCL_ERROR (
"[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
434 int right = cloud_out.
width - cloud_in.
width - left;
436 std::vector<PointT> buff (cloud_out.
width, value);
437 PointT* buff_ptr = &(buff[0]);
438 const PointT* in = &(cloud_in[0]);
439 PointT* out = &(cloud_out[0]);
440 PointT* out_inner = out + cloud_out.
width*top + left;
442 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
445 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
447 memcpy (out_inner - left, buff_ptr, left *
sizeof (
PointT));
448 memcpy (out_inner + cloud_in.
width, buff_ptr, right * sizeof (
PointT));
451 for (
int i = 0; i < top; i++)
453 memcpy (out + i*cloud_out.
width, buff_ptr, cloud_out.
width * sizeof (
PointT));
456 for (
int i = 0; i < bottom; i++)
458 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,