Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_COMMON_IO_H_ 00042 #define PCL_COMMON_IO_H_ 00043 00044 #include <string> 00045 #include <pcl/pcl_base.h> 00046 #include <pcl/PointIndices.h> 00047 #include <pcl/conversions.h> 00048 #include <locale> 00049 00050 namespace pcl 00051 { 00052 /** \brief Get the index of a specified field (i.e., dimension/channel) 00053 * \param[in] cloud the the point cloud message 00054 * \param[in] field_name the string defining the field name 00055 * \ingroup common 00056 */ 00057 inline int 00058 getFieldIndex (const pcl::PCLPointCloud2 &cloud, const std::string &field_name) 00059 { 00060 // Get the index we need 00061 for (size_t d = 0; d < cloud.fields.size (); ++d) 00062 if (cloud.fields[d].name == field_name) 00063 return (static_cast<int>(d)); 00064 return (-1); 00065 } 00066 00067 /** \brief Get the index of a specified field (i.e., dimension/channel) 00068 * \param[in] cloud the the point cloud message 00069 * \param[in] field_name the string defining the field name 00070 * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains 00071 * \ingroup common 00072 */ 00073 template <typename PointT> inline int 00074 getFieldIndex (const pcl::PointCloud<PointT> &cloud, const std::string &field_name, 00075 std::vector<pcl::PCLPointField> &fields); 00076 00077 /** \brief Get the index of a specified field (i.e., dimension/channel) 00078 * \param[in] field_name the string defining the field name 00079 * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains 00080 * \ingroup common 00081 */ 00082 template <typename PointT> inline int 00083 getFieldIndex (const std::string &field_name, 00084 std::vector<pcl::PCLPointField> &fields); 00085 00086 /** \brief Get the list of available fields (i.e., dimension/channel) 00087 * \param[in] cloud the point cloud message 00088 * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains 00089 * \ingroup common 00090 */ 00091 template <typename PointT> inline void 00092 getFields (const pcl::PointCloud<PointT> &cloud, std::vector<pcl::PCLPointField> &fields); 00093 00094 /** \brief Get the list of available fields (i.e., dimension/channel) 00095 * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains 00096 * \ingroup common 00097 */ 00098 template <typename PointT> inline void 00099 getFields (std::vector<pcl::PCLPointField> &fields); 00100 00101 /** \brief Get the list of all fields available in a given cloud 00102 * \param[in] cloud the the point cloud message 00103 * \ingroup common 00104 */ 00105 template <typename PointT> inline std::string 00106 getFieldsList (const pcl::PointCloud<PointT> &cloud); 00107 00108 /** \brief Get the available point cloud fields as a space separated string 00109 * \param[in] cloud a pointer to the PointCloud message 00110 * \ingroup common 00111 */ 00112 inline std::string 00113 getFieldsList (const pcl::PCLPointCloud2 &cloud) 00114 { 00115 std::string result; 00116 for (size_t i = 0; i < cloud.fields.size () - 1; ++i) 00117 result += cloud.fields[i].name + " "; 00118 result += cloud.fields[cloud.fields.size () - 1].name; 00119 return (result); 00120 } 00121 00122 /** \brief Obtains the size of a specific field data type in bytes 00123 * \param[in] datatype the field data type (see PCLPointField.h) 00124 * \ingroup common 00125 */ 00126 inline int 00127 getFieldSize (const int datatype) 00128 { 00129 switch (datatype) 00130 { 00131 case pcl::PCLPointField::INT8: 00132 case pcl::PCLPointField::UINT8: 00133 return (1); 00134 00135 case pcl::PCLPointField::INT16: 00136 case pcl::PCLPointField::UINT16: 00137 return (2); 00138 00139 case pcl::PCLPointField::INT32: 00140 case pcl::PCLPointField::UINT32: 00141 case pcl::PCLPointField::FLOAT32: 00142 return (4); 00143 00144 case pcl::PCLPointField::FLOAT64: 00145 return (8); 00146 00147 default: 00148 return (0); 00149 } 00150 } 00151 00152 /** \brief Obtain a vector with the sizes of all valid fields (e.g., not "_") 00153 * \param[in] fields the input vector containing the fields 00154 * \param[out] field_sizes the resultant field sizes in bytes 00155 */ 00156 PCL_EXPORTS void 00157 getFieldsSizes (const std::vector<pcl::PCLPointField> &fields, 00158 std::vector<int> &field_sizes); 00159 00160 /** \brief Obtains the type of the PCLPointField from a specific size and type 00161 * \param[in] size the size in bytes of the data field 00162 * \param[in] type a char describing the type of the field ('F' = float, 'I' = signed, 'U' = unsigned) 00163 * \ingroup common 00164 */ 00165 inline int 00166 getFieldType (const int size, char type) 00167 { 00168 type = std::toupper (type, std::locale::classic ()); 00169 switch (size) 00170 { 00171 case 1: 00172 if (type == 'I') 00173 return (pcl::PCLPointField::INT8); 00174 if (type == 'U') 00175 return (pcl::PCLPointField::UINT8); 00176 00177 case 2: 00178 if (type == 'I') 00179 return (pcl::PCLPointField::INT16); 00180 if (type == 'U') 00181 return (pcl::PCLPointField::UINT16); 00182 00183 case 4: 00184 if (type == 'I') 00185 return (pcl::PCLPointField::INT32); 00186 if (type == 'U') 00187 return (pcl::PCLPointField::UINT32); 00188 if (type == 'F') 00189 return (pcl::PCLPointField::FLOAT32); 00190 00191 case 8: 00192 return (pcl::PCLPointField::FLOAT64); 00193 00194 default: 00195 return (-1); 00196 } 00197 } 00198 00199 /** \brief Obtains the type of the PCLPointField from a specific PCLPointField as a char 00200 * \param[in] type the PCLPointField field type 00201 * \ingroup common 00202 */ 00203 inline char 00204 getFieldType (const int type) 00205 { 00206 switch (type) 00207 { 00208 case pcl::PCLPointField::INT8: 00209 case pcl::PCLPointField::INT16: 00210 case pcl::PCLPointField::INT32: 00211 return ('I'); 00212 00213 case pcl::PCLPointField::UINT8: 00214 case pcl::PCLPointField::UINT16: 00215 case pcl::PCLPointField::UINT32: 00216 return ('U'); 00217 00218 case pcl::PCLPointField::FLOAT32: 00219 case pcl::PCLPointField::FLOAT64: 00220 return ('F'); 00221 default: 00222 return ('?'); 00223 } 00224 } 00225 00226 /** \brief Concatenate two pcl::PCLPointCloud2. 00227 * \param[in] cloud1 the first input point cloud dataset 00228 * \param[in] cloud2 the second input point cloud dataset 00229 * \param[out] cloud_out the resultant output point cloud dataset 00230 * \return true if successful, false if failed (e.g., name/number of fields differs) 00231 * \ingroup common 00232 */ 00233 PCL_EXPORTS bool 00234 concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1, 00235 const pcl::PCLPointCloud2 &cloud2, 00236 pcl::PCLPointCloud2 &cloud_out); 00237 00238 /** \brief Extract the indices of a given point cloud as a new point cloud 00239 * \param[in] cloud_in the input point cloud dataset 00240 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in 00241 * \param[out] cloud_out the resultant output point cloud dataset 00242 * \note Assumes unique indices. 00243 * \ingroup common 00244 */ 00245 PCL_EXPORTS void 00246 copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, 00247 const std::vector<int> &indices, 00248 pcl::PCLPointCloud2 &cloud_out); 00249 00250 /** \brief Extract the indices of a given point cloud as a new point cloud 00251 * \param[in] cloud_in the input point cloud dataset 00252 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in 00253 * \param[out] cloud_out the resultant output point cloud dataset 00254 * \note Assumes unique indices. 00255 * \ingroup common 00256 */ 00257 PCL_EXPORTS void 00258 copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, 00259 const std::vector<int, Eigen::aligned_allocator<int> > &indices, 00260 pcl::PCLPointCloud2 &cloud_out); 00261 00262 /** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out 00263 * \param[in] cloud_in the input point cloud dataset 00264 * \param[out] cloud_out the resultant output point cloud dataset 00265 * \ingroup common 00266 */ 00267 PCL_EXPORTS void 00268 copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, 00269 pcl::PCLPointCloud2 &cloud_out); 00270 00271 /** \brief Check if two given point types are the same or not. */ 00272 template <typename Point1T, typename Point2T> inline bool 00273 isSamePointType () 00274 { 00275 return (typeid (Point1T) == typeid (Point2T)); 00276 } 00277 00278 /** \brief Extract the indices of a given point cloud as a new point cloud 00279 * \param[in] cloud_in the input point cloud dataset 00280 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in 00281 * \param[out] cloud_out the resultant output point cloud dataset 00282 * \note Assumes unique indices. 00283 * \ingroup common 00284 */ 00285 template <typename PointT> void 00286 copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00287 const std::vector<int> &indices, 00288 pcl::PointCloud<PointT> &cloud_out); 00289 00290 /** \brief Extract the indices of a given point cloud as a new point cloud 00291 * \param[in] cloud_in the input point cloud dataset 00292 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in 00293 * \param[out] cloud_out the resultant output point cloud dataset 00294 * \note Assumes unique indices. 00295 * \ingroup common 00296 */ 00297 template <typename PointT> void 00298 copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00299 const std::vector<int, Eigen::aligned_allocator<int> > &indices, 00300 pcl::PointCloud<PointT> &cloud_out); 00301 00302 /** \brief Extract the indices of a given point cloud as a new point cloud 00303 * \param[in] cloud_in the input point cloud dataset 00304 * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in 00305 * \param[out] cloud_out the resultant output point cloud dataset 00306 * \note Assumes unique indices. 00307 * \ingroup common 00308 */ 00309 template <typename PointT> void 00310 copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00311 const PointIndices &indices, 00312 pcl::PointCloud<PointT> &cloud_out); 00313 00314 /** \brief Extract the indices of a given point cloud as a new point cloud 00315 * \param[in] cloud_in the input point cloud dataset 00316 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in 00317 * \param[out] cloud_out the resultant output point cloud dataset 00318 * \note Assumes unique indices. 00319 * \ingroup common 00320 */ 00321 template <typename PointT> void 00322 copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00323 const std::vector<pcl::PointIndices> &indices, 00324 pcl::PointCloud<PointT> &cloud_out); 00325 00326 /** \brief Copy all the fields from a given point cloud into a new point cloud 00327 * \param[in] cloud_in the input point cloud dataset 00328 * \param[out] cloud_out the resultant output point cloud dataset 00329 * \ingroup common 00330 */ 00331 template <typename PointInT, typename PointOutT> void 00332 copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00333 pcl::PointCloud<PointOutT> &cloud_out); 00334 00335 /** \brief Extract the indices of a given point cloud as a new point cloud 00336 * \param[in] cloud_in the input point cloud dataset 00337 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in 00338 * \param[out] cloud_out the resultant output point cloud dataset 00339 * \note Assumes unique indices. 00340 * \ingroup common 00341 */ 00342 template <typename PointInT, typename PointOutT> void 00343 copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00344 const std::vector<int> &indices, 00345 pcl::PointCloud<PointOutT> &cloud_out); 00346 00347 /** \brief Extract the indices of a given point cloud as a new point cloud 00348 * \param[in] cloud_in the input point cloud dataset 00349 * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in 00350 * \param[out] cloud_out the resultant output point cloud dataset 00351 * \note Assumes unique indices. 00352 * \ingroup common 00353 */ 00354 template <typename PointInT, typename PointOutT> void 00355 copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00356 const std::vector<int, Eigen::aligned_allocator<int> > &indices, 00357 pcl::PointCloud<PointOutT> &cloud_out); 00358 00359 /** \brief Extract the indices of a given point cloud as a new point cloud 00360 * \param[in] cloud_in the input point cloud dataset 00361 * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in 00362 * \param[out] cloud_out the resultant output point cloud dataset 00363 * \note Assumes unique indices. 00364 * \ingroup common 00365 */ 00366 template <typename PointInT, typename PointOutT> void 00367 copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00368 const PointIndices &indices, 00369 pcl::PointCloud<PointOutT> &cloud_out); 00370 00371 /** \brief Extract the indices of a given point cloud as a new point cloud 00372 * \param[in] cloud_in the input point cloud dataset 00373 * \param[in] indices the vector of indices representing the points to be copied from cloud_in 00374 * \param[out] cloud_out the resultant output point cloud dataset 00375 * \note Assumes unique indices. 00376 * \ingroup common 00377 */ 00378 template <typename PointInT, typename PointOutT> void 00379 copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00380 const std::vector<pcl::PointIndices> &indices, 00381 pcl::PointCloud<PointOutT> &cloud_out); 00382 00383 /** \brief Concatenate two datasets representing different fields. 00384 * 00385 * \note If the input datasets have overlapping fields (i.e., both contain 00386 * the same fields), then the data in the second cloud (cloud2_in) will 00387 * overwrite the data in the first (cloud1_in). 00388 * 00389 * \param[in] cloud1_in the first input dataset 00390 * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared) 00391 * \param[out] cloud_out the resultant output dataset created by the concatenation of all the fields in the input datasets 00392 * \ingroup common 00393 */ 00394 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void 00395 concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in, 00396 const pcl::PointCloud<PointIn2T> &cloud2_in, 00397 pcl::PointCloud<PointOutT> &cloud_out); 00398 00399 /** \brief Concatenate two datasets representing different fields. 00400 * 00401 * \note If the input datasets have overlapping fields (i.e., both contain 00402 * the same fields), then the data in the second cloud (cloud2_in) will 00403 * overwrite the data in the first (cloud1_in). 00404 * 00405 * \param[in] cloud1_in the first input dataset 00406 * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared) 00407 * \param[out] cloud_out the output dataset created by concatenating all the fields in the input datasets 00408 * \ingroup common 00409 */ 00410 PCL_EXPORTS bool 00411 concatenateFields (const pcl::PCLPointCloud2 &cloud1_in, 00412 const pcl::PCLPointCloud2 &cloud2_in, 00413 pcl::PCLPointCloud2 &cloud_out); 00414 00415 /** \brief Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format 00416 * \param[in] in the point cloud message 00417 * \param[out] out the resultant Eigen MatrixXf format containing XYZ0 / point 00418 * \ingroup common 00419 */ 00420 PCL_EXPORTS bool 00421 getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out); 00422 00423 /** \brief Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message 00424 * \param[in] in the Eigen MatrixXf format containing XYZ0 / point 00425 * \param[out] out the resultant point cloud message 00426 * \note the method assumes that the PCLPointCloud2 message already has the fields set up properly ! 00427 * \ingroup common 00428 */ 00429 PCL_EXPORTS bool 00430 getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out); 00431 00432 namespace io 00433 { 00434 /** \brief swap bytes order of a char array of length N 00435 * \param bytes char array to swap 00436 * \ingroup common 00437 */ 00438 template <std::size_t N> void 00439 swapByte (char* bytes); 00440 00441 /** \brief specialization of swapByte for dimension 1 00442 * \param bytes char array to swap 00443 */ 00444 template <> inline void 00445 swapByte<1> (char* bytes) { bytes[0] = bytes[0]; } 00446 00447 00448 /** \brief specialization of swapByte for dimension 2 00449 * \param bytes char array to swap 00450 */ 00451 template <> inline void 00452 swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); } 00453 00454 /** \brief specialization of swapByte for dimension 4 00455 * \param bytes char array to swap 00456 */ 00457 template <> inline void 00458 swapByte<4> (char* bytes) 00459 { 00460 std::swap (bytes[0], bytes[3]); 00461 std::swap (bytes[1], bytes[2]); 00462 } 00463 00464 /** \brief specialization of swapByte for dimension 8 00465 * \param bytes char array to swap 00466 */ 00467 template <> inline void 00468 swapByte<8> (char* bytes) 00469 { 00470 std::swap (bytes[0], bytes[7]); 00471 std::swap (bytes[1], bytes[6]); 00472 std::swap (bytes[2], bytes[5]); 00473 std::swap (bytes[3], bytes[4]); 00474 } 00475 00476 /** \brief swaps byte of an arbitrary type T casting it to char* 00477 * \param value the data you want its bytes swapped 00478 */ 00479 template <typename T> void 00480 swapByte (T& value) 00481 { 00482 pcl::io::swapByte<sizeof(T)> (reinterpret_cast<char*> (&value)); 00483 } 00484 } 00485 } 00486 00487 #include <pcl/common/impl/io.hpp> 00488 00489 #endif //#ifndef PCL_COMMON_IO_H_ 00490