Point Cloud Library (PCL)
1.3.1
|
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 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: io.hpp 2532 2011-09-20 20:39:18Z bouffa $ 00037 * 00038 */ 00039 00040 #ifndef PCL_IO_IMPL_IO_HPP_ 00041 #define PCL_IO_IMPL_IO_HPP_ 00042 00043 #include "pcl/common/concatenate.h" 00044 00046 template <typename PointT> int 00047 pcl::getFieldIndex (const pcl::PointCloud<PointT> &cloud, 00048 const std::string &field_name, 00049 std::vector<sensor_msgs::PointField> &fields) 00050 { 00051 fields.clear (); 00052 // Get the fields list 00053 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00054 for (size_t d = 0; d < fields.size (); ++d) 00055 if (fields[d].name == field_name) 00056 return ((int) d); 00057 return (-1); 00058 } 00059 00061 template <typename PointT> void 00062 pcl::getFields (const pcl::PointCloud<PointT> &cloud, std::vector<sensor_msgs::PointField> &fields) 00063 { 00064 fields.clear (); 00065 // Get the fields list 00066 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00067 } 00068 00070 template <typename PointT> std::string 00071 pcl::getFieldsList (const pcl::PointCloud<PointT> &cloud) 00072 { 00073 // Get the fields list 00074 std::vector<sensor_msgs::PointField> fields; 00075 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00076 std::string result; 00077 for (size_t i = 0; i < fields.size () - 1; ++i) 00078 result += fields[i].name + " "; 00079 result += fields[fields.size () - 1].name; 00080 return (result); 00081 } 00082 00084 template <typename PointInT, typename PointOutT> void 00085 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, pcl::PointCloud<PointOutT> &cloud_out) 00086 { 00087 // Allocate enough space and copy the basics 00088 cloud_out.points.resize (cloud_in.points.size ()); 00089 cloud_out.header = cloud_in.header; 00090 cloud_out.width = cloud_in.width; 00091 cloud_out.height = cloud_in.height; 00092 cloud_out.is_dense = cloud_in.is_dense; 00093 // Copy all the data fields from the input cloud to the output one 00094 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00095 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00096 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00097 // Iterate over each point 00098 for (size_t i = 0; i < cloud_in.points.size (); ++i) 00099 { 00100 // Iterate over each dimension 00101 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[i], cloud_out.points[i])); 00102 } 00103 } 00104 00106 template <typename PointT> void 00107 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, const std::vector<int> &indices, 00108 pcl::PointCloud<PointT> &cloud_out) 00109 { 00110 // Allocate enough space and copy the basics 00111 cloud_out.points.resize (indices.size ()); 00112 cloud_out.header = cloud_in.header; 00113 cloud_out.width = indices.size (); 00114 cloud_out.height = 1; 00115 if (cloud_in.is_dense) 00116 cloud_out.is_dense = true; 00117 else 00118 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00119 // To verify this, we would need to iterate over all points and check for NaNs 00120 cloud_out.is_dense = false; 00121 00122 // Copy all the data fields from the input cloud to the output one 00123 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00124 // Iterate over each point 00125 for (size_t i = 0; i < indices.size (); ++i) 00126 // Iterate over each dimension 00127 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00128 } 00129 00131 template <typename PointT> void 00132 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00133 const std::vector<int, Eigen::aligned_allocator<int> > &indices, 00134 pcl::PointCloud<PointT> &cloud_out) 00135 { 00136 // Allocate enough space and copy the basics 00137 cloud_out.points.resize (indices.size ()); 00138 cloud_out.header = cloud_in.header; 00139 cloud_out.width = indices.size (); 00140 cloud_out.height = 1; 00141 if (cloud_in.is_dense) 00142 cloud_out.is_dense = true; 00143 else 00144 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00145 // To verify this, we would need to iterate over all points and check for NaNs 00146 cloud_out.is_dense = false; 00147 00148 // Copy all the data fields from the input cloud to the output one 00149 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00150 // Iterate over each point 00151 for (size_t i = 0; i < indices.size (); ++i) 00152 // Iterate over each dimension 00153 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00154 } 00155 00157 template <typename PointInT, typename PointOutT> void 00158 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, const std::vector<int> &indices, 00159 pcl::PointCloud<PointOutT> &cloud_out) 00160 { 00161 // Allocate enough space and copy the basics 00162 cloud_out.points.resize (indices.size ()); 00163 cloud_out.header = cloud_in.header; 00164 cloud_out.width = indices.size (); 00165 cloud_out.height = 1; 00166 if (cloud_in.is_dense) 00167 cloud_out.is_dense = true; 00168 else 00169 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00170 // To verify this, we would need to iterate over all points and check for NaNs 00171 cloud_out.is_dense = false; 00172 00173 // Copy all the data fields from the input cloud to the output one 00174 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00175 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00176 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00177 // Iterate over each point 00178 for (size_t i = 0; i < indices.size (); ++i) 00179 // Iterate over each dimension 00180 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00181 } 00182 00184 template <typename PointInT, typename PointOutT> void 00185 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, 00186 const std::vector<int, Eigen::aligned_allocator<int> > &indices, 00187 pcl::PointCloud<PointOutT> &cloud_out) 00188 { 00189 // Allocate enough space and copy the basics 00190 cloud_out.points.resize (indices.size ()); 00191 cloud_out.header = cloud_in.header; 00192 cloud_out.width = indices.size (); 00193 cloud_out.height = 1; 00194 if (cloud_in.is_dense) 00195 cloud_out.is_dense = true; 00196 else 00197 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00198 // To verify this, we would need to iterate over all points and check for NaNs 00199 cloud_out.is_dense = false; 00200 00201 // Copy all the data fields from the input cloud to the output one 00202 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00203 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00204 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00205 // Iterate over each point 00206 for (size_t i = 0; i < indices.size (); ++i) 00207 // Iterate over each dimension 00208 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[i]], cloud_out.points[i])); 00209 } 00210 00212 template <typename PointT> void 00213 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, const pcl::PointIndices &indices, 00214 pcl::PointCloud<PointT> &cloud_out) 00215 { 00216 // Allocate enough space and copy the basics 00217 cloud_out.points.resize (indices.indices.size ()); 00218 cloud_out.header = cloud_in.header; 00219 cloud_out.width = indices.indices.size (); 00220 cloud_out.height = 1; 00221 if (cloud_in.is_dense) 00222 cloud_out.is_dense = true; 00223 else 00224 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00225 // To verify this, we would need to iterate over all points and check for NaNs 00226 cloud_out.is_dense = false; 00227 00228 // Copy all the data fields from the input cloud to the output one 00229 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00230 // Iterate over each point 00231 for (size_t i = 0; i < indices.indices.size (); ++i) 00232 // Iterate over each dimension 00233 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (cloud_in.points[indices.indices[i]], cloud_out.points[i])); 00234 } 00235 00237 template <typename PointInT, typename PointOutT> void 00238 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, const pcl::PointIndices &indices, 00239 pcl::PointCloud<PointOutT> &cloud_out) 00240 { 00241 // Allocate enough space and copy the basics 00242 cloud_out.points.resize (indices.indices.size ()); 00243 cloud_out.header = cloud_in.header; 00244 cloud_out.width = indices.indices.size (); 00245 cloud_out.height = 1; 00246 if (cloud_in.is_dense) 00247 cloud_out.is_dense = true; 00248 else 00249 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00250 // To verify this, we would need to iterate over all points and check for NaNs 00251 cloud_out.is_dense = false; 00252 00253 // Copy all the data fields from the input cloud to the output one 00254 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00255 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00256 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00257 // Iterate over each point 00258 for (size_t i = 0; i < indices.indices.size (); ++i) 00259 // Iterate over each dimension 00260 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices.indices[i]], cloud_out.points[i])); 00261 } 00262 00264 template <typename PointT> void 00265 pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, const std::vector<pcl::PointIndices> &indices, 00266 pcl::PointCloud<PointT> &cloud_out) 00267 { 00268 int nr_p = 0; 00269 for (size_t i = 0; i < indices.size (); ++i) 00270 nr_p += indices[i].indices.size (); 00271 00272 // Allocate enough space and copy the basics 00273 cloud_out.points.resize (nr_p); 00274 cloud_out.header = cloud_in.header; 00275 cloud_out.width = nr_p; 00276 cloud_out.height = 1; 00277 if (cloud_in.is_dense) 00278 cloud_out.is_dense = true; 00279 else 00280 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00281 // To verify this, we would need to iterate over all points and check for NaNs 00282 cloud_out.is_dense = false; 00283 00284 // Copy all the data fields from the input cloud to the output one 00285 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00286 // Iterate over each cluster 00287 int cp = 0; 00288 for (size_t cc = 0; cc < indices.size (); ++cc) 00289 { 00290 // Iterate over each idx 00291 for (size_t i = 0; i < indices[cc].indices.size (); ++i) 00292 { 00293 // Iterate over each dimension 00294 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); 00295 cp++; 00296 } 00297 } 00298 } 00299 00301 template <typename PointInT, typename PointOutT> void 00302 pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in, const std::vector<pcl::PointIndices> &indices, 00303 pcl::PointCloud<PointOutT> &cloud_out) 00304 { 00305 int nr_p = 0; 00306 for (size_t i = 0; i < indices.size (); ++i) 00307 nr_p += indices[i].indices.size (); 00308 00309 // Allocate enough space and copy the basics 00310 cloud_out.points.resize (nr_p); 00311 cloud_out.header = cloud_in.header; 00312 cloud_out.width = nr_p; 00313 cloud_out.height = 1; 00314 if (cloud_in.is_dense) 00315 cloud_out.is_dense = true; 00316 else 00317 // It's not necessarily true that is_dense is false if cloud_in.is_dense is false 00318 // To verify this, we would need to iterate over all points and check for NaNs 00319 cloud_out.is_dense = false; 00320 00321 // Copy all the data fields from the input cloud to the output one 00322 typedef typename pcl::traits::fieldList<PointInT>::type FieldListInT; 00323 typedef typename pcl::traits::fieldList<PointOutT>::type FieldListOutT; 00324 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00325 // Iterate over each cluster 00326 int cp = 0; 00327 for (size_t cc = 0; cc < indices.size (); ++cc) 00328 { 00329 // Iterate over each idx 00330 for (size_t i = 0; i < indices[cc].indices.size (); ++i) 00331 { 00332 // Iterate over each dimension 00333 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointInT, PointOutT> (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); 00334 ++cp; 00335 } 00336 } 00337 } 00338 00340 template <typename PointIn1T, typename PointIn2T, typename PointOutT> void 00341 pcl::concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in, 00342 const pcl::PointCloud<PointIn2T> &cloud2_in, 00343 pcl::PointCloud<PointOutT> &cloud_out) 00344 { 00345 typedef typename pcl::traits::fieldList<PointIn1T>::type FieldList1; 00346 typedef typename pcl::traits::fieldList<PointIn2T>::type FieldList2; 00347 00348 if (cloud1_in.points.size () != cloud2_in.points.size ()) 00349 { 00350 PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n"); 00351 return; 00352 } 00353 00354 // Resize the output dataset 00355 cloud_out.points.resize (cloud1_in.points.size ()); 00356 cloud_out.header = cloud1_in.header; 00357 cloud_out.width = cloud1_in.width; 00358 cloud_out.height = cloud1_in.height; 00359 if (!cloud1_in.is_dense || !cloud2_in.is_dense) 00360 cloud_out.is_dense = false; 00361 else 00362 cloud_out.is_dense = true; 00363 00364 // Iterate over each point 00365 for (size_t i = 0; i < cloud_out.points.size (); ++i) 00366 { 00367 // Iterate over each dimension 00368 pcl::for_each_type <FieldList1> (pcl::NdConcatenateFunctor <PointIn1T, PointOutT> (cloud1_in.points[i], cloud_out.points[i])); 00369 pcl::for_each_type <FieldList2> (pcl::NdConcatenateFunctor <PointIn2T, PointOutT> (cloud2_in.points[i], cloud_out.points[i])); 00370 } 00371 } 00372 00373 #endif // PCL_IO_IMPL_IO_H_ 00374