43#include <pcl/conversions.h>
44#include <pcl/common/concatenate.h>
45#include <pcl/common/copy_point.h>
46#include <pcl/common/io.h>
56template <
typename Po
intT>
int
58 const std::string &field_name,
59 std::vector<pcl::PCLPointField> &fields)
61 return getFieldIndex<PointT>(field_name, fields);
65template <
typename Po
intT>
int
67 std::vector<pcl::PCLPointField> &fields)
69 fields = getFields<PointT> ();
70 const auto& ref = fields;
71 return pcl::getFieldIndex<PointT> (field_name, ref);
75template <
typename Po
intT>
int
77 const std::vector<pcl::PCLPointField> &fields)
79 const auto result = std::find_if(fields.begin (), fields.end (),
80 [&field_name](
const auto& field) { return field.name == field_name; });
81 if (result == fields.end ())
83 return std::distance(fields.begin (), result);
87template <
typename Po
intT>
void
90 fields = getFields<PointT> ();
94template <
typename Po
intT>
void
97 fields = getFields<PointT> ();
101template <
typename Po
intT> std::vector<pcl::PCLPointField>
104 std::vector<pcl::PCLPointField> fields;
110template <
typename CloudT>
inline std::vector<pcl::PCLPointField>
113 return pcl::getFields<typename CloudT::PointType>();
117template <>
inline std::vector<pcl::PCLPointField>
123template <
typename Po
intT> std::string
127 const auto fields = getFields<PointT>();
129 for (std::size_t i = 0; i < fields.size () - 1; ++i)
130 result += fields[i].name +
" ";
131 result += fields[fields.size () - 1].name;
149 memcpy (&value, p,
sizeof (DType));
150 return static_cast<RType
>(value);
153template <
typename T> T
point_field_as (
const std::uint8_t* data,
const std::uint8_t type)
157 return reinterpret_and_cast<float, T>(data);
160 return reinterpret_and_cast<std::int8_t, T>(data);
163 return reinterpret_and_cast<std::uint8_t, T>(data);
166 return reinterpret_and_cast<std::uint16_t, T>(data);
169 return reinterpret_and_cast<std::uint32_t, T>(data);
172 return reinterpret_and_cast<std::uint64_t, T>(data);
175 return reinterpret_and_cast<bool, T>(data);
178 return reinterpret_and_cast<double, T>(data);
181 return reinterpret_and_cast<std::int16_t, T>(data);
184 return reinterpret_and_cast<std::int32_t, T>(data);
187 return reinterpret_and_cast<std::int64_t, T>(data);
205 if constexpr (std::is_floating_point_v<DType>)
206 result =
static_cast<DType
>(value);
207 else if (!(value >
static_cast<double>(std::numeric_limits<DType>::lowest())))
208 result = std::numeric_limits<DType>::lowest();
209 else if (value >=
static_cast<double>(std::numeric_limits<DType>::max()))
210 result = std::numeric_limits<DType>::max();
212 result =
static_cast<DType
>(std::round(value));
213 memcpy (p, &result,
sizeof (DType));
216inline void storeFieldValue (
const double value, std::uint8_t* data,
const std::uint8_t type)
220 storeFieldValueAs<float>(value, data);
223 storeFieldValueAs<std::int8_t>(value, data);
226 storeFieldValueAs<std::uint8_t>(value, data);
229 storeFieldValueAs<std::uint16_t>(value, data);
232 storeFieldValueAs<std::uint32_t>(value, data);
235 storeFieldValueAs<std::uint64_t>(value, data);
238 storeFieldValueAs<bool>(value, data);
241 storeFieldValueAs<double>(value, data);
244 storeFieldValueAs<std::int16_t>(value, data);
247 storeFieldValueAs<std::int32_t>(value, data);
250 storeFieldValueAs<std::int64_t>(value, data);
261 template <
typename Po
intInT,
typename Po
intOutT>
void
266 for (std::size_t i = 0; i < cloud_in.
size (); ++i)
271 template <
typename Po
intT>
void
276 std::copy (cloud_in.
data(), cloud_in.
data() + cloud_in.
size (), cloud_out.
data());
281template <
typename Po
intInT,
typename Po
intOutT>
void
294 if (!cloud_in.
empty ())
299template <
typename Po
intT,
typename IndicesVectorAllocator>
void
305 if (indices.size () == cloud_in.
size ())
307 cloud_out = cloud_in;
313 cloud_out.
reserve (indices.size ());
315 cloud_out.
width = indices.size ();
322 for (
const auto& index : indices)
327template <
typename Po
intInT,
typename Po
intOutT,
typename IndicesVectorAllocator>
void
333 cloud_out.
resize (indices.size ());
335 cloud_out.
width = indices.size ();
342 for (std::size_t i = 0; i < indices.size (); ++i)
343 copyPoint (cloud_in[indices[i]], cloud_out[i]);
347template <
typename Po
intT>
void
356template <
typename Po
intInT,
typename Po
intOutT>
void
365template <
typename Po
intT>
void
367 const std::vector<pcl::PointIndices> &indices,
370 std::size_t nr_p = 0;
371 for (
const auto &index : indices)
372 nr_p += index.indices.size ();
375 if (nr_p == cloud_in.
size ())
377 cloud_out = cloud_in;
385 cloud_out.
width = nr_p;
392 for (
const auto &cluster_index : indices)
395 for (
const auto &index : cluster_index.indices)
404template <
typename Po
intInT,
typename Po
intOutT>
void
406 const std::vector<pcl::PointIndices> &indices,
409 const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
410 [](
const auto& acc,
const auto& index) { return index.indices.size() + acc; });
413 if (nr_p == cloud_in.
size ())
422 cloud_out.
width = nr_p;
430 for (
const auto &cluster_index : indices)
433 for (
const auto &index : cluster_index.indices)
435 copyPoint (cloud_in[index], cloud_out[cp]);
442template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void
447 using FieldList1 =
typename pcl::traits::fieldList<PointIn1T>::type;
448 using FieldList2 =
typename pcl::traits::fieldList<PointIn2T>::type;
450 if (cloud1_in.
size () != cloud2_in.
size ())
452 PCL_ERROR (
"[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
467 for (std::size_t i = 0; i < cloud_out.
size (); ++i)
470 pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in[i], cloud_out[i]));
471 pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in[i], cloud_out[i]));
476template <
typename Po
intT>
void
480 if (top < 0 || left < 0 || bottom < 0 || right < 0)
482 std::string faulty = (top < 0) ?
"top" : (left < 0) ?
"left" : (bottom < 0) ?
"bottom" :
"right";
487 if (top == 0 && left == 0 && bottom == 0 && right == 0)
488 cloud_out = cloud_in;
493 cloud_out.
width = cloud_in.
width + left + right;
505 PointT* out_inner = out + cloud_out.
width*top + left;
506 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
508 if (out_inner != in) {
509 std::copy(in, in + cloud_in.
width, out_inner);
520 std::vector<int> padding (cloud_out.
width - cloud_in.
width);
521 int right = cloud_out.
width - cloud_in.
width - left;
524 for (
int i = 0; i < left; i++)
527 for (
int i = 0; i < right; i++)
532 PointT* out_inner = out + cloud_out.
width*top + left;
534 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
536 if (out_inner != in) {
537 std::copy(in, in + cloud_in.
width, out_inner);
540 for (
int j = 0; j < left; j++)
541 out_inner[j - left] = in[padding[j]];
543 for (
int j = 0; j < right; j++)
544 out_inner[j + cloud_in.
width] = in[padding[j + left]];
547 for (
int i = 0; i < top; i++)
550 std::copy(out + (j+top) * cloud_out.
width, out + (j+top) * cloud_out.
width + cloud_out.
width,
551 out + i*cloud_out.
width);
554 for (
int i = 0; i < bottom; i++)
557 std::copy(out + (j+top)*cloud_out.
width, out + (j+top)*cloud_out.
width + cloud_out.
width,
558 out + (i + cloud_in.
height + top)*cloud_out.
width);
563 PCL_ERROR (
"[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
568 int right = cloud_out.
width - cloud_in.
width - left;
570 std::vector<PointT> buff (cloud_out.
width, value);
571 PointT* buff_ptr = buff.data();
574 PointT* out_inner = out + cloud_out.
width*top + left;
576 for (std::uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
578 if (out_inner != in) {
579 std::copy(in, in + cloud_in.
width, out_inner);
582 std::copy(buff_ptr, buff_ptr + left, out_inner - left);
583 std::copy(buff_ptr, buff_ptr + right, out_inner + cloud_in.
width);
586 for (
int i = 0; i < top; i++)
588 std::copy(buff_ptr, buff_ptr + cloud_out.
width, out + i*cloud_out.
width);
591 for (
int i = 0; i < bottom; i++)
593 std::copy(buff_ptr, buff_ptr + cloud_out.
width,
594 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)
T point_field_as(const std::uint8_t *data, const std::uint8_t type)
Get the value of a point field from raw data pointer and field type.
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
void storeFieldValue(const double value, std::uint8_t *data, const std::uint8_t type)
Set the value of a point field from raw data pointer and field type.
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.
std::vector< pcl::PCLPointField > getFields()
Get the list of available fields (i.e., dimension/channel)
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)
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 storeFieldValueAs(const double value, std::uint8_t *p)
convert a value to a type and store it at a pointer.
std::vector< pcl::PCLPointField > getFields< pcl::PCLPointCloud2 >(const pcl::PCLPointCloud2 &cloud)
RType reinterpret_and_cast(const std::uint8_t *p)
reinterpret a pointer as a type, dereference it, and return result as target type
std::vector<::pcl::PCLPointField > fields
A point structure representing Euclidean xyz coordinates, and the RGB color.