Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
io.h
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 <numeric>
44#include <string>
45
46#include <pcl/point_cloud.h>
47#include <pcl/PointIndices.h>
48#include <pcl/pcl_macros.h>
49#include <pcl/PolygonMesh.h>
50#include <locale>
51
52namespace pcl
53{
54 /** \brief Get the index of a specified field (i.e., dimension/channel)
55 * \param[in] cloud the point cloud message
56 * \param[in] field_name the string defining the field name
57 * \return the index of the field or a negative integer if no field with the given name exists
58 * \ingroup common
59 */
60 inline int
61 getFieldIndex (const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
62 {
63 // Get the index we need
64 const auto result = std::find_if(cloud.fields.begin (), cloud.fields.end (),
65 [&field_name](const auto field) { return field.name == field_name; });
66 if (result == cloud.fields.end ())
67 return -1;
68 return std::distance(cloud.fields.begin (), result);
69 }
70
71 /** \brief Get the index of a specified field (i.e., dimension/channel)
72 * \tparam PointT datatype for which fields is being queries
73 * \param[in] field_name the string defining the field name
74 * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
75 * \return the index of the field or a negative integer if no field with the given name exists
76 * \ingroup common
77 */
78 template <typename PointT> inline int
79 getFieldIndex (const std::string &field_name,
80 std::vector<pcl::PCLPointField> &fields);
81 /** \brief Get the index of a specified field (i.e., dimension/channel)
82 * \tparam PointT datatype for which fields is being queries
83 * \param[in] field_name the string defining the field name
84 * \param[in] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains
85 * \ingroup common
86 */
87 template <typename PointT> inline int
88 getFieldIndex (const std::string &field_name,
89 const std::vector<pcl::PCLPointField> &fields);
90
91 /** \brief Get the list of available fields (i.e., dimension/channel)
92 * \tparam PointT datatype whose details are requested
93 * \ingroup common
94 */
95 template <typename PointT> inline std::vector<pcl::PCLPointField>
96 getFields ();
97
98 /** \brief Get the list of all fields available in a given cloud
99 * \param[in] cloud the point cloud message
100 * \ingroup common
101 */
102 template <typename PointT> inline std::string
104
105 /** \brief Get the available point cloud fields as a space separated string
106 * \param[in] cloud a pointer to the PointCloud message
107 * \ingroup common
108 */
109 inline std::string
111 {
112 return std::accumulate(std::next (cloud.fields.begin ()), cloud.fields.end (), cloud.fields[0].name,
113 [](const auto& acc, const auto& field) { return acc + " " + field.name; });
114 }
115
116 /** \brief Obtains the size of a specific field data type in bytes
117 * \param[in] datatype the field data type (see PCLPointField.h)
118 * \ingroup common
119 */
120 inline int
121 getFieldSize (const int datatype)
122 {
123 switch (datatype)
124 {
126 return sizeof(bool);
127
130 return (1);
131
134 return (2);
135
139 return (4);
140
144 return (8);
145
146 default:
147 return (0);
148 }
149 }
150
151 /** \brief Obtain a vector with the sizes of all valid fields (e.g., not "_")
152 * \param[in] fields the input vector containing the fields
153 * \param[out] field_sizes the resultant field sizes in bytes
154 */
155 PCL_EXPORTS void
156 getFieldsSizes (const std::vector<pcl::PCLPointField> &fields,
157 std::vector<int> &field_sizes);
158
159 /** \brief Obtains the type of the PCLPointField from a specific size and type
160 * \param[in] size the size in bytes of the data field
161 * \param[in] type a char describing the type of the field ('B' = bool, 'F' = float, 'I' = signed, 'U' = unsigned)
162 * \ingroup common
163 */
164 inline int
165 getFieldType (const int size, char type)
166 {
167 type = std::toupper (type, std::locale::classic ());
168
169 // extra logic for bool because its size is undefined
170 if (type == 'B') {
171 if (size == sizeof(bool)) {
173 } else {
174 return -1;
175 }
176 }
177
178 switch (size)
179 {
180 case 1:
181 if (type == 'I')
183 if (type == 'U')
185 break;
186
187 case 2:
188 if (type == 'I')
190 if (type == 'U')
192 break;
193
194 case 4:
195 if (type == 'I')
197 if (type == 'U')
199 if (type == 'F')
201 break;
202
203 case 8:
204 if (type == 'I')
206 if (type == 'U')
208 if (type == 'F')
210 break;
211 }
212 return (-1);
213 }
214
215 /** \brief Obtains the type of the PCLPointField from a specific PCLPointField as a char
216 * \param[in] type the PCLPointField field type
217 * \ingroup common
218 */
219 inline char
220 getFieldType (const int type)
221 {
222 switch (type)
223 {
225 return ('B');
226
231 return ('I');
232
237 return ('U');
238
241 return ('F');
242
243 default:
244 return ('?');
245 }
246 }
247
255
256 /** \brief \return the right index according to the interpolation type.
257 * \note this is adapted from OpenCV
258 * \param p the index of point to interpolate
259 * \param length the top/bottom row or left/right column index
260 * \param type the requested interpolation
261 * \throws pcl::BadArgumentException if type is unknown
262 */
263 PCL_EXPORTS int
264 interpolatePointIndex (int p, int length, InterpolationType type);
265
266 /** \brief Concatenate two pcl::PointCloud<PointT>
267 * \param[in] cloud1 the first input point cloud dataset
268 * \param[in] cloud2 the second input point cloud dataset
269 * \param[out] cloud_out the resultant output point cloud dataset
270 * \return true if successful, false otherwise
271 * \ingroup common
272 */
273 template <typename PointT>
274 PCL_EXPORTS bool
276 const pcl::PointCloud<PointT> &cloud2,
277 pcl::PointCloud<PointT> &cloud_out)
278 {
279 return pcl::PointCloud<PointT>::concatenate(cloud1, cloud2, cloud_out);
280 }
281
282 /** \brief Concatenate two pcl::PCLPointCloud2
283 *
284 * \warning This function will concatenate IFF the non-skip fields are in the correct
285 * order and same in number.
286 * \param[in] cloud1 the first input point cloud dataset
287 * \param[in] cloud2 the second input point cloud dataset
288 * \param[out] cloud_out the resultant output point cloud dataset
289 * \return true if successful, false otherwise
290 * \ingroup common
291 */
292 PCL_EXPORTS inline bool
294 const pcl::PCLPointCloud2 &cloud2,
295 pcl::PCLPointCloud2 &cloud_out)
296 {
297 return pcl::PCLPointCloud2::concatenate(cloud1, cloud2, cloud_out);
298 }
299
300 /** \brief Concatenate two pcl::PolygonMesh
301 * \param[in] mesh1 the first input mesh
302 * \param[in] mesh2 the second input mesh
303 * \param[out] mesh_out the resultant output mesh
304 * \return true if successful, false otherwise
305 * \ingroup common
306 */
307 PCL_EXPORTS inline bool
309 const pcl::PolygonMesh &mesh2,
310 pcl::PolygonMesh &mesh_out)
311 {
312 return pcl::PolygonMesh::concatenate(mesh1, mesh2, mesh_out);
313 }
314
315 /** \brief Extract the indices of a given point cloud as a new point cloud
316 * \param[in] cloud_in the input point cloud dataset
317 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
318 * \param[out] cloud_out the resultant output point cloud dataset
319 * \note Assumes unique indices.
320 * \ingroup common
321 */
322 PCL_EXPORTS void
324 const Indices &indices,
325 pcl::PCLPointCloud2 &cloud_out);
326
327 /** \brief Extract the indices of a given point cloud as a new point cloud
328 * \param[in] cloud_in the input point cloud dataset
329 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
330 * \param[out] cloud_out the resultant output point cloud dataset
331 * \note Assumes unique indices.
332 * \ingroup common
333 */
334 PCL_EXPORTS void
336 const IndicesAllocator< Eigen::aligned_allocator<index_t> > &indices,
337 pcl::PCLPointCloud2 &cloud_out);
338
339 /** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out
340 * \param[in] cloud_in the input point cloud dataset
341 * \param[out] cloud_out the resultant output point cloud dataset
342 * \ingroup common
343 */
344 PCL_EXPORTS void
346 pcl::PCLPointCloud2 &cloud_out);
347
348 /** \brief Check if two given point types are the same or not. */
349template <typename Point1T, typename Point2T> constexpr bool
351 {
352 return (std::is_same<remove_cvref_t<Point1T>, remove_cvref_t<Point2T>>::value);
353 }
354
355 /** \brief Extract the indices of a given point cloud as a new point cloud
356 * \param[in] cloud_in the input point cloud dataset
357 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
358 * \param[out] cloud_out the resultant output point cloud dataset
359 * \note Assumes unique indices.
360 * \ingroup common
361 */
362 template <typename PointT, typename IndicesVectorAllocator = std::allocator<index_t>> void
364 const IndicesAllocator< IndicesVectorAllocator> &indices,
365 pcl::PointCloud<PointT> &cloud_out);
366
367 /** \brief Extract the indices of a given point cloud as a new point cloud
368 * \param[in] cloud_in the input point cloud dataset
369 * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
370 * \param[out] cloud_out the resultant output point cloud dataset
371 * \note Assumes unique indices.
372 * \ingroup common
373 */
374 template <typename PointT> void
376 const PointIndices &indices,
377 pcl::PointCloud<PointT> &cloud_out);
378
379 /** \brief Extract the indices of a given point cloud as a new point cloud
380 * \param[in] cloud_in the input point cloud dataset
381 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
382 * \param[out] cloud_out the resultant output point cloud dataset
383 * \note Assumes unique indices.
384 * \ingroup common
385 */
386 template <typename PointT> void
388 const std::vector<pcl::PointIndices> &indices,
389 pcl::PointCloud<PointT> &cloud_out);
390
391 /** \brief Copy all the fields from a given point cloud into a new point cloud
392 * \param[in] cloud_in the input point cloud dataset
393 * \param[out] cloud_out the resultant output point cloud dataset
394 * \ingroup common
395 */
396 template <typename PointInT, typename PointOutT> void
398 pcl::PointCloud<PointOutT> &cloud_out);
399
400 /** \brief Extract the indices of a given point cloud as a new point cloud
401 * \param[in] cloud_in the input point cloud dataset
402 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in
403 * \param[out] cloud_out the resultant output point cloud dataset
404 * \note Assumes unique indices.
405 * \ingroup common
406 */
407 template <typename PointInT, typename PointOutT, typename IndicesVectorAllocator = std::allocator<index_t>> void
409 const IndicesAllocator<IndicesVectorAllocator> &indices,
410 pcl::PointCloud<PointOutT> &cloud_out);
411
412 /** \brief Extract the indices of a given point cloud as a new point cloud
413 * \param[in] cloud_in the input point cloud dataset
414 * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in
415 * \param[out] cloud_out the resultant output point cloud dataset
416 * \note Assumes unique indices.
417 * \ingroup common
418 */
419 template <typename PointInT, typename PointOutT> void
421 const PointIndices &indices,
422 pcl::PointCloud<PointOutT> &cloud_out);
423
424 /** \brief Extract the indices of a given point cloud as a new point cloud
425 * \param[in] cloud_in the input point cloud dataset
426 * \param[in] indices the vector of indices representing the points to be copied from cloud_in
427 * \param[out] cloud_out the resultant output point cloud dataset
428 * \note Assumes unique indices.
429 * \ingroup common
430 */
431 template <typename PointInT, typename PointOutT> void
433 const std::vector<pcl::PointIndices> &indices,
434 pcl::PointCloud<PointOutT> &cloud_out);
435
436 /** \brief Copy a point cloud inside a larger one interpolating borders.
437 * \param[in] cloud_in the input point cloud dataset
438 * \param[out] cloud_out the resultant output point cloud dataset
439 * \param top
440 * \param bottom
441 * \param left
442 * \param right
443 * Position of cloud_in inside cloud_out is given by \a top, \a left, \a bottom \a right.
444 * \param[in] border_type the interpolating method (pcl::BORDER_XXX)
445 * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh
446 * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb
447 * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba
448 * BORDER_WRAP: cdefgh|abcdefgh|abcdefg
449 * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i'
450 * BORDER_TRANSPARENT: mnopqr|abcdefgh|tuvwxyz where m-r and t-z are original values of cloud_out
451 * \param value
452 * \throw pcl::BadArgumentException if any of top, bottom, left or right is negative.
453 * \ingroup common
454 */
455 template <typename PointT> void
457 pcl::PointCloud<PointT> &cloud_out,
458 int top, int bottom, int left, int right,
459 pcl::InterpolationType border_type, const PointT& value);
460
461 /** \brief Concatenate two datasets representing different fields.
462 *
463 * \note If the input datasets have overlapping fields (i.e., both contain
464 * the same fields), then the data in the second cloud (cloud2_in) will
465 * overwrite the data in the first (cloud1_in).
466 *
467 * \param[in] cloud1_in the first input dataset
468 * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
469 * \param[out] cloud_out the resultant output dataset created by the concatenation of all the fields in the input datasets
470 * \ingroup common
471 */
472 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void
474 const pcl::PointCloud<PointIn2T> &cloud2_in,
475 pcl::PointCloud<PointOutT> &cloud_out);
476
477 /** \brief Concatenate two datasets representing different fields.
478 *
479 * \note If the input datasets have overlapping fields (i.e., both contain
480 * the same fields), then the data in the second cloud (cloud2_in) will
481 * overwrite the data in the first (cloud1_in).
482 *
483 * \param[in] cloud1_in the first input dataset
484 * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared)
485 * \param[out] cloud_out the output dataset created by concatenating all the fields in the input datasets
486 * \ingroup common
487 */
488 PCL_EXPORTS bool
490 const pcl::PCLPointCloud2 &cloud2_in,
491 pcl::PCLPointCloud2 &cloud_out);
492
493 /** \brief Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format
494 * \param[in] in the point cloud message
495 * \param[out] out the resultant Eigen MatrixXf format containing XYZ0 / point
496 * \ingroup common
497 */
498 PCL_EXPORTS bool
499 getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out);
500
501 /** \brief Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message
502 * \param[in] in the Eigen MatrixXf format containing XYZ0 / point
503 * \param[out] out the resultant point cloud message
504 * \note the method assumes that the PCLPointCloud2 message already has the fields set up properly !
505 * \ingroup common
506 */
507 PCL_EXPORTS bool
508 getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out);
509
510 namespace io
511 {
512 /** \brief swap bytes order of a char array of length N
513 * \param bytes char array to swap
514 * \ingroup common
515 */
516 template <std::size_t N> void
517 swapByte (char* bytes);
518
519 /** \brief specialization of swapByte for dimension 1
520 * \param bytes char array to swap
521 */
522 template <> inline void
523 swapByte<1> (char* bytes) { bytes[0] = bytes[0]; }
524
525
526 /** \brief specialization of swapByte for dimension 2
527 * \param bytes char array to swap
528 */
529 template <> inline void
530 swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); }
531
532 /** \brief specialization of swapByte for dimension 4
533 * \param bytes char array to swap
534 */
535 template <> inline void
536 swapByte<4> (char* bytes)
537 {
538 std::swap (bytes[0], bytes[3]);
539 std::swap (bytes[1], bytes[2]);
540 }
541
542 /** \brief specialization of swapByte for dimension 8
543 * \param bytes char array to swap
544 */
545 template <> inline void
546 swapByte<8> (char* bytes)
547 {
548 std::swap (bytes[0], bytes[7]);
549 std::swap (bytes[1], bytes[6]);
550 std::swap (bytes[2], bytes[5]);
551 std::swap (bytes[3], bytes[4]);
552 }
553
554 /** \brief swaps byte of an arbitrary type T casting it to char*
555 * \param value the data you want its bytes swapped
556 */
557 template <typename T> void
558 swapByte (T& value)
559 {
560 pcl::io::swapByte<sizeof(T)> (reinterpret_cast<char*> (&value));
561 }
562 }
563}
564
565#include <pcl/common/impl/io.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
static bool concatenate(pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2)
void swapByte(char *bytes)
swap bytes order of a char array of length N
PCL_EXPORTS bool getPointCloudAsEigen(const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out)
Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format.
PCL_EXPORTS bool getEigenAsPointCloud(Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out)
Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message.
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition io.h:121
std::string getFieldsList(const pcl::PointCloud< PointT > &)
Get the list of all fields available in a given cloud.
Definition io.hpp:107
int getFieldType(const int size, char type)
Obtains the type of the PCLPointField from a specific size and type.
Definition io.h:165
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
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud<PointT>
Definition io.h:275
#define PCL_FALLTHROUGH
Macro to add a no-op or a fallthrough attribute based on compiler feature.
Definition pcl_macros.h:433
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 swapByte< 1 >(char *bytes)
specialization of swapByte for dimension 1
Definition io.h:523
void swapByte< 4 >(char *bytes)
specialization of swapByte for dimension 4
Definition io.h:536
void swapByte< 2 >(char *bytes)
specialization of swapByte for dimension 2
Definition io.h:530
void swapByte< 8 >(char *bytes)
specialization of swapByte for dimension 8
Definition io.h:546
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition io.hpp:52
InterpolationType
Definition io.h:249
@ BORDER_REFLECT
Definition io.h:251
@ BORDER_REFLECT_101
Definition io.h:252
@ BORDER_TRANSPARENT
Definition io.h:252
@ BORDER_DEFAULT
Definition io.h:253
@ BORDER_CONSTANT
Definition io.h:250
@ BORDER_WRAP
Definition io.h:251
@ BORDER_REPLICATE
Definition io.h:250
PCL_EXPORTS int interpolatePointIndex(int p, int length, InterpolationType type)
constexpr bool isSamePointType() noexcept
Check if two given point types are the same or not.
Definition io.h:350
std::vector< index_t, Allocator > IndicesAllocator
Type used for indices in PCL.
Definition types.h:128
std::remove_cv_t< std::remove_reference_t< T > > remove_cvref_t
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
PCL_EXPORTS void getFieldsSizes(const std::vector< pcl::PCLPointField > &fields, std::vector< int > &field_sizes)
Obtain a vector with the sizes of all valid fields (e.g., not "_")
Defines all the PCL and non-PCL macros used.
std::vector<::pcl::PCLPointField > fields
static bool concatenate(pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2)
Inplace concatenate two pcl::PCLPointCloud2.
A point structure representing Euclidean xyz coordinates, and the RGB color.
static bool concatenate(pcl::PolygonMesh &mesh1, const pcl::PolygonMesh &mesh2)
Inplace concatenate two pcl::PolygonMesh.
Definition PolygonMesh.h:30