Point Cloud Library (PCL)  1.8.0
smoothed_surfaces_keypoint.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Alexandru-Eugen Ichim
5  * Willow Garage, Inc
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * $Id$
36  */
37 
38 #ifndef PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
39 #define PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
40 
41 #include <pcl/keypoints/smoothed_surfaces_keypoint.h>
42 #include <pcl/kdtree/kdtree_flann.h>
43 
44 //#include <pcl/io/pcd_io.h>
45 
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT, typename PointNT> void
49  const PointCloudNTConstPtr &normals,
50  KdTreePtr &kdtree,
51  float &scale)
52 {
53  clouds_.push_back (cloud);
54  cloud_normals_.push_back (normals);
55  cloud_trees_.push_back (kdtree);
56  scales_.push_back (std::pair<float, size_t> (scale, scales_.size ()));
57 }
58 
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointT, typename PointNT> void
63 {
64  clouds_.clear ();
65  cloud_normals_.clear ();
66  scales_.clear ();
67 }
68 
69 
70 //////////////////////////////////////////////////////////////////////////////////////////////
71 template <typename PointT, typename PointNT> void
73 {
74  // Calculate differences for each cloud
75  std::vector<std::vector<float> > diffs (scales_.size ());
76 
77  // The cloud with the smallest scale has no differences
78  std::vector<float> aux_diffs (input_->points.size (), 0.0f);
79  diffs[scales_[0].second] = aux_diffs;
80 
81  cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]);
82  for (size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i)
83  {
84  size_t cloud_i = scales_[scale_i].second,
85  cloud_i_minus_one = scales_[scale_i - 1].second;
86  diffs[cloud_i].resize (input_->points.size ());
87  PCL_INFO ("cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one);
88  for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
89  diffs[cloud_i][point_i] = cloud_normals_[cloud_i]->points[point_i].getNormalVector3fMap ().dot (
90  clouds_[cloud_i]->points[point_i].getVector3fMap () - clouds_[cloud_i_minus_one]->points[point_i].getVector3fMap ());
91 
92  // Setup kdtree for this cloud
93  cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]);
94  }
95 
96 
97  // Find minima and maxima in differences inside the input cloud
98  typename pcl::search::Search<PointT>::Ptr input_tree = cloud_trees_.back ();
99  for (int point_i = 0; point_i < static_cast<int> (input_->points.size ()); ++point_i)
100  {
101  std::vector<int> nn_indices;
102  std::vector<float> nn_distances;
103  input_tree->radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances);
104 
105  bool is_min = true, is_max = true;
106  for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
107  if (*nn_it != point_i)
108  {
109  if (diffs[input_index_][point_i] < diffs[input_index_][*nn_it])
110  is_max = false;
111  else if (diffs[input_index_][point_i] > diffs[input_index_][*nn_it])
112  is_min = false;
113  }
114 
115  // If the point is a local minimum/maximum, check if it is the same over all the scales
116  if (is_min || is_max)
117  {
118  bool passed_min = true, passed_max = true;
119  for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
120  {
121  size_t cloud_i = scales_[scale_i].second;
122  // skip input cloud
123  if (cloud_i == clouds_.size () - 1)
124  continue;
125 
126  nn_indices.clear (); nn_distances.clear ();
127  cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances);
128 
129  bool is_min_other_scale = true, is_max_other_scale = true;
130  for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
131  if (*nn_it != point_i)
132  {
133  if (diffs[input_index_][point_i] < diffs[cloud_i][*nn_it])
134  is_max_other_scale = false;
135  else if (diffs[input_index_][point_i] > diffs[cloud_i][*nn_it])
136  is_min_other_scale = false;
137  }
138 
139  if (is_min == true && is_min_other_scale == false)
140  passed_min = false;
141  if (is_max == true && is_max_other_scale == false)
142  passed_max = false;
143 
144  if (!passed_min && !passed_max)
145  break;
146  }
147 
148  // check if point was minimum/maximum over all the scales
149  if (passed_min || passed_max)
150  {
151  output.points.push_back (input_->points[point_i]);
152  keypoints_indices_->indices.push_back (point_i);
153  }
154  }
155  }
156 
157  output.header = input_->header;
158  output.width = static_cast<uint32_t> (output.points.size ());
159  output.height = 1;
160 
161  // debug stuff
162 // for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
163 // {
164 // PointCloud<PointXYZI>::Ptr debug_cloud (new PointCloud<PointXYZI> ());
165 // debug_cloud->points.resize (input_->points.size ());
166 // debug_cloud->width = input_->width;
167 // debug_cloud->height = input_->height;
168 // for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
169 // {
170 // debug_cloud->points[point_i].intensity = diffs[scales_[scale_i].second][point_i];
171 // debug_cloud->points[point_i].x = input_->points[point_i].x;
172 // debug_cloud->points[point_i].y = input_->points[point_i].y;
173 // debug_cloud->points[point_i].z = input_->points[point_i].z;
174 // }
175 
176 // char str[512]; sprintf (str, "diffs_%2d.pcd", scale_i);
177 // io::savePCDFile (str, *debug_cloud);
178 // }
179 }
180 
181 //////////////////////////////////////////////////////////////////////////////////////////////
182 template <typename PointT, typename PointNT> bool
184 {
185  PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n");
187  {
188  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
189  return false;
190  }
191 
192  if (!normals_)
193  {
194  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
195  return false;
196  }
197  if (clouds_.size () == 0)
198  {
199  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
200  return false;
201  }
202 
203  if (input_->points.size () != normals_->points.size ())
204  {
205  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
206  return false;
207  }
208 
209  for (size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
210  {
211  if (clouds_[cloud_i]->points.size () != input_->points.size ())
212  {
213  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i);
214  return false;
215  }
216 
217  if (cloud_normals_[cloud_i]->points.size () != input_->points.size ())
218  {
219  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i);
220  return false;
221  }
222  }
223 
224  // Add the input cloud as the last index
225  scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ()));
226  clouds_.push_back (input_);
227  cloud_normals_.push_back (normals_);
228  cloud_trees_.push_back (tree_);
229  // Sort the clouds by their scales
230  sort (scales_.begin (), scales_.end (), compareScalesFunction);
231 
232  // Find the index of the input after sorting
233  for (size_t i = 0; i < scales_.size (); ++i)
234  if (scales_[i].second == scales_.size () - 1)
235  {
236  input_index_ = i;
237  break;
238  }
239 
240  PCL_INFO ("Scales: ");
241  for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
242  PCL_INFO ("\n");
243 
244  return (true);
245 }
246 
247 
248 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>;
249 
250 
251 #endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
Keypoint< PointT, PointT >::KdTreePtr KdTreePtr
void detectKeypoints(PointCloudT &output)
Abstract key point detection method.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
PointCloudNT::ConstPtr PointCloudNTConstPtr
void addSmoothedPointCloud(const PointCloudTConstPtr &cloud, const PointCloudNTConstPtr &normals, KdTreePtr &kdtree, float &scale)
Keypoint represents the base class for key points.
Definition: keypoint.h:49
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407