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