Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * Author: Suat Gedikli (gedikli@willowgarage.com) 00035 * 00036 */ 00037 00038 #ifndef PCL_FILTERS_IMPL_COLOR_H_ 00039 #define PCL_FILTERS_IMPL_COLOR_H_ 00040 00041 #include "pcl/filters/color.h" 00042 #include "pcl/common/io.h" 00043 00045 template <typename PointT> void 00046 pcl::ColorFilter<PointT>::applyFilter (PointCloud &output) 00047 { 00048 // Has the input dataset been set already? 00049 if (!input_) 00050 { 00051 PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); 00052 output.width = output.height = 0; 00053 output.points.clear (); 00054 return; 00055 } 00056 // Check if we're going to keep the organized structure of the cloud or not 00057 if (keep_organized_) 00058 { 00059 output.width = input_->width; 00060 output.height = input_->height; 00061 output.is_dense = false; 00062 } 00063 else 00064 { 00065 output.height = 1; 00066 output.is_dense = input_->is_dense; 00067 } 00068 output.points.resize (input_->points.size ()); 00069 removed_indices_->resize (input_->points.size ()); 00070 00071 int nr_p = 0; 00072 int nr_removed_p = 0; 00073 float nan = std::numeric_limits<float>::quiet_NaN (); 00074 00075 if (keep_organized_) 00076 { 00077 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00078 { 00079 if (lookup_[input_->points[cp].rgba]) 00080 output.points [cp] = input_->points[cp]; 00081 else 00082 { 00083 output.points [cp].x = output.points[cp].y = output.points[cp].z = nan; 00084 // dont loose color information 00085 output.points [cp].rgba = input_->points[cp].rgba; 00086 } 00087 } 00088 nr_p = input_->points.size (); 00089 } 00090 else // Remove filtered points 00091 { 00092 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00093 { 00094 if (lookup_[input_->points[cp].rgba]) 00095 output.points [nr_p++] = input_->points[cp]; 00096 else if (extract_removed_indices_) 00097 (*removed_indices_)[nr_removed_p++] = cp; 00098 } 00099 output.width = nr_p; 00100 } // !keep_organized_ 00101 output.points.resize (output.width * output.height); 00102 removed_indices_->resize(nr_removed_p); 00103 } 00104 00105 #define PCL_INSTANTIATE_ColorFilter(T) template class PCL_EXPORTS pcl::ColorFilter<T>; 00106 00107 #endif // PCL_FILTERS_IMPL_COLOR_H_ 00108