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_IO_IMPL_IO_HPP_ 00042 #define PCL_IO_IMPL_IO_HPP_ 00043 00044 #include <pcl/common/concatenate.h> 00045 #include <pcl/point_types.h> 00046 00047 ////////////////////////////////////////////////////////////////////////////////////////////// 00048 template <typename PointT> int 00049 pcl::getFieldIndex (const pcl::PointCloud<PointT> &, 00050 const std::string &field_name, 00051 std::vector<pcl::PCLPointField> &fields) 00052 { 00053 fields.clear (); 00054 // Get the fields list 00055 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00056 for (size_t d = 0; d < fields.size (); ++d) 00057 if (fields[d].name == field_name) 00058 return (static_cast<int>(d)); 00059 return (-1); 00060 } 00061 00062 ////////////////////////////////////////////////////////////////////////////////////////////// 00063 template <typename PointT> int 00064 pcl::getFieldIndex (const std::string &field_name, 00065 std::vector<pcl::PCLPointField> &fields) 00066 { 00067 fields.clear (); 00068 // Get the fields list 00069 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00070 for (size_t d = 0; d < fields.size (); ++d) 00071 if (fields[d].name == field_name) 00072 return (static_cast<int>(d)); 00073 return (-1); 00074 } 00075 00076 ////////////////////////////////////////////////////////////////////////////////////////////// 00077 template <typename PointT> void 00078 pcl::getFields (const pcl::PointCloud<PointT> &, std::vector<pcl::PCLPointField> &fields) 00079 { 00080 fields.clear (); 00081 // Get the fields list 00082 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00083 } 00084 00085 ////////////////////////////////////////////////////////////////////////////////////////////// 00086 template <typename PointT> void 00087 pcl::getFields (std::vector<pcl::PCLPointField> &fields) 00088 { 00089 fields.clear (); 00090 // Get the fields list 00091 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00092 } 00093 00094 ////////////////////////////////////////////////////////////////////////////////////////////// 00095 template <typename PointT> std::string 00096 pcl::getFieldsList (const pcl::PointCloud<PointT> &) 00097 { 00098 // Get the fields list 00099 std::vector<pcl::PCLPointField> fields; 00100 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00101 std::string result; 00102 for (size_t i = 0; i < fields.size () - 1; ++i) 00103 result += fields[i].name + " "; 00104 result += fields[fields.size () - 1].name; 00105 return (result); 00106 } 00107 00108 ////////////////////////////////////////////////////////////////////////////////////////////// 00109 template <typename PointInT, typename PointOutT> void 00110 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00111 pcl::PointCloud<PointOutT> &cloud_out) 00112 { 00113 // Copy all the data fields from the input cloud to the output one 00114 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00115 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00116 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00117 00118 cloud_out.header = cloud_in.header; 00119 cloud_out.width = cloud_in.width; 00120 cloud_out.height = cloud_in.height; 00121 cloud_out.is_dense = cloud_in.is_dense; 00122 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00123 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00124 cloud_out.points.resize (cloud_in.points.size ()); 00125 00126 // If the point types are the same, don't copy one by one 00127 if (isSamePointType<PointInT, PointOutT> ()) 00128 { 00129 memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT)); 00130 return; 00131 } 00132 00133 std::vector<pcl::PCLPointField> fields_in, fields_out; 00134 pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in)); 00135 pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out)); 00136 00137 // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and 00138 // fix it manually 00139 int rgb_idx_in = -1, rgb_idx_out = -1; 00140 for (size_t i = 0; i < fields_in.size (); ++i) 00141 if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") 00142 { 00143 rgb_idx_in = int (i); 00144 break; 00145 } 00146 for (size_t i = 0; i < fields_out.size (); ++i) 00147 if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") 00148 { 00149 rgb_idx_out = int (i); 00150 break; 00151 } 00152 00153 // We have one of the two cases: RGB vs RGBA or RGBA vs RGB 00154 if (rgb_idx_in != -1 && rgb_idx_out != -1 && 00155 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) 00156 { 00157 size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), 00158 field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); 00159 00160 if (field_size_in == field_size_out) 00161 { 00162 for (size_t i = 0; i < cloud_in.points.size (); ++i) 00163 { 00164 // Copy the rest 00165 pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[i], cloud_out.points[i])); 00166 // Copy RGB<->RGBA 00167 memcpy (reinterpret_cast<char*> (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); 00168 } 00169 return; 00170 } 00171 } 00172 00173 // Iterate over each point if no RGB/RGBA or if their size is different 00174 for (size_t i = 0; i < cloud_in.points.size (); ++i) 00175 // Iterate over each dimension 00176 pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[i], cloud_out.points[i])); 00177 } 00178 00179 ////////////////////////////////////////////////////////////////////////////////////////////// 00180 template <typename PointT> void 00181 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00182 const std::vector<int> &indices, 00183 pcl::PointCloud<PointT> &cloud_out) 00184 { 00185 // Do we want to copy everything? 00186 if (indices.size () == cloud_in.points.size ()) 00187 { 00188 cloud_out = cloud_in; 00189 return; 00190 } 00191 00192 // Allocate enough space and copy the basics 00193 cloud_out.points.resize (indices.size ()); 00194 cloud_out.header = cloud_in.header; 00195 cloud_out.width = static_cast<uint32_t>(indices.size ()); 00196 cloud_out.height = 1; 00197 cloud_out.is_dense = cloud_in.is_dense; 00198 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00199 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00200 00201 // Iterate over each point 00202 for (size_t i = 0; i < indices.size (); ++i) 00203 cloud_out.points[i] = cloud_in.points[indices[i]]; 00204 } 00205 00206 ////////////////////////////////////////////////////////////////////////////////////////////// 00207 template <typename PointT> void 00208 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00209 const std::vector<int, Eigen::aligned_allocator<int> > &indices, 00210 pcl::PointCloud<PointT> &cloud_out) 00211 { 00212 // Do we want to copy everything? 00213 if (indices.size () == cloud_in.points.size ()) 00214 { 00215 cloud_out = cloud_in; 00216 return; 00217 } 00218 00219 // Allocate enough space and copy the basics 00220 cloud_out.points.resize (indices.size ()); 00221 cloud_out.header = cloud_in.header; 00222 cloud_out.width = static_cast<uint32_t> (indices.size ()); 00223 cloud_out.height = 1; 00224 cloud_out.is_dense = cloud_in.is_dense; 00225 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00226 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00227 00228 // Iterate over each point 00229 for (size_t i = 0; i < indices.size (); ++i) 00230 cloud_out.points[i] = cloud_in.points[indices[i]]; 00231 } 00232 00233 ////////////////////////////////////////////////////////////////////////////////////////////// 00234 template <typename PointInT, typename PointOutT> void 00235 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00236 const std::vector<int> &indices, 00237 pcl::PointCloud<PointOutT> &cloud_out) 00238 { 00239 // Allocate enough space and copy the basics 00240 cloud_out.points.resize (indices.size ()); 00241 cloud_out.header = cloud_in.header; 00242 cloud_out.width = uint32_t (indices.size ()); 00243 cloud_out.height = 1; 00244 cloud_out.is_dense = cloud_in.is_dense; 00245 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00246 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00247 00248 // Copy all the data fields from the input cloud to the output one 00249 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00250 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00251 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00252 00253 // If the point types are the same, don't copy one by one 00254 if (isSamePointType<PointInT, PointOutT> ()) 00255 { 00256 // Iterate over each point 00257 for (size_t i = 0; i < indices.size (); ++i) 00258 memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT)); 00259 return; 00260 } 00261 00262 std::vector<pcl::PCLPointField> fields_in, fields_out; 00263 pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in)); 00264 pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out)); 00265 00266 // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and 00267 // fix it manually 00268 int rgb_idx_in = -1, rgb_idx_out = -1; 00269 for (size_t i = 0; i < fields_in.size (); ++i) 00270 if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") 00271 { 00272 rgb_idx_in = int (i); 00273 break; 00274 } 00275 for (size_t i = 0; int (i) < fields_out.size (); ++i) 00276 if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") 00277 { 00278 rgb_idx_out = int (i); 00279 break; 00280 } 00281 00282 // We have one of the two cases: RGB vs RGBA or RGBA vs RGB 00283 if (rgb_idx_in != -1 && rgb_idx_out != -1 && 00284 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) 00285 { 00286 size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), 00287 field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); 00288 00289 if (field_size_in == field_size_out) 00290 { 00291 for (size_t i = 0; i < indices.size (); ++i) 00292 { 00293 // Copy the rest 00294 pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00295 // Copy RGB<->RGBA 00296 memcpy (reinterpret_cast<char*> (&cloud_out.points[indices[i]]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); 00297 } 00298 return; 00299 } 00300 } 00301 00302 // Iterate over each point if no RGB/RGBA or if their size is different 00303 for (size_t i = 0; i < indices.size (); ++i) 00304 // Iterate over each dimension 00305 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00306 } 00307 00308 ////////////////////////////////////////////////////////////////////////////////////////////// 00309 template <typename PointInT, typename PointOutT> void 00310 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00311 const std::vector<int, Eigen::aligned_allocator<int> > &indices, 00312 pcl::PointCloud<PointOutT> &cloud_out) 00313 { 00314 // Allocate enough space and copy the basics 00315 cloud_out.points.resize (indices.size ()); 00316 cloud_out.header = cloud_in.header; 00317 cloud_out.width = static_cast<uint32_t> (indices.size ()); 00318 cloud_out.height = 1; 00319 cloud_out.is_dense = cloud_in.is_dense; 00320 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00321 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00322 00323 // Copy all the data fields from the input cloud to the output one 00324 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00325 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00326 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00327 00328 // If the point types are the same, don't copy one by one 00329 if (isSamePointType<PointInT, PointOutT> ()) 00330 { 00331 // Iterate over each point 00332 for (size_t i = 0; i < indices.size (); ++i) 00333 memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT)); 00334 return; 00335 } 00336 00337 std::vector<pcl::PCLPointField> fields_in, fields_out; 00338 pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in)); 00339 pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out)); 00340 00341 // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and 00342 // fix it manually 00343 int rgb_idx_in = -1, rgb_idx_out = -1; 00344 for (size_t i = 0; i < fields_in.size (); ++i) 00345 if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") 00346 { 00347 rgb_idx_in = int (i); 00348 break; 00349 } 00350 for (size_t i = 0; i < fields_out.size (); ++i) 00351 if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") 00352 { 00353 rgb_idx_out = int (i); 00354 break; 00355 } 00356 00357 // We have one of the two cases: RGB vs RGBA or RGBA vs RGB 00358 if (rgb_idx_in != -1 && rgb_idx_out != -1 && 00359 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) 00360 { 00361 size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), 00362 field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); 00363 00364 if (field_size_in == field_size_out) 00365 { 00366 for (size_t i = 0; i < indices.size (); ++i) 00367 { 00368 // Copy the rest 00369 pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00370 // Copy RGB<->RGBA 00371 memcpy (reinterpret_cast<char*> (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); 00372 } 00373 return; 00374 } 00375 } 00376 00377 // Iterate over each point if no RGB/RGBA or if their size is different 00378 for (size_t i = 0; i < indices.size (); ++i) 00379 // Iterate over each dimension 00380 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00381 } 00382 00383 ////////////////////////////////////////////////////////////////////////////////////////////// 00384 template <typename PointT> void 00385 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00386 const pcl::PointIndices &indices, 00387 pcl::PointCloud<PointT> &cloud_out) 00388 { 00389 // Do we want to copy everything? 00390 if (indices.indices.size () == cloud_in.points.size ()) 00391 { 00392 cloud_out = cloud_in; 00393 return; 00394 } 00395 00396 // Allocate enough space and copy the basics 00397 cloud_out.points.resize (indices.indices.size ()); 00398 cloud_out.header = cloud_in.header; 00399 cloud_out.width = indices.indices.size (); 00400 cloud_out.height = 1; 00401 cloud_out.is_dense = cloud_in.is_dense; 00402 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00403 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00404 00405 // Iterate over each point 00406 for (size_t i = 0; i < indices.indices.size (); ++i) 00407 cloud_out.points[i] = cloud_in.points[indices.indices[i]]; 00408 } 00409 00410 /////////////////////////////////////////////////////////////////////////////////////////////// 00411 template <typename PointInT, typename PointOutT> void 00412 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00413 const pcl::PointIndices &indices, 00414 pcl::PointCloud<PointOutT> &cloud_out) 00415 { 00416 // Allocate enough space and copy the basics 00417 cloud_out.points.resize (indices.indices.size ()); 00418 cloud_out.header = cloud_in.header; 00419 cloud_out.width = indices.indices.size (); 00420 cloud_out.height = 1; 00421 cloud_out.is_dense = cloud_in.is_dense; 00422 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00423 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00424 00425 // Copy all the data fields from the input cloud to the output one 00426 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00427 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00428 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00429 00430 // If the point types are the same, don't copy one by one 00431 if (isSamePointType<PointInT, PointOutT> ()) 00432 { 00433 // Iterate over each point 00434 for (size_t i = 0; i < indices.indices.size (); ++i) 00435 memcpy (&cloud_out.points[i], &cloud_in.points[indices.indices[i]], sizeof (PointInT)); 00436 return; 00437 } 00438 00439 std::vector<pcl::PCLPointField> fields_in, fields_out; 00440 pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in)); 00441 pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out)); 00442 00443 // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and 00444 // fix it manually 00445 int rgb_idx_in = -1, rgb_idx_out = -1; 00446 for (size_t i = 0; i < fields_in.size (); ++i) 00447 if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") 00448 { 00449 rgb_idx_in = int (i); 00450 break; 00451 } 00452 for (size_t i = 0; i < fields_out.size (); ++i) 00453 if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") 00454 { 00455 rgb_idx_out = int (i); 00456 break; 00457 } 00458 00459 // We have one of the two cases: RGB vs RGBA or RGBA vs RGB 00460 if (rgb_idx_in != -1 && rgb_idx_out != -1 && 00461 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) 00462 { 00463 size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), 00464 field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); 00465 00466 if (field_size_in == field_size_out) 00467 { 00468 for (size_t i = 0; i < indices.indices.size (); ++i) 00469 { 00470 // Copy the rest 00471 pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices.indices[i]], cloud_out.points[i])); 00472 // Copy RGB<->RGBA 00473 memcpy (reinterpret_cast<char*> (&cloud_out.points[indices.indices[i]]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); 00474 } 00475 return; 00476 } 00477 } 00478 00479 // Iterate over each point if no RGB/RGBA or if their size is different 00480 for (size_t i = 0; i < indices.indices.size (); ++i) 00481 // Iterate over each dimension 00482 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices.indices[i]], cloud_out.points[i])); 00483 } 00484 00485 ////////////////////////////////////////////////////////////////////////////////////////////// 00486 template <typename PointT> void 00487 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00488 const std::vector<pcl::PointIndices> &indices, 00489 pcl::PointCloud<PointT> &cloud_out) 00490 { 00491 int nr_p = 0; 00492 for (size_t i = 0; i < indices.size (); ++i) 00493 nr_p += indices[i].indices.size (); 00494 00495 // Do we want to copy everything? Remember we assume UNIQUE indices 00496 if (nr_p == cloud_in.points.size ()) 00497 { 00498 cloud_out = cloud_in; 00499 return; 00500 } 00501 00502 // Allocate enough space and copy the basics 00503 cloud_out.points.resize (nr_p); 00504 cloud_out.header = cloud_in.header; 00505 cloud_out.width = nr_p; 00506 cloud_out.height = 1; 00507 cloud_out.is_dense = cloud_in.is_dense; 00508 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00509 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00510 00511 // Iterate over each cluster 00512 int cp = 0; 00513 for (size_t cc = 0; cc < indices.size (); ++cc) 00514 { 00515 // Iterate over each idx 00516 for (size_t i = 0; i < indices[cc].indices.size (); ++i) 00517 { 00518 // Iterate over each dimension 00519 cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]]; 00520 cp++; 00521 } 00522 } 00523 } 00524 00525 ////////////////////////////////////////////////////////////////////////////////////////////// 00526 template <typename PointInT, typename PointOutT> void 00527 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00528 const std::vector<pcl::PointIndices> &indices, 00529 pcl::PointCloud<PointOutT> &cloud_out) 00530 { 00531 int nr_p = 0; 00532 for (size_t i = 0; i < indices.size (); ++i) 00533 nr_p += indices[i].indices.size (); 00534 00535 // Do we want to copy everything? Remember we assume UNIQUE indices 00536 if (nr_p == cloud_in.points.size ()) 00537 { 00538 copyPointCloud<PointInT, PointOutT> (cloud_in, cloud_out); 00539 return; 00540 } 00541 00542 // Allocate enough space and copy the basics 00543 cloud_out.points.resize (nr_p); 00544 cloud_out.header = cloud_in.header; 00545 cloud_out.width = nr_p; 00546 cloud_out.height = 1; 00547 cloud_out.is_dense = cloud_in.is_dense; 00548 cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; 00549 cloud_out.sensor_origin_ = cloud_in.sensor_origin_; 00550 00551 // Copy all the data fields from the input cloud to the output one 00552 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00553 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00554 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00555 00556 // If the point types are the same, don't copy one by one 00557 if (isSamePointType<PointInT, PointOutT> ()) 00558 { 00559 // Iterate over each cluster 00560 int cp = 0; 00561 for (size_t cc = 0; cc < indices.size (); ++cc) 00562 { 00563 // Iterate over each idx 00564 for (size_t i = 0; i < indices[cc].indices.size (); ++i) 00565 { 00566 cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]]; 00567 ++cp; 00568 } 00569 } 00570 return; 00571 } 00572 00573 std::vector<pcl::PCLPointField> fields_in, fields_out; 00574 pcl::for_each_type<FieldListInT> (pcl::detail::FieldAdder<PointInT> (fields_in)); 00575 pcl::for_each_type<FieldListOutT> (pcl::detail::FieldAdder<PointOutT> (fields_out)); 00576 00577 // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and 00578 // fix it manually 00579 int rgb_idx_in = -1, rgb_idx_out = -1; 00580 for (size_t i = 0; i < fields_in.size (); ++i) 00581 if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") 00582 { 00583 rgb_idx_in = int (i); 00584 break; 00585 } 00586 for (size_t i = 0; i < fields_out.size (); ++i) 00587 if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") 00588 { 00589 rgb_idx_out = int (i); 00590 break; 00591 } 00592 00593 // We have one of the two cases: RGB vs RGBA or RGBA vs RGB 00594 if (rgb_idx_in != -1 && rgb_idx_out != -1 && 00595 fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) 00596 { 00597 size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), 00598 field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); 00599 00600 if (field_size_in == field_size_out) 00601 { 00602 // Iterate over each cluster 00603 int cp = 0; 00604 for (size_t cc = 0; cc < indices.size (); ++cc) 00605 { 00606 // Iterate over each idx 00607 for (size_t i = 0; i < indices[cc].indices.size (); ++i) 00608 { 00609 // Iterate over each dimension 00610 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); 00611 // Copy RGB<->RGBA 00612 memcpy (reinterpret_cast<char*> (&cloud_out.points[cp]) + fields_out[rgb_idx_out].offset, reinterpret_cast<const char*> (&cloud_in.points[indices[cp].indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); 00613 ++cp; 00614 } 00615 } 00616 return; 00617 } 00618 } 00619 00620 // Iterate over each cluster 00621 int cp = 0; 00622 for (size_t cc = 0; cc < indices.size (); ++cc) 00623 { 00624 // Iterate over each idx 00625 for (size_t i = 0; i < indices[cc].indices.size (); ++i) 00626 { 00627 // Iterate over each dimension 00628 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); 00629 ++cp; 00630 } 00631 } 00632 } 00633 00634 ////////////////////////////////////////////////////////////////////////////////////////////// 00635 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void 00636 pcl::concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in, 00637 const pcl::PointCloud<PointIn2T> &cloud2_in, 00638 pcl::PointCloud<PointOutT> &cloud_out) 00639 { 00640 typedef typename pcl::traits::fieldList<PointIn1T>::type FieldList1; 00641 typedef typename pcl::traits::fieldList<PointIn2T>::type FieldList2; 00642 00643 if (cloud1_in.points.size () != cloud2_in.points.size ()) 00644 { 00645 PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n"); 00646 return; 00647 } 00648 00649 // Resize the output dataset 00650 cloud_out.points.resize (cloud1_in.points.size ()); 00651 cloud_out.header = cloud1_in.header; 00652 cloud_out.width = cloud1_in.width; 00653 cloud_out.height = cloud1_in.height; 00654 if (!cloud1_in.is_dense || !cloud2_in.is_dense) 00655 cloud_out.is_dense = false; 00656 else 00657 cloud_out.is_dense = true; 00658 00659 // Iterate over each point 00660 for (size_t i = 0; i < cloud_out.points.size (); ++i) 00661 { 00662 // Iterate over each dimension 00663 pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in.points[i], cloud_out.points[i])); 00664 pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in.points[i], cloud_out.points[i])); 00665 } 00666 } 00667 00668 #endif // PCL_IO_IMPL_IO_H_ 00669