Point Cloud Library (PCL) 1.13.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
48namespace pcl
49{
50
51template <typename PointT> int
53 const std::string &field_name,
54 std::vector<pcl::PCLPointField> &fields)
55{
56 return getFieldIndex<PointT>(field_name, fields);
57}
58
59
60template <typename PointT> int
61getFieldIndex (const std::string &field_name,
62 std::vector<pcl::PCLPointField> &fields)
63{
64 fields = getFields<PointT> ();
65 const auto& ref = fields;
66 return pcl::getFieldIndex<PointT> (field_name, ref);
67}
68
69
70template <typename PointT> int
71getFieldIndex (const std::string &field_name,
72 const std::vector<pcl::PCLPointField> &fields)
73{
74 const auto result = std::find_if(fields.begin (), fields.end (),
75 [&field_name](const auto& field) { return field.name == field_name; });
76 if (result == fields.end ())
77 return -1;
78 return std::distance(fields.begin (), result);
79}
80
81
82template <typename PointT> void
83getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
84{
85 fields = getFields<PointT> ();
86}
87
88
89template <typename PointT> void
90getFields (std::vector<pcl::PCLPointField> &fields)
91{
92 fields = getFields<PointT> ();
93}
94
95
96template <typename PointT> std::vector<pcl::PCLPointField>
98{
99 std::vector<pcl::PCLPointField> fields;
100 // Get the fields list
101 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
102 return fields;
103}
104
105
106template <typename PointT> std::string
108{
109 // Get the fields list
110 const auto fields = getFields<PointT>();
111 std::string result;
112 for (std::size_t i = 0; i < fields.size () - 1; ++i)
113 result += fields[i].name + " ";
114 result += fields[fields.size () - 1].name;
115 return (result);
116}
117
118namespace detail
119{
120
121 template <typename PointInT, typename PointOutT> void
124 {
125 // Iterate over each point, if the point types of two clouds are different
126 for (std::size_t i = 0; i < cloud_in.size (); ++i)
127 copyPoint (cloud_in[i], cloud_out[i]);
128 }
129
130
131 template <typename PointT> void
133 pcl::PointCloud<PointT> &cloud_out)
134 {
135 // Use std::copy directly, if the point types of two clouds are same
136 std::copy (&cloud_in[0], (&cloud_in[0]) + cloud_in.size (), &cloud_out[0]);
137 }
138
139} // namespace detail
140
141template <typename PointInT, typename PointOutT> void
144{
145 // Allocate enough space and copy the basics
146 cloud_out.header = cloud_in.header;
147 cloud_out.width = cloud_in.width;
148 cloud_out.height = cloud_in.height;
149 cloud_out.is_dense = cloud_in.is_dense;
150 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
151 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
152 cloud_out.resize (cloud_in.size ());
153
154 if (!cloud_in.empty ())
155 detail::copyPointCloudMemcpy (cloud_in, cloud_out);
156}
157
158
159template <typename PointT, typename IndicesVectorAllocator> void
162 pcl::PointCloud<PointT> &cloud_out)
163{
164 // Do we want to copy everything?
165 if (indices.size () == cloud_in.size ())
166 {
167 cloud_out = cloud_in;
168 return;
169 }
170
171 // Allocate enough space and copy the basics
172 cloud_out.clear ();
173 cloud_out.reserve (indices.size ());
174 cloud_out.header = cloud_in.header;
175 cloud_out.width = indices.size ();
176 cloud_out.height = 1;
177 cloud_out.is_dense = cloud_in.is_dense;
178 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
179 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
180
181 // Iterate over each point
182 for (const auto& index : indices)
183 cloud_out.transient_push_back (cloud_in[index]);
184}
185
186
187template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
191{
192 // Allocate enough space and copy the basics
193 cloud_out.resize (indices.size ());
194 cloud_out.header = cloud_in.header;
195 cloud_out.width = indices.size ();
196 cloud_out.height = 1;
197 cloud_out.is_dense = cloud_in.is_dense;
198 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
199 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
200
201 // Iterate over each point
202 for (std::size_t i = 0; i < indices.size (); ++i)
203 copyPoint (cloud_in[indices[i]], cloud_out[i]);
204}
205
206
207template <typename PointT> void
209 const pcl::PointIndices &indices,
210 pcl::PointCloud<PointT> &cloud_out)
211{
212 copyPointCloud (cloud_in, indices.indices, cloud_out);
213}
214
215
216template <typename PointInT, typename PointOutT> void
218 const pcl::PointIndices &indices,
220{
221 copyPointCloud (cloud_in, indices.indices, cloud_out);
222}
223
224
225template <typename PointT> void
227 const std::vector<pcl::PointIndices> &indices,
228 pcl::PointCloud<PointT> &cloud_out)
229{
230 std::size_t nr_p = 0;
231 for (const auto &index : indices)
232 nr_p += index.indices.size ();
233
234 // Do we want to copy everything? Remember we assume UNIQUE indices
235 if (nr_p == cloud_in.size ())
236 {
237 cloud_out = cloud_in;
238 return;
239 }
240
241 // Allocate enough space and copy the basics
242 cloud_out.clear ();
243 cloud_out.reserve (nr_p);
244 cloud_out.header = cloud_in.header;
245 cloud_out.width = nr_p;
246 cloud_out.height = 1;
247 cloud_out.is_dense = cloud_in.is_dense;
248 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
249 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
250
251 // Iterate over each cluster
252 for (const auto &cluster_index : indices)
253 {
254 // Iterate over each idx
255 for (const auto &index : cluster_index.indices)
256 {
257 // Iterate over each dimension
258 cloud_out.transient_push_back (cloud_in[index]);
259 }
260 }
261}
262
263
264template <typename PointInT, typename PointOutT> void
266 const std::vector<pcl::PointIndices> &indices,
268{
269 const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
270 [](const auto& acc, const auto& index) { return index.indices.size() + acc; });
271
272 // Do we want to copy everything? Remember we assume UNIQUE indices
273 if (nr_p == cloud_in.size ())
274 {
275 copyPointCloud (cloud_in, cloud_out);
276 return;
277 }
278
279 // Allocate enough space and copy the basics
280 cloud_out.resize (nr_p);
281 cloud_out.header = cloud_in.header;
282 cloud_out.width = nr_p;
283 cloud_out.height = 1;
284 cloud_out.is_dense = cloud_in.is_dense;
285 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
286 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
287
288 // Iterate over each cluster
289 std::size_t cp = 0;
290 for (const auto &cluster_index : indices)
291 {
292 // Iterate over each idx
293 for (const auto &index : cluster_index.indices)
294 {
295 copyPoint (cloud_in[index], cloud_out[cp]);
296 ++cp;
297 }
298 }
299}
300
301
302template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
304 const pcl::PointCloud<PointIn2T> &cloud2_in,
306{
307 using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
308 using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
309
310 if (cloud1_in.size () != cloud2_in.size ())
311 {
312 PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
313 return;
314 }
315
316 // Resize the output dataset
317 cloud_out.resize (cloud1_in.size ());
318 cloud_out.header = cloud1_in.header;
319 cloud_out.width = cloud1_in.width;
320 cloud_out.height = cloud1_in.height;
321 if (!cloud1_in.is_dense || !cloud2_in.is_dense)
322 cloud_out.is_dense = false;
323 else
324 cloud_out.is_dense = true;
325
326 // Iterate over each point
327 for (std::size_t i = 0; i < cloud_out.size (); ++i)
328 {
329 // Iterate over each dimension
330 pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in[i], cloud_out[i]));
331 pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in[i], cloud_out[i]));
332 }
333}
334
335
336template <typename PointT> void
338 int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
339{
340 if (top < 0 || left < 0 || bottom < 0 || right < 0)
341 {
342 std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
343 PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
344 return;
345 }
346
347 if (top == 0 && left == 0 && bottom == 0 && right == 0)
348 cloud_out = cloud_in;
349 else
350 {
351 // Allocate enough space and copy the basics
352 cloud_out.header = cloud_in.header;
353 cloud_out.width = cloud_in.width + left + right;
354 cloud_out.height = cloud_in.height + top + bottom;
355 if (cloud_out.size () != cloud_out.width * cloud_out.height)
356 cloud_out.resize (cloud_out.width * cloud_out.height);
357 cloud_out.is_dense = cloud_in.is_dense;
358 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
359 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
360
361 if (border_type == pcl::BORDER_TRANSPARENT)
362 {
363 const PointT* in = &(cloud_in[0]);
364 PointT* out = &(cloud_out[0]);
365 PointT* out_inner = out + cloud_out.width*top + left;
366 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
367 {
368 if (out_inner != in) {
369 std::copy(in, in + cloud_in.width, out_inner);
370 }
371 }
372 }
373 else
374 {
375 // Copy the data
376 if (border_type != pcl::BORDER_CONSTANT)
377 {
378 try
379 {
380 std::vector<int> padding (cloud_out.width - cloud_in.width);
381 int right = cloud_out.width - cloud_in.width - left;
382 int bottom = cloud_out.height - cloud_in.height - top;
383
384 for (int i = 0; i < left; i++)
385 padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
386
387 for (int i = 0; i < right; i++)
388 padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
389
390 const PointT* in = &(cloud_in[0]);
391 PointT* out = &(cloud_out[0]);
392 PointT* out_inner = out + cloud_out.width*top + left;
393
394 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
395 {
396 if (out_inner != in) {
397 std::copy(in, in + cloud_in.width, out_inner);
398 }
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 std::copy(out + (j+top) * cloud_out.width, out + (j+top) * cloud_out.width + cloud_out.width,
411 out + i*cloud_out.width);
412 }
413
414 for (int i = 0; i < bottom; i++)
415 {
416 int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
417 std::copy(out + (j+top)*cloud_out.width, out + (j+top)*cloud_out.width + cloud_out.width,
418 out + (i + cloud_in.height + top)*cloud_out.width);
419 }
420 }
422 {
423 PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
424 }
425 }
426 else
427 {
428 int right = cloud_out.width - cloud_in.width - left;
429 int bottom = cloud_out.height - cloud_in.height - top;
430 std::vector<PointT> buff (cloud_out.width, value);
431 PointT* buff_ptr = &(buff[0]);
432 const PointT* in = &(cloud_in[0]);
433 PointT* out = &(cloud_out[0]);
434 PointT* out_inner = out + cloud_out.width*top + left;
435
436 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
437 {
438 if (out_inner != in) {
439 std::copy(in, in + cloud_in.width, out_inner);
440 }
441
442 std::copy(buff_ptr, buff_ptr + left, out_inner - left);
443 std::copy(buff_ptr, buff_ptr + right, out_inner + cloud_in.width);
444 }
445
446 for (int i = 0; i < top; i++)
447 {
448 std::copy(buff_ptr, buff_ptr + cloud_out.width, out + i*cloud_out.width);
449 }
450
451 for (int i = 0; i < bottom; i++)
452 {
453 std::copy(buff_ptr, buff_ptr + cloud_out.width,
454 out + (i + cloud_in.height + top)*cloud_out.width);
455 }
456 }
457 }
458 }
459}
460
461} // namespace pcl
462
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.
bool empty() const
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.
std::size_t size() const
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
void reserve(std::size_t n)
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
Definition io.hpp:107
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:303
std::vector< pcl::PCLPointField > getFields()
Get the list of available fields (i.e., dimension/channel)
Definition io.hpp:97
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:142
void copyPointCloudMemcpy(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Definition io.hpp:122
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition io.hpp:52
InterpolationType
Definition io.h:247
@ BORDER_TRANSPARENT
Definition io.h:250
@ BORDER_CONSTANT
Definition io.h:248
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
Definition types.h:128
A point structure representing Euclidean xyz coordinates, and the RGB color.