Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, 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 the copyright holder(s) 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: extract_indices.hpp 1897 2011-07-26 20:35:49Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FILTERS_IMPL_RANDOM_SAMPLE_H_ 00039 #define PCL_FILTERS_IMPL_RANDOM_SAMPLE_H_ 00040 00041 #include <pcl/filters/random_sample.h> 00042 #include <pcl/common/io.h> 00043 #include <pcl/point_traits.h> 00044 00045 00046 /////////////////////////////////////////////////////////////////////////////// 00047 template<typename PointT> void 00048 pcl::RandomSample<PointT>::applyFilter (PointCloud &output) 00049 { 00050 std::vector<int> indices; 00051 if (keep_organized_) 00052 { 00053 bool temp = extract_removed_indices_; 00054 extract_removed_indices_ = true; 00055 applyFilter (indices); 00056 extract_removed_indices_ = temp; 00057 copyPointCloud (*input_, output); 00058 // Get X, Y, Z fields 00059 std::vector<pcl::PCLPointField> fields; 00060 pcl::getFields (*input_, fields); 00061 std::vector<size_t> offsets; 00062 for (size_t i = 0; i < fields.size (); ++i) 00063 { 00064 if (fields[i].name == "x" || 00065 fields[i].name == "y" || 00066 fields[i].name == "z") 00067 offsets.push_back (fields[i].offset); 00068 } 00069 // For every "removed" point, set the x,y,z fields to user_filter_value_ 00070 const static float user_filter_value = user_filter_value_; 00071 for (size_t rii = 0; rii < removed_indices_->size (); ++rii) 00072 { 00073 uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output[(*removed_indices_)[rii]]); 00074 for (size_t i = 0; i < offsets.size (); ++i) 00075 { 00076 memcpy (pt_data + offsets[i], &user_filter_value, sizeof (float)); 00077 } 00078 if (!pcl_isfinite (user_filter_value_)) 00079 output.is_dense = false; 00080 } 00081 } 00082 else 00083 { 00084 output.is_dense = true; 00085 applyFilter (indices); 00086 copyPointCloud (*input_, indices, output); 00087 } 00088 } 00089 00090 /////////////////////////////////////////////////////////////////////////////// 00091 template<typename PointT> 00092 void 00093 pcl::RandomSample<PointT>::applyFilter (std::vector<int> &indices) 00094 { 00095 unsigned N = static_cast<unsigned> (indices_->size ()); 00096 00097 unsigned int sample_size = negative_ ? N - sample_ : sample_; 00098 // If sample size is 0 or if the sample size is greater then input cloud size 00099 // then return all indices 00100 if (sample_size >= N) 00101 { 00102 indices = *indices_; 00103 removed_indices_->clear (); 00104 } 00105 else 00106 { 00107 // Resize output indices to sample size 00108 indices.resize (static_cast<size_t> (sample_size)); 00109 if (extract_removed_indices_) 00110 removed_indices_->resize (static_cast<size_t> (N - sample_size)); 00111 00112 // Set random seed so derived indices are the same each time the filter runs 00113 std::srand (seed_); 00114 00115 // Algorithm A 00116 unsigned top = N - sample_size; 00117 unsigned i = 0; 00118 unsigned index = 0; 00119 std::vector<bool> added; 00120 if (extract_removed_indices_) 00121 added.resize (indices_->size (), false); 00122 for (size_t n = sample_size; n >= 2; n--) 00123 { 00124 float V = unifRand (); 00125 unsigned S = 0; 00126 float quot = static_cast<float> (top) / static_cast<float> (N); 00127 while (quot > V) 00128 { 00129 S++; 00130 top--; 00131 N--; 00132 quot = quot * static_cast<float> (top) / static_cast<float> (N); 00133 } 00134 index += S; 00135 if (extract_removed_indices_) 00136 added[index] = true; 00137 indices[i++] = (*indices_)[index++]; 00138 N--; 00139 } 00140 00141 index += N * static_cast<unsigned> (unifRand ()); 00142 if (extract_removed_indices_) 00143 added[index] = true; 00144 indices[i++] = (*indices_)[index++]; 00145 00146 // Now populate removed_indices_ appropriately 00147 if (extract_removed_indices_) 00148 { 00149 unsigned ri = 0; 00150 for (size_t i = 0; i < added.size (); i++) 00151 { 00152 if (!added[i]) 00153 { 00154 (*removed_indices_)[ri++] = (*indices_)[i]; 00155 } 00156 } 00157 } 00158 } 00159 } 00160 00161 #define PCL_INSTANTIATE_RandomSample(T) template class PCL_EXPORTS pcl::RandomSample<T>; 00162 00163 #endif // PCL_FILTERS_IMPL_RANDOM_SAMPLE_H_