Point Cloud Library (PCL)  1.3.1
io.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines