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