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;
123 template <
typename Po
intInT,
typename Po
intOutT>
void
128 for (std::size_t i = 0; i < cloud_in.
size (); ++i)
133 template <
typename Po
intT>
void
138 std::copy (&cloud_in[0], (&cloud_in[0]) + cloud_in.
size (), &cloud_out[0]);
143 template <
typename Po
intInT,
typename Po
intOutT>
void
156 if (!cloud_in.
empty ())
161 template <
typename Po
intT,
typename IndicesVectorAllocator>
void
167 if (indices.size () == cloud_in.
size ())
169 cloud_out = cloud_in;
175 cloud_out.
reserve (indices.size ());
177 cloud_out.
width = indices.size ();
184 for (
const auto& index : indices)
189 template <
typename Po
intInT,
typename Po
intOutT,
typename IndicesVectorAllocator>
void
195 cloud_out.
resize (indices.size ());
197 cloud_out.
width = indices.size ();
204 for (std::size_t i = 0; i < indices.size (); ++i)
205 copyPoint (cloud_in[indices[i]], cloud_out[i]);
209 template <
typename Po
intT>
void
218 template <
typename Po
intInT,
typename Po
intOutT>
void
227 template <
typename Po
intT>
void
229 const std::vector<pcl::PointIndices> &indices,
232 std::size_t nr_p = 0;
233 for (
const auto &index : indices)
234 nr_p += index.indices.size ();
237 if (nr_p == cloud_in.
size ())
239 cloud_out = cloud_in;
247 cloud_out.
width = nr_p;
254 for (
const auto &cluster_index : indices)
257 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.
size ())
284 cloud_out.
width = nr_p;
292 for (
const auto &cluster_index : indices)
295 for (
const auto &index : cluster_index.indices)
297 copyPoint (cloud_in[index], cloud_out[cp]);
304 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void
309 using FieldList1 =
typename pcl::traits::fieldList<PointIn1T>::type;
310 using FieldList2 =
typename pcl::traits::fieldList<PointIn2T>::type;
312 if (cloud1_in.
size () != cloud2_in.
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.
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;
365 const PointT* in = &(cloud_in[0]);
366 PointT* out = &(cloud_out[0]);
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++)
391 const PointT* in = &(cloud_in[0]);
392 PointT* out = &(cloud_out[0]);
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]);
434 const PointT* in = &(cloud_in[0]);
435 PointT* out = &(cloud_out[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,
An exception that is thrown when the arguments number or type is wrong/unhandled.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
void transient_push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
void reserve(std::size_t n)
Defines all the PCL implemented PointT point type structures.
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
void concatenateFields(const pcl::PointCloud< PointIn1T > &cloud1_in, const pcl::PointCloud< PointIn2T > &cloud2_in, pcl::PointCloud< PointOutT > &cloud_out)
Concatenate two datasets representing different fields.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
void copyPointCloudMemcpy(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
float distance(const PointT &p1, const PointT &p2)
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
void getFields(const pcl::PointCloud< PointT > &, std::vector< pcl::PCLPointField > &fields)
Helper functor structure for concatenate.
A point structure representing Euclidean xyz coordinates, and the RGB color.