Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, 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 * $Id: passthrough.hpp 3317 2011-12-02 11:17:47Z mdixon $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FILTERS_IMPL_PASSTHROUGH_H_ 00039 #define PCL_FILTERS_IMPL_PASSTHROUGH_H_ 00040 00041 #include "pcl/filters/passthrough.h" 00042 #include "pcl/common/io.h" 00043 00045 template <typename PointT> void 00046 pcl::PassThrough<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 00057 // Check if we're going to keep the organized structure of the cloud or not 00058 if (keep_organized_) 00059 { 00060 if (filter_field_name_.empty ()) 00061 { 00062 // Silly - if no filtering is actually done, and we want to keep the data organized, 00063 // just copy everything. Any optimizations that can be done here??? 00064 output = *input_; 00065 return; 00066 } 00067 00068 output.width = input_->width; 00069 output.height = input_->height; 00070 // Check what the user value is: if !finite, set is_dense to false, true otherwise 00071 if (!pcl_isfinite (user_filter_value_)) 00072 output.is_dense = false; 00073 else 00074 output.is_dense = input_->is_dense; 00075 } 00076 else 00077 { 00078 output.height = 1; // filtering breaks the organized structure 00079 // Because we're doing explit checks for isfinite, is_dense = true 00080 output.is_dense = true; 00081 } 00082 output.points.resize (input_->points.size ()); 00083 removed_indices_->resize (input_->points.size ()); 00084 00085 int nr_p = 0; 00086 int nr_removed_p = 0; 00087 00088 // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... 00089 if (!filter_field_name_.empty ()) 00090 { 00091 // Get the distance field index 00092 std::vector<sensor_msgs::PointField> fields; 00093 int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields); 00094 if (distance_idx == -1) 00095 { 00096 PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); 00097 output.width = output.height = 0; 00098 output.points.clear (); 00099 return; 00100 } 00101 00102 if (keep_organized_) 00103 { 00104 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00105 { 00106 if (pcl_isnan (input_->points[cp].x) || 00107 pcl_isnan (input_->points[cp].y) || 00108 pcl_isnan (input_->points[cp].z)) 00109 { 00110 output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_; 00111 continue; 00112 } 00113 00114 // Copy the point 00115 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[cp])); 00116 00117 // Filter it. Get the distance value 00118 uint8_t* pt_data = (uint8_t*)&input_->points[cp]; 00119 float distance_value = 0; 00120 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00121 00122 if (filter_limit_negative_) 00123 { 00124 // Use a threshold for cutting out points which inside the interval 00125 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) 00126 { 00127 output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_; 00128 continue; 00129 } 00130 else 00131 { 00132 if (extract_removed_indices_) 00133 { 00134 (*removed_indices_)[nr_removed_p]=cp; 00135 nr_removed_p++; 00136 } 00137 } 00138 } 00139 else 00140 { 00141 // Use a threshold for cutting out points which are too close/far away 00142 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) 00143 { 00144 output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_; 00145 continue; 00146 } 00147 else 00148 { 00149 if (extract_removed_indices_) 00150 { 00151 (*removed_indices_)[nr_removed_p] = cp; 00152 nr_removed_p++; 00153 } 00154 } 00155 } 00156 } 00157 nr_p = input_->points.size (); 00158 } 00159 // Remove filtered points 00160 else 00161 { 00162 // Go over all points 00163 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00164 { 00165 // Check if the point is invalid 00166 if (!pcl_isfinite (input_->points[cp].x) || !pcl_isfinite (input_->points[cp].y) || !pcl_isfinite (input_->points[cp].z)) 00167 { 00168 if (extract_removed_indices_) 00169 { 00170 (*removed_indices_)[nr_removed_p] = cp; 00171 nr_removed_p++; 00172 } 00173 continue; 00174 } 00175 00176 // Get the distance value 00177 uint8_t* pt_data = (uint8_t*)&input_->points[cp]; 00178 float distance_value = 0; 00179 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00180 00181 if (filter_limit_negative_) 00182 { 00183 // Use a threshold for cutting out points which inside the interval 00184 if (distance_value < filter_limit_max_ && distance_value > filter_limit_min_) 00185 { 00186 if (extract_removed_indices_) 00187 { 00188 (*removed_indices_)[nr_removed_p] = cp; 00189 nr_removed_p++; 00190 } 00191 continue; 00192 } 00193 } 00194 else 00195 { 00196 // Use a threshold for cutting out points which are too close/far away 00197 if (distance_value > filter_limit_max_ || distance_value < filter_limit_min_) 00198 { 00199 if (extract_removed_indices_) 00200 { 00201 (*removed_indices_)[nr_removed_p] = cp; 00202 nr_removed_p++; 00203 } 00204 continue; 00205 } 00206 } 00207 00208 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[nr_p])); 00209 nr_p++; 00210 } 00211 output.width = nr_p; 00212 } // !keep_organized_ 00213 } 00214 // No distance filtering, process all data. No need to check for is_organized here as we did it above 00215 else 00216 { 00217 // First pass: go over all points and insert them into the right leaf 00218 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00219 { 00220 // Check if the point is invalid 00221 if (!pcl_isfinite (input_->points[cp].x) || !pcl_isfinite (input_->points[cp].y) || !pcl_isfinite (input_->points[cp].z)) 00222 { 00223 if (extract_removed_indices_) 00224 { 00225 (*removed_indices_)[nr_removed_p] = cp; 00226 nr_removed_p++; 00227 } 00228 continue; 00229 } 00230 00231 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[nr_p])); 00232 nr_p++; 00233 } 00234 output.width = nr_p; 00235 } 00236 00237 output.points.resize (output.width * output.height); 00238 removed_indices_->resize(nr_removed_p); 00239 } 00240 00241 #define PCL_INSTANTIATE_PassThrough(T) template class PCL_EXPORTS pcl::PassThrough<T>; 00242 00243 #endif // PCL_FILTERS_IMPL_PASSTHROUGH_H_ 00244