Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Alexandru-Eugen Ichim 00005 * Willow Garage, Inc 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * $Id$ 00036 */ 00037 00038 #ifndef PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ 00039 #define PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ 00040 00041 #include <pcl/keypoints/smoothed_surfaces_keypoint.h> 00042 #include <pcl/kdtree/kdtree_flann.h> 00043 00044 //#include <pcl/io/pcd_io.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT, typename PointNT> void 00048 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::addSmoothedPointCloud (const PointCloudTConstPtr &cloud, 00049 const PointCloudNTConstPtr &normals, 00050 KdTreePtr &kdtree, 00051 float &scale) 00052 { 00053 clouds_.push_back (cloud); 00054 cloud_normals_.push_back (normals); 00055 cloud_trees_.push_back (kdtree); 00056 scales_.push_back (std::pair<float, size_t> (scale, scales_.size ())); 00057 } 00058 00059 00060 ////////////////////////////////////////////////////////////////////////////////////////////// 00061 template <typename PointT, typename PointNT> void 00062 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::resetClouds () 00063 { 00064 clouds_.clear (); 00065 cloud_normals_.clear (); 00066 scales_.clear (); 00067 } 00068 00069 00070 ////////////////////////////////////////////////////////////////////////////////////////////// 00071 template <typename PointT, typename PointNT> void 00072 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &output) 00073 { 00074 // Calculate differences for each cloud 00075 std::vector<std::vector<float> > diffs (scales_.size ()); 00076 00077 // The cloud with the smallest scale has no differences 00078 std::vector<float> aux_diffs (input_->points.size (), 0.0f); 00079 diffs[scales_[0].second] = aux_diffs; 00080 00081 cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]); 00082 for (size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i) 00083 { 00084 size_t cloud_i = scales_[scale_i].second, 00085 cloud_i_minus_one = scales_[scale_i - 1].second; 00086 diffs[cloud_i].resize (input_->points.size ()); 00087 PCL_INFO ("cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one); 00088 for (size_t point_i = 0; point_i < input_->points.size (); ++point_i) 00089 diffs[cloud_i][point_i] = cloud_normals_[cloud_i]->points[point_i].getNormalVector3fMap ().dot ( 00090 clouds_[cloud_i]->points[point_i].getVector3fMap () - clouds_[cloud_i_minus_one]->points[point_i].getVector3fMap ()); 00091 00092 // Setup kdtree for this cloud 00093 cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]); 00094 } 00095 00096 00097 // Find minima and maxima in differences inside the input cloud 00098 typename pcl::search::Search<PointT>::Ptr input_tree = cloud_trees_.back (); 00099 for (int point_i = 0; point_i < static_cast<int> (input_->points.size ()); ++point_i) 00100 { 00101 std::vector<int> nn_indices; 00102 std::vector<float> nn_distances; 00103 input_tree->radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances); 00104 00105 bool is_min = true, is_max = true; 00106 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it) 00107 if (*nn_it != point_i) 00108 { 00109 if (diffs[input_index_][point_i] < diffs[input_index_][*nn_it]) 00110 is_max = false; 00111 else if (diffs[input_index_][point_i] > diffs[input_index_][*nn_it]) 00112 is_min = false; 00113 } 00114 00115 // If the point is a local minimum/maximum, check if it is the same over all the scales 00116 if (is_min || is_max) 00117 { 00118 bool passed_min = true, passed_max = true; 00119 for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i) 00120 { 00121 size_t cloud_i = scales_[scale_i].second; 00122 // skip input cloud 00123 if (cloud_i == clouds_.size () - 1) 00124 continue; 00125 00126 nn_indices.clear (); nn_distances.clear (); 00127 cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances); 00128 00129 bool is_min_other_scale = true, is_max_other_scale = true; 00130 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it) 00131 if (*nn_it != point_i) 00132 { 00133 if (diffs[input_index_][point_i] < diffs[cloud_i][*nn_it]) 00134 is_max_other_scale = false; 00135 else if (diffs[input_index_][point_i] > diffs[cloud_i][*nn_it]) 00136 is_min_other_scale = false; 00137 } 00138 00139 if (is_min == true && is_min_other_scale == false) 00140 passed_min = false; 00141 if (is_max == true && is_max_other_scale == false) 00142 passed_max = false; 00143 00144 if (!passed_min && !passed_max) 00145 break; 00146 } 00147 00148 // check if point was minimum/maximum over all the scales 00149 if (passed_min || passed_max) 00150 output.points.push_back (input_->points[point_i]); 00151 } 00152 } 00153 00154 output.header = input_->header; 00155 output.width = static_cast<uint32_t> (output.points.size ()); 00156 output.height = 1; 00157 00158 // debug stuff 00159 // for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i) 00160 // { 00161 // PointCloud<PointXYZI>::Ptr debug_cloud (new PointCloud<PointXYZI> ()); 00162 // debug_cloud->points.resize (input_->points.size ()); 00163 // debug_cloud->width = input_->width; 00164 // debug_cloud->height = input_->height; 00165 // for (size_t point_i = 0; point_i < input_->points.size (); ++point_i) 00166 // { 00167 // debug_cloud->points[point_i].intensity = diffs[scales_[scale_i].second][point_i]; 00168 // debug_cloud->points[point_i].x = input_->points[point_i].x; 00169 // debug_cloud->points[point_i].y = input_->points[point_i].y; 00170 // debug_cloud->points[point_i].z = input_->points[point_i].z; 00171 // } 00172 00173 // char str[512]; sprintf (str, "diffs_%2d.pcd", scale_i); 00174 // io::savePCDFile (str, *debug_cloud); 00175 // } 00176 } 00177 00178 ////////////////////////////////////////////////////////////////////////////////////////////// 00179 template <typename PointT, typename PointNT> bool 00180 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::initCompute () 00181 { 00182 PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n"); 00183 if ( !Keypoint<PointT, PointT>::initCompute ()) 00184 { 00185 PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n"); 00186 return false; 00187 } 00188 00189 if (!normals_) 00190 { 00191 PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n"); 00192 return false; 00193 } 00194 if (clouds_.size () == 0) 00195 { 00196 PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n"); 00197 return false; 00198 } 00199 00200 if (input_->points.size () != normals_->points.size ()) 00201 { 00202 PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n"); 00203 return false; 00204 } 00205 00206 for (size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i) 00207 { 00208 if (clouds_[cloud_i]->points.size () != input_->points.size ()) 00209 { 00210 PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i); 00211 return false; 00212 } 00213 00214 if (cloud_normals_[cloud_i]->points.size () != input_->points.size ()) 00215 { 00216 PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i); 00217 return false; 00218 } 00219 } 00220 00221 // Add the input cloud as the last index 00222 scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ())); 00223 clouds_.push_back (input_); 00224 cloud_normals_.push_back (normals_); 00225 cloud_trees_.push_back (tree_); 00226 // Sort the clouds by their scales 00227 sort (scales_.begin (), scales_.end (), compareScalesFunction); 00228 00229 // Find the index of the input after sorting 00230 for (size_t i = 0; i < scales_.size (); ++i) 00231 if (scales_[i].second == scales_.size () - 1) 00232 { 00233 input_index_ = i; 00234 break; 00235 } 00236 00237 PCL_INFO ("Scales: "); 00238 for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first); 00239 PCL_INFO ("\n"); 00240 00241 return true; 00242 } 00243 00244 00245 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>; 00246 00247 00248 #endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */