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> 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 (cloud_in.
points.size () == 0)
126 if (isSamePointType<PointInT, PointOutT> ())
128 memcpy (&cloud_out.
points[0], &cloud_in.
points[0], cloud_in.
points.size () *
sizeof (PointInT));
131 for (
size_t i = 0; i < cloud_in.
points.size (); ++i)
136 template <
typename Po
intT>
void 138 const std::vector<int> &indices,
142 if (indices.size () == cloud_in.
points.size ())
144 cloud_out = cloud_in;
149 cloud_out.
points.resize (indices.size ());
151 cloud_out.
width = static_cast<uint32_t>(indices.size ());
158 for (
size_t i = 0; i < indices.size (); ++i)
163 template <
typename Po
intT>
void 165 const std::vector<
int, Eigen::aligned_allocator<int> > &indices,
169 if (indices.size () == cloud_in.
points.size ())
171 cloud_out = cloud_in;
176 cloud_out.
points.resize (indices.size ());
178 cloud_out.
width = static_cast<uint32_t> (indices.size ());
185 for (
size_t i = 0; i < indices.size (); ++i)
190 template <
typename Po
intInT,
typename Po
intOutT>
void 192 const std::vector<int> &indices,
196 cloud_out.
points.resize (indices.size ());
198 cloud_out.
width = uint32_t (indices.size ());
205 for (
size_t i = 0; i < indices.size (); ++i)
210 template <
typename Po
intInT,
typename Po
intOutT>
void 212 const std::vector<
int, Eigen::aligned_allocator<int> > &indices,
216 cloud_out.
points.resize (indices.size ());
218 cloud_out.
width = static_cast<uint32_t> (indices.size ());
225 for (
size_t i = 0; i < indices.size (); ++i)
230 template <
typename Po
intT>
void 238 cloud_out = cloud_in;
252 for (
size_t i = 0; i < indices.
indices.size (); ++i)
257 template <
typename Po
intInT,
typename Po
intOutT>
void 266 template <
typename Po
intT>
void 268 const std::vector<pcl::PointIndices> &indices,
272 for (
size_t i = 0; i < indices.size (); ++i)
273 nr_p += indices[i].indices.size ();
276 if (nr_p == cloud_in.
points.size ())
278 cloud_out = cloud_in;
283 cloud_out.
points.resize (nr_p);
285 cloud_out.
width = nr_p;
293 for (
size_t cc = 0; cc < indices.size (); ++cc)
296 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
299 cloud_out.
points[cp] = cloud_in.
points[indices[cc].indices[i]];
306 template <
typename Po
intInT,
typename Po
intOutT>
void 308 const std::vector<pcl::PointIndices> &indices,
312 for (
size_t i = 0; i < indices.size (); ++i)
313 nr_p += indices[i].indices.size ();
316 if (nr_p == cloud_in.
points.size ())
318 copyPointCloud<PointInT, PointOutT> (cloud_in, cloud_out);
323 cloud_out.
points.resize (nr_p);
325 cloud_out.
width = nr_p;
333 for (
size_t cc = 0; cc < indices.size (); ++cc)
336 for (
size_t i = 0; i < indices[cc].indices.size (); ++i)
345 template <
typename Po
intIn1T,
typename Po
intIn2T,
typename Po
intOutT>
void 353 if (cloud1_in.
points.size () != cloud2_in.
points.size ())
355 PCL_ERROR (
"[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
370 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
379 template <
typename Po
intT>
void 383 if (top < 0 || left < 0 || bottom < 0 || right < 0)
385 std::string faulty = (top < 0) ?
"top" : (left < 0) ?
"left" : (bottom < 0) ?
"bottom" :
"right";
390 if (top == 0 && left == 0 && bottom == 0 && right == 0)
391 cloud_out = cloud_in;
396 cloud_out.
width = cloud_in.
width + left + right;
408 PointT* out_inner = out + cloud_out.
width*top + left;
409 for (uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
412 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
422 std::vector<int> padding (cloud_out.
width - cloud_in.
width);
423 int right = cloud_out.
width - cloud_in.
width - left;
426 for (
int i = 0; i < left; i++)
429 for (
int i = 0; i < right; i++)
434 PointT* out_inner = out + cloud_out.
width*top + left;
436 for (uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
439 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
441 for (
int j = 0; j < left; j++)
442 out_inner[j - left] = in[padding[j]];
444 for (
int j = 0; j < right; j++)
445 out_inner[j + cloud_in.
width] = in[padding[j + left]];
448 for (
int i = 0; i < top; i++)
451 memcpy (out + i*cloud_out.
width,
452 out + (j+top) * cloud_out.
width,
456 for (
int i = 0; i < bottom; i++)
459 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
460 out + (j+top)*cloud_out.
width,
466 PCL_ERROR (
"[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
471 int right = cloud_out.
width - cloud_in.
width - left;
473 std::vector<PointT> buff (cloud_out.
width, value);
474 PointT* buff_ptr = &(buff[0]);
477 PointT* out_inner = out + cloud_out.
width*top + left;
479 for (uint32_t i = 0; i < cloud_in.
height; i++, out_inner += cloud_out.
width, in += cloud_in.
width)
482 memcpy (out_inner, in, cloud_in.
width * sizeof (
PointT));
484 memcpy (out_inner - left, buff_ptr, left *
sizeof (
PointT));
485 memcpy (out_inner + cloud_in.
width, buff_ptr, right * sizeof (
PointT));
488 for (
int i = 0; i < top; i++)
490 memcpy (out + i*cloud_out.
width, buff_ptr, cloud_out.
width * sizeof (
PointT));
493 for (
int i = 0; i < bottom; i++)
495 memcpy (out + (i + cloud_in.
height + top)*cloud_out.
width,
504 #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).
Defines all the PCL implemented PointT point type structures.
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)