Point Cloud Library (PCL)  1.9.1-dev
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 
43 //#include <pcl/io/pcd_io.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointT, typename PointNT> void
48  const PointCloudNTConstPtr &normals,
49  KdTreePtr &kdtree,
50  float &scale)
51 {
52  clouds_.push_back (cloud);
53  cloud_normals_.push_back (normals);
54  cloud_trees_.push_back (kdtree);
55  scales_.push_back (std::pair<float, size_t> (scale, scales_.size ()));
56 }
57 
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointT, typename PointNT> void
62 {
63  clouds_.clear ();
64  cloud_normals_.clear ();
65  scales_.clear ();
66 }
67 
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT, typename PointNT> void
72 {
73  // Calculate differences for each cloud
74  std::vector<std::vector<float> > diffs (scales_.size ());
75 
76  // The cloud with the smallest scale has no differences
77  std::vector<float> aux_diffs (input_->points.size (), 0.0f);
78  diffs[scales_[0].second] = aux_diffs;
79 
80  cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]);
81  for (size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i)
82  {
83  size_t cloud_i = scales_[scale_i].second,
84  cloud_i_minus_one = scales_[scale_i - 1].second;
85  diffs[cloud_i].resize (input_->points.size ());
86  PCL_INFO ("cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one);
87  for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
88  diffs[cloud_i][point_i] = cloud_normals_[cloud_i]->points[point_i].getNormalVector3fMap ().dot (
89  clouds_[cloud_i]->points[point_i].getVector3fMap () - clouds_[cloud_i_minus_one]->points[point_i].getVector3fMap ());
90 
91  // Setup kdtree for this cloud
92  cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]);
93  }
94 
95 
96  // Find minima and maxima in differences inside the input cloud
97  typename pcl::search::Search<PointT>::Ptr input_tree = cloud_trees_.back ();
98  for (int point_i = 0; point_i < static_cast<int> (input_->points.size ()); ++point_i)
99  {
100  std::vector<int> nn_indices;
101  std::vector<float> nn_distances;
102  input_tree->radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances);
103 
104  bool is_min = true, is_max = true;
105  for (const int &nn_index : nn_indices)
106  if (nn_index != point_i)
107  {
108  if (diffs[input_index_][point_i] < diffs[input_index_][nn_index])
109  is_max = false;
110  else if (diffs[input_index_][point_i] > diffs[input_index_][nn_index])
111  is_min = false;
112  }
113 
114  // If the point is a local minimum/maximum, check if it is the same over all the scales
115  if (is_min || is_max)
116  {
117  bool passed_min = true, passed_max = true;
118  for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
119  {
120  size_t cloud_i = scales_[scale_i].second;
121  // skip input cloud
122  if (cloud_i == clouds_.size () - 1)
123  continue;
124 
125  nn_indices.clear (); nn_distances.clear ();
126  cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances);
127 
128  bool is_min_other_scale = true, is_max_other_scale = true;
129  for (const int &nn_index : nn_indices)
130  if (nn_index != point_i)
131  {
132  if (diffs[input_index_][point_i] < diffs[cloud_i][nn_index])
133  is_max_other_scale = false;
134  else if (diffs[input_index_][point_i] > diffs[cloud_i][nn_index])
135  is_min_other_scale = false;
136  }
137 
138  if (is_min && !is_min_other_scale)
139  passed_min = false;
140  if (is_max && !is_max_other_scale)
141  passed_max = false;
142 
143  if (!passed_min && !passed_max)
144  break;
145  }
146 
147  // check if point was minimum/maximum over all the scales
148  if (passed_min || passed_max)
149  {
150  output.points.push_back (input_->points[point_i]);
151  keypoints_indices_->indices.push_back (point_i);
152  }
153  }
154  }
155 
156  output.header = input_->header;
157  output.width = static_cast<uint32_t> (output.points.size ());
158  output.height = 1;
159 
160  // debug stuff
161 // for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
162 // {
163 // PointCloud<PointXYZI>::Ptr debug_cloud (new PointCloud<PointXYZI> ());
164 // debug_cloud->points.resize (input_->points.size ());
165 // debug_cloud->width = input_->width;
166 // debug_cloud->height = input_->height;
167 // for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
168 // {
169 // debug_cloud->points[point_i].intensity = diffs[scales_[scale_i].second][point_i];
170 // debug_cloud->points[point_i].x = input_->points[point_i].x;
171 // debug_cloud->points[point_i].y = input_->points[point_i].y;
172 // debug_cloud->points[point_i].z = input_->points[point_i].z;
173 // }
174 
175 // char str[512]; sprintf (str, "diffs_%2d.pcd", scale_i);
176 // io::savePCDFile (str, *debug_cloud);
177 // }
178 }
179 
180 //////////////////////////////////////////////////////////////////////////////////////////////
181 template <typename PointT, typename PointNT> bool
183 {
184  PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n");
186  {
187  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
188  return false;
189  }
190 
191  if (!normals_)
192  {
193  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
194  return false;
195  }
196  if (clouds_.empty ())
197  {
198  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
199  return false;
200  }
201 
202  if (input_->points.size () != normals_->points.size ())
203  {
204  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
205  return false;
206  }
207 
208  for (size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
209  {
210  if (clouds_[cloud_i]->points.size () != input_->points.size ())
211  {
212  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i);
213  return false;
214  }
215 
216  if (cloud_normals_[cloud_i]->points.size () != input_->points.size ())
217  {
218  PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i);
219  return false;
220  }
221  }
222 
223  // Add the input cloud as the last index
224  scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ()));
225  clouds_.push_back (input_);
226  cloud_normals_.push_back (normals_);
227  cloud_trees_.push_back (tree_);
228  // Sort the clouds by their scales
229  sort (scales_.begin (), scales_.end (), compareScalesFunction);
230 
231  // Find the index of the input after sorting
232  for (size_t i = 0; i < scales_.size (); ++i)
233  if (scales_[i].second == scales_.size () - 1)
234  {
235  input_index_ = i;
236  break;
237  }
238 
239  PCL_INFO ("Scales: ");
240  for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
241  PCL_INFO ("\n");
242 
243  return (true);
244 }
245 
246 
247 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>;
248 
249 
250 #endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
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.
typename Keypoint< PointT, PointT >::KdTreePtr KdTreePtr
typename PointCloudNT::ConstPtr PointCloudNTConstPtr
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:428
void addSmoothedPointCloud(const PointCloudTConstPtr &cloud, const PointCloudNTConstPtr &normals, KdTreePtr &kdtree, float &scale)
Keypoint represents the base class for key points.
Definition: keypoint.h:48
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:426
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:420
void detectKeypoints(PointCloudT &output) override
Abstract key point detection method.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
typename PointCloudT::ConstPtr PointCloudTConstPtr