41 #ifndef PCL_IO_IMPL_IO_HPP_ 42 #define PCL_IO_IMPL_IO_HPP_ 44 #include <pcl/common/concatenate.h> 45 #include <pcl/common/copy_point.h> 46 #include <pcl/point_types.h> 49 template <
typename Po
intT>
int 51 const std::string &field_name,
52 std::vector<pcl::PCLPointField> &fields)
57 for (
size_t d = 0; d < fields.size (); ++d)
58 if (fields[d].name == field_name)
59 return (static_cast<int>(d));
64 template <
typename Po
intT>
int 66 std::vector<pcl::PCLPointField> &fields)
71 for (
size_t d = 0; d < fields.size (); ++d)
72 if (fields[d].name == field_name)
73 return (static_cast<int>(d));
78 template <
typename Po
intT>
void 87 template <
typename Po
intT>
void 96 template <
typename Po
intT> std::string
100 std::vector<pcl::PCLPointField> fields;
103 for (
size_t i = 0; i < fields.size () - 1; ++i)
104 result += fields[i].name +
" ";
105 result += fields[fields.size () - 1].name;
110 template <
typename Po
intInT,
typename Po
intOutT>
void 123 if (isSamePointType<PointInT, PointOutT> ())
125 memcpy (&cloud_out.
points[0], &cloud_in.
points[0], cloud_in.
points.size () *
sizeof (PointInT));
128 for (
size_t i = 0; i < cloud_in.
points.size (); ++i)
133 template <
typename Po
intT>
void 135 const std::vector<int> &indices,
139 if (indices.size () == cloud_in.
points.size ())
141 cloud_out = cloud_in;
146 cloud_out.
points.resize (indices.size ());
148 cloud_out.
width =
static_cast<uint32_t
>(indices.size ());
155 for (
size_t i = 0; i < indices.size (); ++i)
160 template <
typename Po
intT>
void 162 const std::vector<
int, Eigen::aligned_allocator<int> > &indices,
166 if (indices.size () == cloud_in.
points.size ())
168 cloud_out = cloud_in;
173 cloud_out.
points.resize (indices.size ());
175 cloud_out.
width =
static_cast<uint32_t
> (indices.size ());
182 for (
size_t i = 0; i < indices.size (); ++i)
187 template <
typename Po
intInT,
typename Po
intOutT>
void 189 const std::vector<int> &indices,
193 cloud_out.
points.resize (indices.size ());
195 cloud_out.
width = uint32_t (indices.size ());
202 for (
size_t i = 0; i < indices.size (); ++i)
207 template <
typename Po
intInT,
typename Po
intOutT>
void 209 const std::vector<
int, Eigen::aligned_allocator<int> > &indices,
213 cloud_out.
points.resize (indices.size ());
215 cloud_out.
width =
static_cast<uint32_t
> (indices.size ());
222 for (
size_t i = 0; i < indices.size (); ++i)
227 template <
typename Po
intT>
void 235 cloud_out = cloud_in;
249 for (
size_t i = 0; i < indices.
indices.size (); ++i)
254 template <
typename Po
intInT,
typename Po
intOutT>
void 263 template <
typename Po
intT>
void 265 const std::vector<pcl::PointIndices> &indices,
269 for (
size_t i = 0; i < indices.size (); ++i)
270 nr_p += indices[i].indices.size ();
273 if (nr_p == cloud_in.
points.size ())
275 cloud_out = cloud_in;
280 cloud_out.
points.resize (nr_p);
282 cloud_out.
width = nr_p;
290 for (
size_t cc = 0; cc < indices.size (); ++cc)
293 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
296 cloud_out.
points[cp] = cloud_in.
points[indices[cc].indices[i]];
303 template <
typename Po
intInT,
typename Po
intOutT>
void 305 const std::vector<pcl::PointIndices> &indices,
309 for (
size_t i = 0; i < indices.size (); ++i)
310 nr_p += indices[i].indices.size ();
313 if (nr_p == cloud_in.
points.size ())
315 copyPointCloud<PointInT, PointOutT> (cloud_in, cloud_out);
320 cloud_out.
points.resize (nr_p);
322 cloud_out.
width = nr_p;
330 for (
size_t cc = 0; cc < indices.size (); ++cc)
333 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
342 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void 350 if (cloud1_in.
points.size () != cloud2_in.
points.size ())
352 PCL_ERROR (
"[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
367 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
376 template <
typename Po
intT>
void 380 if (top < 0 || left < 0 || bottom < 0 || right < 0)
382 std::string faulty = (top < 0) ?
"top" : (left < 0) ?
"left" : (bottom < 0) ?
"bottom" :
"right";
387 if (top == 0 && left == 0 && bottom == 0 && right == 0)
388 cloud_out = cloud_in;
393 cloud_out.
width = cloud_in.
width + left + right;
405 PointT* out_inner = out + cloud_out.
width*top + left;
406 for (uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
409 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
419 std::vector<int> padding (cloud_out.
width - cloud_in.
width);
420 int right = cloud_out.
width - cloud_in.
width - left;
423 for (
int i = 0; i < left; i++)
426 for (
int i = 0; i < right; i++)
431 PointT* out_inner = out + cloud_out.
width*top + left;
433 for (uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
436 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
438 for (
int j = 0; j < left; j++)
439 out_inner[j - left] = in[padding[j]];
441 for (
int j = 0; j < right; j++)
442 out_inner[j + cloud_in.
width] = in[padding[j + left]];
445 for (
int i = 0; i < top; i++)
448 memcpy (out + i*cloud_out.
width,
449 out + (j+top) * cloud_out.
width,
453 for (
int i = 0; i < bottom; i++)
456 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
457 out + (j+top)*cloud_out.
width,
463 PCL_ERROR (
"[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
468 int right = cloud_out.
width - cloud_in.
width - left;
470 std::vector<PointT> buff (cloud_out.
width, value);
471 PointT* buff_ptr = &(buff[0]);
474 PointT* out_inner = out + cloud_out.
width*top + left;
476 for (uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
479 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
481 memcpy (out_inner - left, buff_ptr, left *
sizeof (
PointT));
482 memcpy (out_inner + cloud_in.
width, buff_ptr, right * sizeof (
PointT));
485 for (
int i = 0; i < top; i++)
487 memcpy (out + i*cloud_out.
width, buff_ptr, cloud_out.
width * sizeof (
PointT));
490 for (
int i = 0; i < bottom; i++)
492 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
501 #endif // PCL_IO_IMPL_IO_H_
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::string getFieldsList(const pcl::PointCloud< PointT > &cloud)
Get the list of all fields available in a given cloud.
An exception that is thrown when the argments number or type is wrong/unhandled.
std::vector< int > indices
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
uint32_t height
The point cloud height (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
uint32_t width
The point cloud width (if organized as an image-structure).
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void resize(size_t n)
Resize the cloud.
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.
Helper functor structure for concatenate.
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 > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)