extract_indices.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-2012, 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: extract_indices.hpp 6144 2012-07-04 22:06:28Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
00041 #define PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
00042 
00043 #include <pcl/filters/extract_indices.h>
00044 #include <pcl/common/io.h>
00045 
00047 template <typename PointT> void
00048 pcl::ExtractIndices<PointT>::filterDirectly (PointCloudPtr &cloud)
00049 {
00050   std::vector<int> indices;
00051   bool temp = extract_removed_indices_;
00052   extract_removed_indices_ = true;
00053   this->setInputCloud (cloud);
00054   applyFilterIndices (indices);
00055   extract_removed_indices_ = temp;
00056 
00057   std::vector<sensor_msgs::PointField> fields; 
00058   pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
00059   for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
00060   {
00061     uint8_t* pt_data = reinterpret_cast<uint8_t*> (&cloud->points[(*removed_indices_)[rii]]);
00062     for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi)  // fi = field iterator
00063       memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
00064   }
00065   if (!pcl_isfinite (user_filter_value_))
00066     cloud->is_dense = false;
00067 }
00068 
00070 template <typename PointT> void
00071 pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
00072 {
00073   std::vector<int> indices;
00074   if (keep_organized_)
00075   {
00076     bool temp = extract_removed_indices_;
00077     extract_removed_indices_ = true;
00078     applyFilterIndices (indices);
00079     extract_removed_indices_ = temp;
00080 
00081     output = *input_;
00082     std::vector<sensor_msgs::PointField> fields; 
00083     pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
00084     for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
00085     {
00086       uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output.points[(*removed_indices_)[rii]]);
00087       for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi)  // fi = field iterator
00088         memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
00089     }
00090     if (!pcl_isfinite (user_filter_value_))
00091       output.is_dense = false;
00092   }
00093   else
00094   {
00095     applyFilterIndices (indices);
00096     copyPointCloud (*input_, indices, output);
00097   }
00098 }
00099 
00101 template <typename PointT> void
00102 pcl::ExtractIndices<PointT>::applyFilterIndices (std::vector<int> &indices)
00103 {
00104   if (indices_->size () > input_->points.size ())
00105   {
00106     PCL_ERROR ("[pcl::%s::applyFilter] The indices size exceeds the size of the input.\n", getClassName ().c_str ());
00107     indices.clear ();
00108     removed_indices_->clear ();
00109     return;
00110   }
00111 
00112   if (!negative_)  // Normal functionality
00113   {
00114     indices = *indices_;
00115 
00116     if (extract_removed_indices_)
00117     {
00118       // Set up the full indices set
00119       std::vector<int> full_indices (input_->points.size ());
00120       for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii)  // fii = full indices iterator
00121         full_indices[fii] = fii;
00122 
00123       // Set up the sorted input indices
00124       std::vector<int> sorted_input_indices = *indices_;
00125       std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
00126 
00127       // Store the difference in removed_indices
00128       removed_indices_->clear ();
00129       set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (*removed_indices_, removed_indices_->begin ()));
00130     }
00131   }
00132   else  // Inverted functionality
00133   {
00134     // Set up the full indices set
00135     std::vector<int> full_indices (input_->points.size ());
00136     for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii)  // fii = full indices iterator
00137       full_indices[fii] = fii;
00138 
00139     // Set up the sorted input indices
00140     std::vector<int> sorted_input_indices = *indices_;
00141     std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
00142 
00143     // Store the difference in indices
00144     indices.clear ();
00145     set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (indices, indices.begin ()));
00146 
00147     if (extract_removed_indices_)
00148       removed_indices_ = indices_;
00149   }
00150 }
00151 
00152 #define PCL_INSTANTIATE_ExtractIndices(T) template class PCL_EXPORTS pcl::ExtractIndices<T>;
00153 
00154 #endif  // PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
00155 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:59