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_