extract_indices_patch.h
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  *  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 #include <pcl/filters/impl/extract_indices.hpp>
00042 
00044 void
00045 pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
00046 {
00047   if (keep_organized_)
00048   {
00049     output = *input_;
00050     if (negative_)
00051     {
00052       // Prepare the output and copy the data
00053       for (size_t i = 0; i < indices_->size (); ++i)
00054         for (size_t j = 0; j < output.fields.size(); ++j)
00055           memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[j].offset],
00056                   &user_filter_value_, sizeof(float));
00057     }
00058     else
00059     {
00060       // Prepare a vector holding all indices
00061       std::vector<int> all_indices (input_->width * input_->height);
00062       for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
00063         all_indices[i] = i;
00064 
00065       std::vector<int> indices = *indices_;
00066       std::sort (indices.begin (), indices.end ());
00067 
00068       // Get the diference
00069       std::vector<int> remaining_indices;
00070       set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
00071                       inserter (remaining_indices, remaining_indices.begin ()));
00072 
00073       // Prepare the output and copy the data
00074       for (size_t i = 0; i < remaining_indices.size (); ++i)
00075         for (size_t j = 0; j < output.fields.size(); ++j)
00076           memcpy (&output.data[remaining_indices[i] * output.point_step + output.fields[j].offset],
00077                   &user_filter_value_, sizeof(float));
00078     }
00079     if (!pcl_isfinite (user_filter_value_))
00080       output.is_dense = false;
00081     return;
00082   }
00083   if (indices_->empty () || (input_->width * input_->height == 0))
00084   {
00085     output.width = output.height = 0;
00086     output.data.clear ();
00087     // If negative, copy all the data
00088     if (negative_)
00089       output = *input_;
00090     return;
00091   }
00092   if (indices_->size () == (input_->width * input_->height))
00093   {
00094     // If negative, then return an empty cloud
00095     if (negative_)
00096     {
00097       output.width = output.height = 0;
00098       output.data.clear ();
00099     }
00100     // else, we need to return all points
00101     else
00102       output = *input_;
00103     return;
00104   }
00105 
00106   // Copy the common fields (header and fields should have already been copied)
00107   output.is_bigendian = input_->is_bigendian;
00108   output.point_step   = input_->point_step;
00109   output.height       = 1;
00110   // TODO: check the output cloud and assign is_dense based on whether the points are valid or not
00111   output.is_dense     = false;
00112 
00113   if (negative_)
00114   {
00115     // Prepare a vector holding all indices
00116     std::vector<int> all_indices (input_->width * input_->height);
00117     for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
00118       all_indices[i] = i;
00119 
00120     std::vector<int> indices = *indices_;
00121     std::sort (indices.begin (), indices.end ());
00122 
00123     // Get the diference
00124     std::vector<int> remaining_indices;
00125     set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
00126                     inserter (remaining_indices, remaining_indices.begin ()));
00127 
00128     // Prepare the output and copy the data
00129     output.width = static_cast<uint32_t> (remaining_indices.size ());
00130     output.data.resize (remaining_indices.size () * output.point_step);
00131     for (size_t i = 0; i < remaining_indices.size (); ++i)
00132       memcpy (&output.data[i * output.point_step], &input_->data[remaining_indices[i] * output.point_step], output.point_step);
00133   }
00134   else
00135   {
00136     // Prepare the output and copy the data
00137     output.width = static_cast<uint32_t> (indices_->size ());
00138     output.data.resize (indices_->size () * output.point_step);
00139     for (size_t i = 0; i < indices_->size (); ++i)
00140       memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * output.point_step], output.point_step);
00141   }
00142   output.row_step = output.point_step * output.width;
00143 }
00144 
00146 void
00147 pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
00148 {
00149   if (negative_)
00150   {
00151     // If the subset is the full set
00152     if (indices_->size () == (input_->width * input_->height))
00153     {
00154       // Empty set copy
00155       indices.clear ();
00156       return;
00157     }
00158 
00159     // Set up the full indices set
00160     std::vector<int> indices_fullset (input_->width * input_->height);
00161     for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
00162       indices_fullset[p_it] = p_it;
00163 
00164     // If the subset is the empty set
00165     if (indices_->empty () || (input_->width * input_->height == 0))
00166     {
00167       // Full set copy
00168       indices = indices_fullset;
00169       return;
00170     }
00171 
00172     // If the subset is a proper subset
00173     // Set up the subset input indices
00174     std::vector<int> indices_subset = *indices_;
00175     std::sort (indices_subset.begin (), indices_subset.end ());
00176 
00177     // Get the difference
00178     set_difference (indices_fullset.begin (), indices_fullset.end (), indices_subset.begin (), indices_subset.end (), inserter (indices, indices.begin ()));
00179   }
00180   else
00181     indices = *indices_;
00182 }
00183 
00184 #ifndef PCL_NO_PRECOMPILE
00185 #include <pcl/impl/instantiate.hpp>
00186 #include <pcl/point_types.h>
00187 
00188 #ifdef PCL_ONLY_CORE_POINT_TYPES
00189   PCL_INSTANTIATE(ExtractIndices, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::Normal)(pcl::PointXYZRGBNormal))
00190 #else
00191   PCL_INSTANTIATE(ExtractIndices, PCL_POINT_TYPES)
00192 #endif
00193 
00194 #endif    // PCL_NO_PRECOMPILE


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49