Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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 */ 00037 00038 #ifndef PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_ 00039 #define PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_ 00040 00041 #include <pcl/filters/normal_space.h> 00042 #include <pcl/common/io.h> 00043 00044 #include <vector> 00045 #include <list> 00046 00047 /////////////////////////////////////////////////////////////////////////////// 00048 template<typename PointT, typename NormalT> bool 00049 pcl::NormalSpaceSampling<PointT, NormalT>::initCompute () 00050 { 00051 if (!FilterIndices<PointT>::initCompute ()) 00052 return false; 00053 00054 // If sample size is 0 or if the sample size is greater then input cloud size then return entire copy of cloud 00055 if (sample_ >= input_->size ()) 00056 { 00057 PCL_ERROR ("[NormalSpaceSampling::initCompute] Requested more samples than the input cloud size: %d vs %zu\n", 00058 sample_, input_->size ()); 00059 return false; 00060 } 00061 00062 boost::mt19937 rng (static_cast<unsigned int> (seed_)); 00063 boost::uniform_int<unsigned int> uniform_distrib (0, unsigned (input_->size ())); 00064 if (rng_uniform_distribution_ != NULL) 00065 delete rng_uniform_distribution_; 00066 rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned int> > (rng, uniform_distrib); 00067 00068 return (true); 00069 } 00070 00071 /////////////////////////////////////////////////////////////////////////////// 00072 template<typename PointT, typename NormalT> void 00073 pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (PointCloud &output) 00074 { 00075 std::vector<int> indices; 00076 if (keep_organized_) 00077 { 00078 bool temp = extract_removed_indices_; 00079 extract_removed_indices_ = true; 00080 applyFilter (indices); 00081 extract_removed_indices_ = temp; 00082 00083 output = *input_; 00084 for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator 00085 output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_; 00086 if (!pcl_isfinite (user_filter_value_)) 00087 output.is_dense = false; 00088 } 00089 else 00090 { 00091 output.is_dense = true; 00092 applyFilter (indices); 00093 pcl::copyPointCloud (*input_, indices, output); 00094 } 00095 } 00096 00097 /////////////////////////////////////////////////////////////////////////////// 00098 template<typename PointT, typename NormalT> bool 00099 pcl::NormalSpaceSampling<PointT, NormalT>::isEntireBinSampled (boost::dynamic_bitset<> &array, 00100 unsigned int start_index, 00101 unsigned int length) 00102 { 00103 bool status = true; 00104 for (unsigned int i = start_index; i < start_index + length; i++) 00105 { 00106 status = status & array.test (i); 00107 } 00108 return status; 00109 } 00110 00111 /////////////////////////////////////////////////////////////////////////////// 00112 template<typename PointT, typename NormalT> unsigned int 00113 pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal, unsigned int) 00114 { 00115 unsigned int bin_number = 0; 00116 // Holds the bin numbers for direction cosines in x,y,z directions 00117 unsigned int t[3] = {0,0,0}; 00118 00119 // dcos is the direction cosine. 00120 float dcos = 0.0; 00121 float bin_size = 0.0; 00122 // max_cos and min_cos are the maximum and minimum values of cos(theta) respectively 00123 float max_cos = 1.0; 00124 float min_cos = -1.0; 00125 00126 // dcos = cosf (normal[0]); 00127 dcos = normal[0]; 00128 bin_size = (max_cos - min_cos) / static_cast<float> (binsx_); 00129 00130 // Finding bin number for direction cosine in x direction 00131 unsigned int k = 0; 00132 for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) 00133 { 00134 if (dcos >= i && dcos <= (i+bin_size)) 00135 { 00136 break; 00137 } 00138 } 00139 t[0] = k; 00140 00141 // dcos = cosf (normal[1]); 00142 dcos = normal[1]; 00143 bin_size = (max_cos - min_cos) / static_cast<float> (binsy_); 00144 00145 // Finding bin number for direction cosine in y direction 00146 k = 0; 00147 for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) 00148 { 00149 if (dcos >= i && dcos <= (i+bin_size)) 00150 { 00151 break; 00152 } 00153 } 00154 t[1] = k; 00155 00156 // dcos = cosf (normal[2]); 00157 dcos = normal[2]; 00158 bin_size = (max_cos - min_cos) / static_cast<float> (binsz_); 00159 00160 // Finding bin number for direction cosine in z direction 00161 k = 0; 00162 for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) 00163 { 00164 if (dcos >= i && dcos <= (i+bin_size)) 00165 { 00166 break; 00167 } 00168 } 00169 t[2] = k; 00170 00171 bin_number = t[0] * (binsy_*binsz_) + t[1] * binsz_ + t[2]; 00172 return bin_number; 00173 } 00174 00175 /////////////////////////////////////////////////////////////////////////////// 00176 template<typename PointT, typename NormalT> void 00177 pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (std::vector<int> &indices) 00178 { 00179 if (!initCompute ()) 00180 { 00181 indices = *indices_; 00182 return; 00183 } 00184 00185 unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ())); 00186 // Resize output indices to sample size 00187 indices.resize (max_values); 00188 removed_indices_->resize (max_values); 00189 00190 // Allocate memory for the histogram of normals. Normals will then be sampled from each bin. 00191 unsigned int n_bins = binsx_ * binsy_ * binsz_; 00192 // list<int> holds the indices of points in that bin. Using list to avoid repeated resizing of vectors. 00193 // Helps when the point cloud is large. 00194 std::vector<std::list <int> > normals_hg; 00195 normals_hg.reserve (n_bins); 00196 for (unsigned int i = 0; i < n_bins; i++) 00197 normals_hg.push_back (std::list<int> ()); 00198 00199 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it) 00200 { 00201 unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins); 00202 normals_hg[bin_number].push_back (*it); 00203 } 00204 00205 00206 // Setting up random access for the list created above. Maintaining the iterators to individual elements of the list 00207 // in a vector. Using vector now as the size of the list is known. 00208 std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ()); 00209 for (unsigned int i = 0; i < normals_hg.size (); i++) 00210 { 00211 random_access.push_back (std::vector<std::list<int>::iterator> ()); 00212 random_access[i].resize (normals_hg[i].size ()); 00213 00214 unsigned int j = 0; 00215 for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++) 00216 random_access[i][j] = itr; 00217 } 00218 std::vector<unsigned int> start_index (normals_hg.size ()); 00219 start_index[0] = 0; 00220 unsigned int prev_index = start_index[0]; 00221 for (unsigned int i = 1; i < normals_hg.size (); i++) 00222 { 00223 start_index[i] = prev_index + static_cast<unsigned int> (normals_hg[i-1].size ()); 00224 prev_index = start_index[i]; 00225 } 00226 00227 // Maintaining flags to check if a point is sampled 00228 boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ()); 00229 // Maintaining flags to check if all points in the bin are sampled 00230 boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ()); 00231 unsigned int i = 0; 00232 while (i < sample_) 00233 { 00234 // Iterating through every bin and picking one point at random, until the required number of points are sampled. 00235 for (unsigned int j = 0; j < normals_hg.size (); j++) 00236 { 00237 unsigned int M = static_cast<unsigned int> (normals_hg[j].size ()); 00238 if (M == 0 || bin_empty_flag.test (j)) // bin_empty_flag(i) is set if all points in that bin are sampled.. 00239 continue; 00240 00241 unsigned int pos = 0; 00242 unsigned int random_index = 0; 00243 00244 // Picking up a sample at random from jth bin 00245 do 00246 { 00247 random_index = static_cast<unsigned int> ((*rng_uniform_distribution_) () % M); 00248 pos = start_index[j] + random_index; 00249 } while (is_sampled_flag.test (pos)); 00250 00251 is_sampled_flag.flip (start_index[j] + random_index); 00252 00253 // Checking if all points in bin j are sampled. 00254 if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ()))) 00255 bin_empty_flag.flip (j); 00256 00257 unsigned int index = *(random_access[j][random_index]); 00258 indices[i] = index; 00259 i++; 00260 if (i == sample_) 00261 break; 00262 } 00263 } 00264 00265 // If we need to return the indices that we haven't sampled 00266 if (extract_removed_indices_) 00267 { 00268 std::vector<int> indices_temp = indices; 00269 std::sort (indices_temp.begin (), indices_temp.end ()); 00270 00271 std::vector<int> all_indices_temp = *indices_; 00272 std::sort (all_indices_temp.begin (), all_indices_temp.end ()); 00273 set_difference (all_indices_temp.begin (), all_indices_temp.end (), 00274 indices_temp.begin (), indices_temp.end (), 00275 inserter (*removed_indices_, removed_indices_->begin ())); 00276 } 00277 } 00278 00279 #define PCL_INSTANTIATE_NormalSpaceSampling(T,NT) template class PCL_EXPORTS pcl::NormalSpaceSampling<T,NT>; 00280 00281 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_