43 #include <pcl/common/concatenate.h>
44 #include <pcl/common/copy_point.h>
45 #include <pcl/common/io.h>
46 #include <pcl/point_types.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 ())
79 return std::distance(fields.begin (), result);
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.
points[0], &cloud_in.
points[0], cloud_in.
points.size () *
sizeof (PointInT));
141 for (std::size_t i = 0; i < cloud_in.
points.size (); ++i)
146 template <
typename Po
intT,
typename IndicesVectorAllocator>
void
148 const std::vector<int, IndicesVectorAllocator> &indices,
152 if (indices.size () == cloud_in.
points.size ())
154 cloud_out = cloud_in;
159 cloud_out.
points.resize (indices.size ());
161 cloud_out.
width =
static_cast<std::uint32_t
>(indices.size ());
168 for (std::size_t i = 0; i < indices.size (); ++i)
173 template <
typename Po
intInT,
typename Po
intOutT,
typename IndicesVectorAllocator>
void
175 const std::vector<int, IndicesVectorAllocator> &indices,
179 cloud_out.
points.resize (indices.size ());
181 cloud_out.
width = std::uint32_t (indices.size ());
188 for (std::size_t i = 0; i < indices.size (); ++i)
193 template <
typename Po
intT>
void
201 cloud_out = cloud_in;
215 for (std::size_t i = 0; i < indices.
indices.size (); ++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.
points.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)
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.
points.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)
307 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void
315 if (cloud1_in.
points.size () != cloud2_in.
points.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.
points.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;
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++)
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]);
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,
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
An exception that is thrown when the arguments number or type is wrong/unhandled. ...
std::uint32_t width
The point cloud width (if organized as an image-structure).
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)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
std::uint32_t height
The point cloud height (if organized as an image-structure).
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 resize(std::size_t n)
Resize the cloud.
Helper functor structure for concatenate.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
void getFields(const pcl::PointCloud< PointT > &, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
pcl::PCLHeader header
The point cloud header.