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) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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_SIFT_KEYPOINT_IMPL_H_ 00039 #define PCL_SIFT_KEYPOINT_IMPL_H_ 00040 00041 #include <pcl/keypoints/sift_keypoint.h> 00042 #include <pcl/common/io.h> 00043 #include <pcl/filters/voxel_grid.h> 00044 00045 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointInT, typename PointOutT> void 00047 pcl::SIFTKeypoint<PointInT, PointOutT>::setScales (float min_scale, int nr_octaves, int nr_scales_per_octave) 00048 { 00049 min_scale_ = min_scale; 00050 nr_octaves_ = nr_octaves; 00051 nr_scales_per_octave_ = nr_scales_per_octave; 00052 } 00053 00054 00055 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00056 template <typename PointInT, typename PointOutT> void 00057 pcl::SIFTKeypoint<PointInT, PointOutT>::setMinimumContrast (float min_contrast) 00058 { 00059 min_contrast_ = min_contrast; 00060 } 00061 00062 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00063 template <typename PointInT, typename PointOutT> bool 00064 pcl::SIFTKeypoint<PointInT, PointOutT>::initCompute () 00065 { 00066 if (min_scale_ <= 0) 00067 { 00068 PCL_ERROR ("[pcl::%s::initCompute] : Minimum scale (%f) must be strict positive!\n", 00069 name_.c_str (), min_scale_); 00070 return (false); 00071 } 00072 if (nr_octaves_ < 1) 00073 { 00074 PCL_ERROR ("[pcl::%s::initCompute] : Number of octaves (%d) must be at least 1!\n", 00075 name_.c_str (), nr_octaves_); 00076 return (false); 00077 } 00078 if (nr_scales_per_octave_ < 1) 00079 { 00080 PCL_ERROR ("[pcl::%s::initCompute] : Number of scales per octave (%d) must be at least 1!\n", 00081 name_.c_str (), nr_scales_per_octave_); 00082 return (false); 00083 } 00084 if (min_contrast_ < 0) 00085 { 00086 PCL_ERROR ("[pcl::%s::initCompute] : Minimum contrast (%f) must be non-negative!\n", 00087 name_.c_str (), min_contrast_); 00088 return (false); 00089 } 00090 00091 this->setKSearch (1); 00092 tree_.reset (new pcl::search::KdTree<PointInT> (true)); 00093 return (true); 00094 } 00095 00096 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00097 template <typename PointInT, typename PointOutT> void 00098 pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output) 00099 { 00100 if (surface_ && surface_ != input_) 00101 { 00102 PCL_WARN ("[pcl::%s::detectKeypoints] : ", name_.c_str ()); 00103 PCL_WARN ("A search surface has been set by setSearchSurface, but this SIFT keypoint detection algorithm does "); 00104 PCL_WARN ("not support search surfaces other than the input cloud. "); 00105 PCL_WARN ("The cloud provided in setInputCloud is being used instead.\n"); 00106 } 00107 00108 // Check if the output has a "scale" field 00109 scale_idx_ = pcl::getFieldIndex<PointOutT> (output, "scale", out_fields_); 00110 00111 // Make sure the output cloud is empty 00112 output.points.clear (); 00113 00114 // Create a local copy of the input cloud that will be resized for each octave 00115 boost::shared_ptr<pcl::PointCloud<PointInT> > cloud (new pcl::PointCloud<PointInT> (*input_)); 00116 00117 VoxelGrid<PointInT> voxel_grid; 00118 // Search for keypoints at each octave 00119 float scale = min_scale_; 00120 for (int i_octave = 0; i_octave < nr_octaves_; ++i_octave) 00121 { 00122 // Downsample the point cloud 00123 const float s = 1.0f * scale; // note: this can be adjusted 00124 voxel_grid.setLeafSize (s, s, s); 00125 voxel_grid.setInputCloud (cloud); 00126 boost::shared_ptr<pcl::PointCloud<PointInT> > temp (new pcl::PointCloud<PointInT>); 00127 voxel_grid.filter (*temp); 00128 cloud = temp; 00129 00130 // Make sure the downsampled cloud still has enough points 00131 const size_t min_nr_points = 25; 00132 if (cloud->points.size () < min_nr_points) 00133 break; 00134 00135 // Update the KdTree with the downsampled points 00136 tree_->setInputCloud (cloud); 00137 00138 // Detect keypoints for the current scale 00139 detectKeypointsForOctave (*cloud, *tree_, scale, nr_scales_per_octave_, output); 00140 00141 // Increase the scale by another octave 00142 scale *= 2; 00143 } 00144 00145 output.height = 1; 00146 output.width = static_cast<uint32_t> (output.points.size ()); 00147 } 00148 00149 00150 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00151 template <typename PointInT, typename PointOutT> void 00152 pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypointsForOctave ( 00153 const PointCloudIn &input, KdTree &tree, float base_scale, int nr_scales_per_octave, 00154 PointCloudOut &output) 00155 { 00156 // Compute the difference of Gaussians (DoG) scale space 00157 std::vector<float> scales (nr_scales_per_octave + 3); 00158 for (int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale) 00159 { 00160 scales[i_scale] = base_scale * powf (2.0f, (1.0f * static_cast<float> (i_scale) - 1.0f) / static_cast<float> (nr_scales_per_octave)); 00161 } 00162 Eigen::MatrixXf diff_of_gauss; 00163 computeScaleSpace (input, tree, scales, diff_of_gauss); 00164 00165 // Find extrema in the DoG scale space 00166 std::vector<int> extrema_indices, extrema_scales; 00167 findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales); 00168 00169 output.points.reserve (output.points.size () + extrema_indices.size ()); 00170 // Save scale? 00171 if (scale_idx_ != -1) 00172 { 00173 // Add keypoints to output 00174 for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint) 00175 { 00176 PointOutT keypoint; 00177 const int &keypoint_index = extrema_indices[i_keypoint]; 00178 00179 keypoint.x = input.points[keypoint_index].x; 00180 keypoint.y = input.points[keypoint_index].y; 00181 keypoint.z = input.points[keypoint_index].z; 00182 memcpy (reinterpret_cast<char*> (&keypoint) + out_fields_[scale_idx_].offset, 00183 &scales[extrema_scales[i_keypoint]], sizeof (float)); 00184 output.points.push_back (keypoint); 00185 } 00186 } 00187 else 00188 { 00189 // Add keypoints to output 00190 for (size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint) 00191 { 00192 PointOutT keypoint; 00193 const int &keypoint_index = extrema_indices[i_keypoint]; 00194 00195 keypoint.x = input.points[keypoint_index].x; 00196 keypoint.y = input.points[keypoint_index].y; 00197 keypoint.z = input.points[keypoint_index].z; 00198 00199 output.points.push_back (keypoint); 00200 } 00201 } 00202 } 00203 00204 00205 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00206 template <typename PointInT, typename PointOutT> 00207 void pcl::SIFTKeypoint<PointInT, PointOutT>::computeScaleSpace ( 00208 const PointCloudIn &input, KdTree &tree, const std::vector<float> &scales, 00209 Eigen::MatrixXf &diff_of_gauss) 00210 { 00211 diff_of_gauss.resize (input.size (), scales.size () - 1); 00212 00213 // For efficiency, we will only filter over points within 3 standard deviations 00214 const float max_radius = 3.0f * scales.back (); 00215 00216 for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point) 00217 { 00218 std::vector<int> nn_indices; 00219 std::vector<float> nn_dist; 00220 tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // * 00221 // * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale, 00222 // regardless of the configurable search method specified by the user, so we directly employ tree.radiusSearch 00223 // here instead of using searchForNeighbors. 00224 00225 // For each scale, compute the Gaussian "filter response" at the current point 00226 float filter_response = 0.0f; 00227 float previous_filter_response; 00228 for (size_t i_scale = 0; i_scale < scales.size (); ++i_scale) 00229 { 00230 float sigma_sqr = powf (scales[i_scale], 2.0f); 00231 00232 float numerator = 0.0f; 00233 float denominator = 0.0f; 00234 for (size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor) 00235 { 00236 const float &value = getFieldValue_ (input.points[nn_indices[i_neighbor]]); 00237 const float &dist_sqr = nn_dist[i_neighbor]; 00238 if (dist_sqr <= 9*sigma_sqr) 00239 { 00240 float w = expf (-0.5f * dist_sqr / sigma_sqr); 00241 numerator += value * w; 00242 denominator += w; 00243 } 00244 else break; // i.e. if dist > 3 standard deviations, then terminate early 00245 } 00246 previous_filter_response = filter_response; 00247 filter_response = numerator / denominator; 00248 00249 // Compute the difference between adjacent scales 00250 if (i_scale > 0) 00251 diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response; 00252 } 00253 } 00254 } 00255 00256 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00257 template <typename PointInT, typename PointOutT> void 00258 pcl::SIFTKeypoint<PointInT, PointOutT>::findScaleSpaceExtrema ( 00259 const PointCloudIn &input, KdTree &tree, const Eigen::MatrixXf &diff_of_gauss, 00260 std::vector<int> &extrema_indices, std::vector<int> &extrema_scales) 00261 { 00262 const int k = 25; 00263 std::vector<int> nn_indices (k); 00264 std::vector<float> nn_dist (k); 00265 00266 const int nr_scales = static_cast<int> (diff_of_gauss.cols ()); 00267 std::vector<float> min_val (nr_scales), max_val (nr_scales); 00268 00269 for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point) 00270 { 00271 // Define the local neighborhood around the current point 00272 const size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist); //* 00273 // * note: the neighborhood for finding local extrema is best defined as a small fixed-k neighborhood, regardless of 00274 // the configurable search method specified by the user, so we directly employ tree.nearestKSearch here instead 00275 // of using searchForNeighbors 00276 00277 // At each scale, find the extreme values of the DoG within the current neighborhood 00278 for (int i_scale = 0; i_scale < nr_scales; ++i_scale) 00279 { 00280 min_val[i_scale] = std::numeric_limits<float>::max (); 00281 max_val[i_scale] = -std::numeric_limits<float>::max (); 00282 00283 for (size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor) 00284 { 00285 const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale); 00286 00287 min_val[i_scale] = (std::min) (min_val[i_scale], d); 00288 max_val[i_scale] = (std::max) (max_val[i_scale], d); 00289 } 00290 } 00291 00292 // If the current point is an extreme value with high enough contrast, add it as a keypoint 00293 for (int i_scale = 1; i_scale < nr_scales - 1; ++i_scale) 00294 { 00295 const float &val = diff_of_gauss (i_point, i_scale); 00296 00297 // Does the point have sufficient contrast? 00298 if (fabs (val) >= min_contrast_) 00299 { 00300 // Is it a local minimum? 00301 if ((val == min_val[i_scale]) && 00302 (val < min_val[i_scale - 1]) && 00303 (val < min_val[i_scale + 1])) 00304 { 00305 extrema_indices.push_back (i_point); 00306 extrema_scales.push_back (i_scale); 00307 } 00308 // Is it a local maximum? 00309 else if ((val == max_val[i_scale]) && 00310 (val > max_val[i_scale - 1]) && 00311 (val > max_val[i_scale + 1])) 00312 { 00313 extrema_indices.push_back (i_point); 00314 extrema_scales.push_back (i_scale); 00315 } 00316 } 00317 } 00318 } 00319 } 00320 00321 #define PCL_INSTANTIATE_SIFTKeypoint(T,U) template class PCL_EXPORTS pcl::SIFTKeypoint<T,U>; 00322 00323 #endif // #ifndef PCL_SIFT_KEYPOINT_IMPL_H_ 00324