Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
io.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <pcl/conversions.h> // for FieldAdder
44#include <pcl/common/concatenate.h>
45#include <pcl/common/copy_point.h>
46#include <pcl/common/io.h>
47#include <pcl/point_types.h>
48
49
50namespace pcl
51{
52
53template <typename PointT> int
55 const std::string &field_name,
56 std::vector<pcl::PCLPointField> &fields)
57{
58 return getFieldIndex<PointT>(field_name, fields);
59}
60
61
62template <typename PointT> int
63getFieldIndex (const std::string &field_name,
64 std::vector<pcl::PCLPointField> &fields)
65{
66 fields = getFields<PointT> ();
67 const auto& ref = fields;
68 return pcl::getFieldIndex<PointT> (field_name, ref);
69}
70
71
72template <typename PointT> int
73getFieldIndex (const std::string &field_name,
74 const std::vector<pcl::PCLPointField> &fields)
75{
76 const auto result = std::find_if(fields.begin (), fields.end (),
77 [&field_name](const auto& field) { return field.name == field_name; });
78 if (result == fields.end ())
79 return -1;
80 return std::distance(fields.begin (), result);
81}
82
83
84template <typename PointT> void
85getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
86{
87 fields = getFields<PointT> ();
88}
89
90
91template <typename PointT> void
92getFields (std::vector<pcl::PCLPointField> &fields)
93{
94 fields = getFields<PointT> ();
95}
96
97
98template <typename PointT> std::vector<pcl::PCLPointField>
100{
101 std::vector<pcl::PCLPointField> fields;
102 // Get the fields list
103 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
104 return fields;
105}
106
107
108template <typename PointT> std::string
110{
111 // Get the fields list
112 const auto fields = getFields<PointT>();
113 std::string result;
114 for (std::size_t i = 0; i < fields.size () - 1; ++i)
115 result += fields[i].name + " ";
116 result += fields[fields.size () - 1].name;
117 return (result);
118}
119
120namespace detail
121{
122
123 template <typename PointInT, typename PointOutT> void
126 {
127 // Iterate over each point, if the point types of two clouds are different
128 for (std::size_t i = 0; i < cloud_in.size (); ++i)
129 copyPoint (cloud_in[i], cloud_out[i]);
130 }
131
132
133 template <typename PointT> void
135 pcl::PointCloud<PointT> &cloud_out)
136 {
137 // Use std::copy directly, if the point types of two clouds are same
138 std::copy (&cloud_in[0], (&cloud_in[0]) + cloud_in.size (), &cloud_out[0]);
139 }
140
141} // namespace detail
142
143template <typename PointInT, typename PointOutT> void
146{
147 // Allocate enough space and copy the basics
148 cloud_out.header = cloud_in.header;
149 cloud_out.width = cloud_in.width;
150 cloud_out.height = cloud_in.height;
151 cloud_out.is_dense = cloud_in.is_dense;
152 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
153 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
154 cloud_out.resize (cloud_in.size ());
155
156 if (!cloud_in.empty ())
157 detail::copyPointCloudMemcpy (cloud_in, cloud_out);
158}
159
160
161template <typename PointT, typename IndicesVectorAllocator> void
164 pcl::PointCloud<PointT> &cloud_out)
165{
166 // Do we want to copy everything?
167 if (indices.size () == cloud_in.size ())
168 {
169 cloud_out = cloud_in;
170 return;
171 }
172
173 // Allocate enough space and copy the basics
174 cloud_out.clear ();
175 cloud_out.reserve (indices.size ());
176 cloud_out.header = cloud_in.header;
177 cloud_out.width = indices.size ();
178 cloud_out.height = 1;
179 cloud_out.is_dense = cloud_in.is_dense;
180 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
181 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
182
183 // Iterate over each point
184 for (const auto& index : indices)
185 cloud_out.transient_push_back (cloud_in[index]);
186}
187
188
189template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
193{
194 // Allocate enough space and copy the basics
195 cloud_out.resize (indices.size ());
196 cloud_out.header = cloud_in.header;
197 cloud_out.width = indices.size ();
198 cloud_out.height = 1;
199 cloud_out.is_dense = cloud_in.is_dense;
200 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
201 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
202
203 // Iterate over each point
204 for (std::size_t i = 0; i < indices.size (); ++i)
205 copyPoint (cloud_in[indices[i]], cloud_out[i]);
206}
207
208
209template <typename PointT> void
211 const pcl::PointIndices &indices,
212 pcl::PointCloud<PointT> &cloud_out)
213{
214 copyPointCloud (cloud_in, indices.indices, cloud_out);
215}
216
217
218template <typename PointInT, typename PointOutT> void
220 const pcl::PointIndices &indices,
222{
223 copyPointCloud (cloud_in, indices.indices, cloud_out);
224}
225
226
227template <typename PointT> void
229 const std::vector<pcl::PointIndices> &indices,
230 pcl::PointCloud<PointT> &cloud_out)
231{
232 std::size_t nr_p = 0;
233 for (const auto &index : indices)
234 nr_p += index.indices.size ();
235
236 // Do we want to copy everything? Remember we assume UNIQUE indices
237 if (nr_p == cloud_in.size ())
238 {
239 cloud_out = cloud_in;
240 return;
241 }
242
243 // Allocate enough space and copy the basics
244 cloud_out.clear ();
245 cloud_out.reserve (nr_p);
246 cloud_out.header = cloud_in.header;
247 cloud_out.width = nr_p;
248 cloud_out.height = 1;
249 cloud_out.is_dense = cloud_in.is_dense;
250 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
251 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
252
253 // Iterate over each cluster
254 for (const auto &cluster_index : indices)
255 {
256 // Iterate over each idx
257 for (const auto &index : cluster_index.indices)
258 {
259 // Iterate over each dimension
260 cloud_out.transient_push_back (cloud_in[index]);
261 }
262 }
263}
264
265
266template <typename PointInT, typename PointOutT> void
268 const std::vector<pcl::PointIndices> &indices,
270{
271 const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
272 [](const auto& acc, const auto& index) { return index.indices.size() + acc; });
273
274 // Do we want to copy everything? Remember we assume UNIQUE indices
275 if (nr_p == cloud_in.size ())
276 {
277 copyPointCloud (cloud_in, cloud_out);
278 return;
279 }
280
281 // Allocate enough space and copy the basics
282 cloud_out.resize (nr_p);
283 cloud_out.header = cloud_in.header;
284 cloud_out.width = nr_p;
285 cloud_out.height = 1;
286 cloud_out.is_dense = cloud_in.is_dense;
287 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
288 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
289
290 // Iterate over each cluster
291 std::size_t cp = 0;
292 for (const auto &cluster_index : indices)
293 {
294 // Iterate over each idx
295 for (const auto &index : cluster_index.indices)
296 {
297 copyPoint (cloud_in[index], cloud_out[cp]);
298 ++cp;
299 }
300 }
301}
302
303
304template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
306 const pcl::PointCloud<PointIn2T> &cloud2_in,
308{
309 using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
310 using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
311
312 if (cloud1_in.size () != cloud2_in.size ())
313 {
314 PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
315 return;
316 }
317
318 // Resize the output dataset
319 cloud_out.resize (cloud1_in.size ());
320 cloud_out.header = cloud1_in.header;
321 cloud_out.width = cloud1_in.width;
322 cloud_out.height = cloud1_in.height;
323 if (!cloud1_in.is_dense || !cloud2_in.is_dense)
324 cloud_out.is_dense = false;
325 else
326 cloud_out.is_dense = true;
327
328 // Iterate over each point
329 for (std::size_t i = 0; i < cloud_out.size (); ++i)
330 {
331 // Iterate over each dimension
332 pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in[i], cloud_out[i]));
333 pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in[i], cloud_out[i]));
334 }
335}
336
337
338template <typename PointT> void
340 int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
341{
342 if (top < 0 || left < 0 || bottom < 0 || right < 0)
343 {
344 std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
345 PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
346 return;
347 }
348
349 if (top == 0 && left == 0 && bottom == 0 && right == 0)
350 cloud_out = cloud_in;
351 else
352 {
353 // Allocate enough space and copy the basics
354 cloud_out.header = cloud_in.header;
355 cloud_out.width = cloud_in.width + left + right;
356 cloud_out.height = cloud_in.height + top + bottom;
357 if (cloud_out.size () != cloud_out.width * cloud_out.height)
358 cloud_out.resize (cloud_out.width * cloud_out.height);
359 cloud_out.is_dense = cloud_in.is_dense;
360 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
361 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
362
363 if (border_type == pcl::BORDER_TRANSPARENT)
364 {
365 const PointT* in = &(cloud_in[0]);
366 PointT* out = &(cloud_out[0]);
367 PointT* out_inner = out + cloud_out.width*top + left;
368 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
369 {
370 if (out_inner != in)
371 memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
372 }
373 }
374 else
375 {
376 // Copy the data
377 if (border_type != pcl::BORDER_CONSTANT)
378 {
379 try
380 {
381 std::vector<int> padding (cloud_out.width - cloud_in.width);
382 int right = cloud_out.width - cloud_in.width - left;
383 int bottom = cloud_out.height - cloud_in.height - top;
384
385 for (int i = 0; i < left; i++)
386 padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
387
388 for (int i = 0; i < right; i++)
389 padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
390
391 const PointT* in = &(cloud_in[0]);
392 PointT* out = &(cloud_out[0]);
393 PointT* out_inner = out + cloud_out.width*top + left;
394
395 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
396 {
397 if (out_inner != in)
398 memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
399
400 for (int j = 0; j < left; j++)
401 out_inner[j - left] = in[padding[j]];
402
403 for (int j = 0; j < right; j++)
404 out_inner[j + cloud_in.width] = in[padding[j + left]];
405 }
406
407 for (int i = 0; i < top; i++)
408 {
409 int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
410 memcpy (out + i*cloud_out.width,
411 out + (j+top) * cloud_out.width,
412 sizeof (PointT) * cloud_out.width);
413 }
414
415 for (int i = 0; i < bottom; i++)
416 {
417 int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
418 memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
419 out + (j+top)*cloud_out.width,
420 cloud_out.width * sizeof (PointT));
421 }
422 }
424 {
425 PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
426 }
427 }
428 else
429 {
430 int right = cloud_out.width - cloud_in.width - left;
431 int bottom = cloud_out.height - cloud_in.height - top;
432 std::vector<PointT> buff (cloud_out.width, value);
433 PointT* buff_ptr = &(buff[0]);
434 const PointT* in = &(cloud_in[0]);
435 PointT* out = &(cloud_out[0]);
436 PointT* out_inner = out + cloud_out.width*top + left;
437
438 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
439 {
440 if (out_inner != in)
441 memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
442
443 memcpy (out_inner - left, buff_ptr, left * sizeof (PointT));
444 memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT));
445 }
446
447 for (int i = 0; i < top; i++)
448 {
449 memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT));
450 }
451
452 for (int i = 0; i < bottom; i++)
453 {
454 memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
455 buff_ptr,
456 cloud_out.width * sizeof (PointT));
457 }
458 }
459 }
460 }
461}
462
463} // namespace pcl
464
An exception that is thrown when the arguments number or type is wrong/unhandled.
Definition exceptions.h:256
PointCloud represents the base class in PCL for storing collections of 3D points.
void reserve(std::size_t n)
void resize(std::size_t count)
Resizes the container to contain count elements.
std::size_t size() const
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
void clear()
Removes all points in a cloud and sets the width and height to 0.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
pcl::PCLHeader header
The point cloud header.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
bool empty() const
void transient_push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Defines all the PCL implemented PointT point type structures.
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
Definition io.hpp:109
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.
Definition io.hpp:305
std::vector< pcl::PCLPointField > getFields()
Get the list of available fields (i.e., dimension/channel)
Definition io.hpp:99
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.
Definition io.hpp:144
void copyPointCloudMemcpy(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Definition io.hpp:124
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition io.hpp:54
InterpolationType
Definition io.h:222
@ BORDER_TRANSPARENT
Definition io.h:225
@ BORDER_CONSTANT
Definition io.h:223
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
Definition types.h:128
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
void for_each_type(F f)
A point structure representing Euclidean xyz coordinates, and the RGB color.