Point Cloud Library (PCL)  1.7.1
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  output.points.push_back (input_->points[point_i]);
151  }
152  }
153 
154  output.header = input_->header;
155  output.width = static_cast<uint32_t> (output.points.size ());
156  output.height = 1;
157 
158  // debug stuff
159 // for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
160 // {
161 // PointCloud<PointXYZI>::Ptr debug_cloud (new PointCloud<PointXYZI> ());
162 // debug_cloud->points.resize (input_->points.size ());
163 // debug_cloud->width = input_->width;
164 // debug_cloud->height = input_->height;
165 // for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
166 // {
167 // debug_cloud->points[point_i].intensity = diffs[scales_[scale_i].second][point_i];
168 // debug_cloud->points[point_i].x = input_->points[point_i].x;
169 // debug_cloud->points[point_i].y = input_->points[point_i].y;
170 // debug_cloud->points[point_i].z = input_->points[point_i].z;
171 // }
172 
173 // char str[512]; sprintf (str, "diffs_%2d.pcd", scale_i);
174 // io::savePCDFile (str, *debug_cloud);
175 // }
176 }
177 
178 //////////////////////////////////////////////////////////////////////////////////////////////
179 template <typename PointT, typename PointNT> bool
181 {
182  PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n");
184  {
185  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
186  return false;
187  }
188 
189  if (!normals_)
190  {
191  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
192  return false;
193  }
194  if (clouds_.size () == 0)
195  {
196  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
197  return false;
198  }
199 
200  if (input_->points.size () != normals_->points.size ())
201  {
202  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
203  return false;
204  }
205 
206  for (size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
207  {
208  if (clouds_[cloud_i]->points.size () != input_->points.size ())
209  {
210  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i);
211  return false;
212  }
213 
214  if (cloud_normals_[cloud_i]->points.size () != input_->points.size ())
215  {
216  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i);
217  return false;
218  }
219  }
220 
221  // Add the input cloud as the last index
222  scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ()));
223  clouds_.push_back (input_);
224  cloud_normals_.push_back (normals_);
225  cloud_trees_.push_back (tree_);
226  // Sort the clouds by their scales
227  sort (scales_.begin (), scales_.end (), compareScalesFunction);
228 
229  // Find the index of the input after sorting
230  for (size_t i = 0; i < scales_.size (); ++i)
231  if (scales_[i].second == scales_.size () - 1)
232  {
233  input_index_ = i;
234  break;
235  }
236 
237  PCL_INFO ("Scales: ");
238  for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
239  PCL_INFO ("\n");
240 
241  return true;
242 }
243 
244 
245 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>;
246 
247 
248 #endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */