Point Cloud Library (PCL) 1.15.1-dev
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
48#include <cmath> // for std::round
49#include <cstring> // for memcpy
50#include <limits>
51#include <type_traits>
52
53namespace pcl
54{
55
56template <typename PointT> int
58 const std::string &field_name,
59 std::vector<pcl::PCLPointField> &fields)
60{
61 return getFieldIndex<PointT>(field_name, fields);
62}
63
64
65template <typename PointT> int
66getFieldIndex (const std::string &field_name,
67 std::vector<pcl::PCLPointField> &fields)
68{
69 fields = getFields<PointT> ();
70 const auto& ref = fields;
71 return pcl::getFieldIndex<PointT> (field_name, ref);
72}
73
74
75template <typename PointT> int
76getFieldIndex (const std::string &field_name,
77 const std::vector<pcl::PCLPointField> &fields)
78{
79 const auto result = std::find_if(fields.begin (), fields.end (),
80 [&field_name](const auto& field) { return field.name == field_name; });
81 if (result == fields.end ())
82 return -1;
83 return std::distance(fields.begin (), result);
84}
85
86
87template <typename PointT> void
88getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields)
89{
90 fields = getFields<PointT> ();
91}
92
93
94template <typename PointT> void
95getFields (std::vector<pcl::PCLPointField> &fields)
96{
97 fields = getFields<PointT> ();
98}
99
100
101template <typename PointT> std::vector<pcl::PCLPointField>
103{
104 std::vector<pcl::PCLPointField> fields;
105 // Get the fields list
106 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
107 return fields;
108}
109
110template <typename CloudT> inline std::vector<pcl::PCLPointField>
111getFields (const CloudT& /*cloud*/)
112{
113 return pcl::getFields<typename CloudT::PointType>();
114}
115
116// Template specialization for PCLPointCloud2
117template <> inline std::vector<pcl::PCLPointField>
119 return cloud.fields;
120}
121
122
123template <typename PointT> std::string
125{
126 // Get the fields list
127 const auto fields = getFields<PointT>();
128 std::string result;
129 for (std::size_t i = 0; i < fields.size () - 1; ++i)
130 result += fields[i].name + " ";
131 result += fields[fields.size () - 1].name;
132 return (result);
133}
134
135/**
136 * \brief reinterpret a pointer as a type, dereference it, and return result as target
137 * type
138 *
139 * \tparam DType Pointer type of the data
140 * \tparam RType Return type which the dereferenced result is cast to
141 *
142 * \return value
143 */
144template <typename DType, typename RType> RType reinterpret_and_cast (const std::uint8_t* p)
145{
146 // memcpy instead of dereferencing p as a DType*: the fields of a PCLPointCloud2 need
147 // not be aligned (e.g. a double right after a 1 byte field)
148 DType value;
149 memcpy (&value, p, sizeof (DType));
150 return static_cast<RType>(value);
151}
152
153template <typename T> T point_field_as (const std::uint8_t* data, const std::uint8_t type)
154{
155 switch (type) {
157 return reinterpret_and_cast<float, T>(data);
158 break;
160 return reinterpret_and_cast<std::int8_t, T>(data);
161 break;
163 return reinterpret_and_cast<std::uint8_t, T>(data);
164 break;
166 return reinterpret_and_cast<std::uint16_t, T>(data);
167 break;
169 return reinterpret_and_cast<std::uint32_t, T>(data);
170 break;
172 return reinterpret_and_cast<std::uint64_t, T>(data);
173 break;
175 return reinterpret_and_cast<bool, T>(data);
176 break;
178 return reinterpret_and_cast<double, T>(data);
179 break;
181 return reinterpret_and_cast<std::int16_t, T>(data);
182 break;
184 return reinterpret_and_cast<std::int32_t, T>(data);
185 break;
187 return reinterpret_and_cast<std::int64_t, T>(data);
188 break;
189 default:
190 return 0;
191 break;
192 }
193}
194
195/**
196 * \brief convert a value to a type and store it at a pointer. Integer types are rounded
197 * to the nearest value and saturated to the range of the type (64 bit integers are not
198 * exact in double).
199 *
200 * \tparam DType Type the value is stored as
201 */
202template <typename DType> void storeFieldValueAs (const double value, std::uint8_t* p)
203{
204 DType result;
205 if constexpr (std::is_floating_point_v<DType>)
206 result = static_cast<DType>(value);
207 else if (!(value > static_cast<double>(std::numeric_limits<DType>::lowest())))
208 result = std::numeric_limits<DType>::lowest();
209 else if (value >= static_cast<double>(std::numeric_limits<DType>::max()))
210 result = std::numeric_limits<DType>::max();
211 else
212 result = static_cast<DType>(std::round(value));
213 memcpy (p, &result, sizeof (DType));
214}
215
216inline void storeFieldValue (const double value, std::uint8_t* data, const std::uint8_t type)
217{
218 switch (type) {
220 storeFieldValueAs<float>(value, data);
221 break;
223 storeFieldValueAs<std::int8_t>(value, data);
224 break;
226 storeFieldValueAs<std::uint8_t>(value, data);
227 break;
229 storeFieldValueAs<std::uint16_t>(value, data);
230 break;
232 storeFieldValueAs<std::uint32_t>(value, data);
233 break;
235 storeFieldValueAs<std::uint64_t>(value, data);
236 break;
238 storeFieldValueAs<bool>(value, data);
239 break;
241 storeFieldValueAs<double>(value, data);
242 break;
244 storeFieldValueAs<std::int16_t>(value, data);
245 break;
247 storeFieldValueAs<std::int32_t>(value, data);
248 break;
250 storeFieldValueAs<std::int64_t>(value, data);
251 break;
252 default:
253 break;
254 }
255}
256
257
258namespace detail
259{
260
261 template <typename PointInT, typename PointOutT> void
264 {
265 // Iterate over each point, if the point types of two clouds are different
266 for (std::size_t i = 0; i < cloud_in.size (); ++i)
267 copyPoint (cloud_in[i], cloud_out[i]);
268 }
269
270
271 template <typename PointT> void
273 pcl::PointCloud<PointT> &cloud_out)
274 {
275 // Use std::copy directly, if the point types of two clouds are same
276 std::copy (cloud_in.data(), cloud_in.data() + cloud_in.size (), cloud_out.data());
277 }
278
279} // namespace detail
280
281template <typename PointInT, typename PointOutT> void
284{
285 // Allocate enough space and copy the basics
286 cloud_out.header = cloud_in.header;
287 cloud_out.width = cloud_in.width;
288 cloud_out.height = cloud_in.height;
289 cloud_out.is_dense = cloud_in.is_dense;
290 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
291 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
292 cloud_out.resize (cloud_in.size ());
293
294 if (!cloud_in.empty ())
295 detail::copyPointCloudMemcpy (cloud_in, cloud_out);
296}
297
298
299template <typename PointT, typename IndicesVectorAllocator> void
302 pcl::PointCloud<PointT> &cloud_out)
303{
304 // Do we want to copy everything?
305 if (indices.size () == cloud_in.size ())
306 {
307 cloud_out = cloud_in;
308 return;
309 }
310
311 // Allocate enough space and copy the basics
312 cloud_out.clear ();
313 cloud_out.reserve (indices.size ());
314 cloud_out.header = cloud_in.header;
315 cloud_out.width = indices.size ();
316 cloud_out.height = 1;
317 cloud_out.is_dense = cloud_in.is_dense;
318 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
319 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
320
321 // Iterate over each point
322 for (const auto& index : indices)
323 cloud_out.transient_push_back (cloud_in[index]);
324}
325
326
327template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator> void
331{
332 // Allocate enough space and copy the basics
333 cloud_out.resize (indices.size ());
334 cloud_out.header = cloud_in.header;
335 cloud_out.width = indices.size ();
336 cloud_out.height = 1;
337 cloud_out.is_dense = cloud_in.is_dense;
338 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
339 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
340
341 // Iterate over each point
342 for (std::size_t i = 0; i < indices.size (); ++i)
343 copyPoint (cloud_in[indices[i]], cloud_out[i]);
344}
345
346
347template <typename PointT> void
349 const pcl::PointIndices &indices,
350 pcl::PointCloud<PointT> &cloud_out)
351{
352 copyPointCloud (cloud_in, indices.indices, cloud_out);
353}
354
355
356template <typename PointInT, typename PointOutT> void
358 const pcl::PointIndices &indices,
360{
361 copyPointCloud (cloud_in, indices.indices, cloud_out);
362}
363
364
365template <typename PointT> void
367 const std::vector<pcl::PointIndices> &indices,
368 pcl::PointCloud<PointT> &cloud_out)
369{
370 std::size_t nr_p = 0;
371 for (const auto &index : indices)
372 nr_p += index.indices.size ();
373
374 // Do we want to copy everything? Remember we assume UNIQUE indices
375 if (nr_p == cloud_in.size ())
376 {
377 cloud_out = cloud_in;
378 return;
379 }
380
381 // Allocate enough space and copy the basics
382 cloud_out.clear ();
383 cloud_out.reserve (nr_p);
384 cloud_out.header = cloud_in.header;
385 cloud_out.width = nr_p;
386 cloud_out.height = 1;
387 cloud_out.is_dense = cloud_in.is_dense;
388 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
389 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
390
391 // Iterate over each cluster
392 for (const auto &cluster_index : indices)
393 {
394 // Iterate over each idx
395 for (const auto &index : cluster_index.indices)
396 {
397 // Iterate over each dimension
398 cloud_out.transient_push_back (cloud_in[index]);
399 }
400 }
401}
402
403
404template <typename PointInT, typename PointOutT> void
406 const std::vector<pcl::PointIndices> &indices,
408{
409 const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0,
410 [](const auto& acc, const auto& index) { return index.indices.size() + acc; });
411
412 // Do we want to copy everything? Remember we assume UNIQUE indices
413 if (nr_p == cloud_in.size ())
414 {
415 copyPointCloud (cloud_in, cloud_out);
416 return;
417 }
418
419 // Allocate enough space and copy the basics
420 cloud_out.resize (nr_p);
421 cloud_out.header = cloud_in.header;
422 cloud_out.width = nr_p;
423 cloud_out.height = 1;
424 cloud_out.is_dense = cloud_in.is_dense;
425 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
426 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
427
428 // Iterate over each cluster
429 std::size_t cp = 0;
430 for (const auto &cluster_index : indices)
431 {
432 // Iterate over each idx
433 for (const auto &index : cluster_index.indices)
434 {
435 copyPoint (cloud_in[index], cloud_out[cp]);
436 ++cp;
437 }
438 }
439}
440
441
442template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
444 const pcl::PointCloud<PointIn2T> &cloud2_in,
446{
447 using FieldList1 = typename pcl::traits::fieldList<PointIn1T>::type;
448 using FieldList2 = typename pcl::traits::fieldList<PointIn2T>::type;
449
450 if (cloud1_in.size () != cloud2_in.size ())
451 {
452 PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n");
453 return;
454 }
455
456 // Resize the output dataset
457 cloud_out.resize (cloud1_in.size ());
458 cloud_out.header = cloud1_in.header;
459 cloud_out.width = cloud1_in.width;
460 cloud_out.height = cloud1_in.height;
461 if (!cloud1_in.is_dense || !cloud2_in.is_dense)
462 cloud_out.is_dense = false;
463 else
464 cloud_out.is_dense = true;
465
466 // Iterate over each point
467 for (std::size_t i = 0; i < cloud_out.size (); ++i)
468 {
469 // Iterate over each dimension
470 pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in[i], cloud_out[i]));
471 pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in[i], cloud_out[i]));
472 }
473}
474
475
476template <typename PointT> void
478 int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
479{
480 if (top < 0 || left < 0 || bottom < 0 || right < 0)
481 {
482 std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
483 PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
484 return;
485 }
486
487 if (top == 0 && left == 0 && bottom == 0 && right == 0)
488 cloud_out = cloud_in;
489 else
490 {
491 // Allocate enough space and copy the basics
492 cloud_out.header = cloud_in.header;
493 cloud_out.width = cloud_in.width + left + right;
494 cloud_out.height = cloud_in.height + top + bottom;
495 if (cloud_out.size () != cloud_out.width * cloud_out.height)
496 cloud_out.resize (cloud_out.width * cloud_out.height);
497 cloud_out.is_dense = cloud_in.is_dense;
498 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
499 cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
500
501 if (border_type == pcl::BORDER_TRANSPARENT)
502 {
503 const PointT* in = cloud_in.data();
504 PointT* out = cloud_out.data();
505 PointT* out_inner = out + cloud_out.width*top + left;
506 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
507 {
508 if (out_inner != in) {
509 std::copy(in, in + cloud_in.width, out_inner);
510 }
511 }
512 }
513 else
514 {
515 // Copy the data
516 if (border_type != pcl::BORDER_CONSTANT)
517 {
518 try
519 {
520 std::vector<int> padding (cloud_out.width - cloud_in.width);
521 int right = cloud_out.width - cloud_in.width - left;
522 int bottom = cloud_out.height - cloud_in.height - top;
523
524 for (int i = 0; i < left; i++)
525 padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
526
527 for (int i = 0; i < right; i++)
528 padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
529
530 const PointT* in = cloud_in.data();
531 PointT* out = cloud_out.data();
532 PointT* out_inner = out + cloud_out.width*top + left;
533
534 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
535 {
536 if (out_inner != in) {
537 std::copy(in, in + cloud_in.width, out_inner);
538 }
539
540 for (int j = 0; j < left; j++)
541 out_inner[j - left] = in[padding[j]];
542
543 for (int j = 0; j < right; j++)
544 out_inner[j + cloud_in.width] = in[padding[j + left]];
545 }
546
547 for (int i = 0; i < top; i++)
548 {
549 int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
550 std::copy(out + (j+top) * cloud_out.width, out + (j+top) * cloud_out.width + cloud_out.width,
551 out + i*cloud_out.width);
552 }
553
554 for (int i = 0; i < bottom; i++)
555 {
556 int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
557 std::copy(out + (j+top)*cloud_out.width, out + (j+top)*cloud_out.width + cloud_out.width,
558 out + (i + cloud_in.height + top)*cloud_out.width);
559 }
560 }
562 {
563 PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
564 }
565 }
566 else
567 {
568 int right = cloud_out.width - cloud_in.width - left;
569 int bottom = cloud_out.height - cloud_in.height - top;
570 std::vector<PointT> buff (cloud_out.width, value);
571 PointT* buff_ptr = buff.data();
572 const PointT* in = cloud_in.data();
573 PointT* out = cloud_out.data();
574 PointT* out_inner = out + cloud_out.width*top + left;
575
576 for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
577 {
578 if (out_inner != in) {
579 std::copy(in, in + cloud_in.width, out_inner);
580 }
581
582 std::copy(buff_ptr, buff_ptr + left, out_inner - left);
583 std::copy(buff_ptr, buff_ptr + right, out_inner + cloud_in.width);
584 }
585
586 for (int i = 0; i < top; i++)
587 {
588 std::copy(buff_ptr, buff_ptr + cloud_out.width, out + i*cloud_out.width);
589 }
590
591 for (int i = 0; i < bottom; i++)
592 {
593 std::copy(buff_ptr, buff_ptr + cloud_out.width,
594 out + (i + cloud_in.height + top)*cloud_out.width);
595 }
596 }
597 }
598 }
599}
600
601} // namespace pcl
An exception that is thrown when the arguments number or type is wrong/unhandled.
Definition exceptions.h:259
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.
PointT * data() noexcept
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)
T point_field_as(const std::uint8_t *data, const std::uint8_t type)
Get the value of a point field from raw data pointer and field type.
Definition io.hpp:153
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
Definition io.hpp:124
void storeFieldValue(const double value, std::uint8_t *data, const std::uint8_t type)
Set the value of a point field from raw data pointer and field type.
Definition io.hpp:216
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:443
std::vector< pcl::PCLPointField > getFields()
Get the list of available fields (i.e., dimension/channel)
Definition io.hpp:102
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:282
void copyPointCloudMemcpy(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Definition io.hpp:262
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition io.hpp:57
InterpolationType
Definition io.h:290
@ BORDER_TRANSPARENT
Definition io.h:293
@ BORDER_CONSTANT
Definition io.h:291
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
void storeFieldValueAs(const double value, std::uint8_t *p)
convert a value to a type and store it at a pointer.
Definition io.hpp:202
std::vector< pcl::PCLPointField > getFields< pcl::PCLPointCloud2 >(const pcl::PCLPointCloud2 &cloud)
Definition io.hpp:118
RType reinterpret_and_cast(const std::uint8_t *p)
reinterpret a pointer as a type, dereference it, and return result as target type
Definition io.hpp:144
std::vector<::pcl::PCLPointField > fields
A point structure representing Euclidean xyz coordinates, and the RGB color.