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.
points.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.
points.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.
points.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;
246 cloud_out.
points.resize (nr_p);
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 ())
285 cloud_out.
points.resize (nr_p);
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;
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;
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;
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,
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).
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).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Defines all the PCL implemented PointT point type structures.
void getFields(const pcl::PointCloud< PointT > &, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Get the index of a specified field (i.e., dimension/channel)
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.
float distance(const PointT &p1, const PointT &p2)
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
Helper functor structure for concatenate.
A point structure representing Euclidean xyz coordinates, and the RGB color.