Point Cloud Library (PCL)
1.3.1
|
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 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: uniform_sampling.hpp 2414 2011-09-07 07:01:06Z svn $ 00035 * 00036 */ 00037 00038 #ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_ 00039 #define PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_ 00040 00041 #include "pcl/common/common.h" 00042 #include "pcl/keypoints/uniform_sampling.h" 00043 00045 template <typename PointInT> void 00046 pcl::UniformSampling<PointInT>::detectKeypoints (PointCloudOut &output) 00047 { 00048 // Has the input dataset been set already? 00049 if (!input_) 00050 { 00051 PCL_WARN ("[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ()); 00052 output.width = output.height = 0; 00053 output.points.clear (); 00054 return; 00055 } 00056 00057 output.height = 1; // downsampling breaks the organized structure 00058 output.is_dense = true; // we filter out invalid points 00059 00060 Eigen::Vector4f min_p, max_p; 00061 // Get the minimum and maximum dimensions 00062 pcl::getMinMax3D<PointInT>(*input_, min_p, max_p); 00063 00064 // Compute the minimum and maximum bounding box values 00065 min_b_[0] = (int)(floor (min_p[0] * inverse_leaf_size_[0])); 00066 max_b_[0] = (int)(floor (max_p[0] * inverse_leaf_size_[0])); 00067 min_b_[1] = (int)(floor (min_p[1] * inverse_leaf_size_[1])); 00068 max_b_[1] = (int)(floor (max_p[1] * inverse_leaf_size_[1])); 00069 min_b_[2] = (int)(floor (min_p[2] * inverse_leaf_size_[2])); 00070 max_b_[2] = (int)(floor (max_p[2] * inverse_leaf_size_[2])); 00071 00072 // Compute the number of divisions needed along all axis 00073 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); 00074 div_b_[3] = 0; 00075 00076 // Clear the leaves 00077 leaves_.clear (); 00078 00079 // Set up the division multiplier 00080 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); 00081 00082 // First pass: build a set of leaves with the point index closest to the leaf center 00083 for (size_t cp = 0; cp < indices_->size (); ++cp) 00084 { 00085 if (!input_->is_dense) 00086 // Check if the point is invalid 00087 if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) || 00088 !pcl_isfinite (input_->points[(*indices_)[cp]].y) || 00089 !pcl_isfinite (input_->points[(*indices_)[cp]].z)) 00090 continue; 00091 00092 Eigen::Vector4i ijk = Eigen::Vector4i::Zero (); 00093 ijk[0] = (int)(floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0])); 00094 ijk[1] = (int)(floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1])); 00095 ijk[2] = (int)(floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2])); 00096 00097 // Compute the leaf index 00098 int idx = (ijk - min_b_).dot (divb_mul_); 00099 Leaf& leaf = leaves_[idx]; 00100 // First time we initialize the index 00101 if (leaf.idx == -1) 00102 { 00103 leaf.idx = (*indices_)[cp]; 00104 continue; 00105 } 00106 00107 // Check to see if this point is closer to the leaf center than the previous one we saved 00108 float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm (); 00109 float diff_prev = (input_->points[leaf.idx].getVector4fMap () - ijk.cast<float> ()).squaredNorm (); 00110 00111 // If current point is closer, copy its index instead 00112 if (diff_cur < diff_prev) 00113 leaf.idx = (*indices_)[cp]; 00114 } 00115 00116 // Second pass: go over all leaves and copy data 00117 output.points.resize (leaves_.size ()); 00118 int cp = 0; 00119 00120 for (typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it) 00121 output.points[cp++] = it->second.idx; 00122 output.width = output.points.size (); 00123 } 00124 00125 #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>; 00126 00127 #endif // PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_ 00128