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
addSmoothedPointCloud(const PointCloudTConstPtr & cloud,const PointCloudNTConstPtr & normals,KdTreePtr & kdtree,float & scale)47 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::addSmoothedPointCloud (const PointCloudTConstPtr &cloud,
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, std::size_t> (scale, scales_.size ()));
56 }
57
58
59 //////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointT, typename PointNT> void
resetClouds()61 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::resetClouds ()
62 {
63 clouds_.clear ();
64 cloud_normals_.clear ();
65 scales_.clear ();
66 }
67
68
69 //////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT, typename PointNT> void
detectKeypoints(PointCloudT & output)71 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &output)
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_->size (), 0.0f);
78 diffs[scales_[0].second] = aux_diffs;
79
80 cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]);
81 for (std::size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i)
82 {
83 std::size_t cloud_i = scales_[scale_i].second,
84 cloud_i_minus_one = scales_[scale_i - 1].second;
85 diffs[cloud_i].resize (input_->size ());
86 PCL_INFO ("cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one);
87 for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
88 diffs[cloud_i][point_i] = (*cloud_normals_[cloud_i])[point_i].getNormalVector3fMap ().dot (
89 (*clouds_[cloud_i])[point_i].getVector3fMap () - (*clouds_[cloud_i_minus_one])[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 (pcl::index_t point_i = 0; point_i < static_cast<pcl::index_t> (input_->size ()); ++point_i)
99 {
100 pcl::Indices 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 auto &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 (std::size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
119 {
120 std::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 auto &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.push_back ((*input_)[point_i]);
151 keypoints_indices_->indices.push_back (point_i);
152 }
153 }
154 }
155
156 output.header = input_->header;
157 output.width = output.size ();
158 output.height = 1;
159
160 // debug stuff
161 // for (std::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_->size ());
165 // debug_cloud->width = input_->width;
166 // debug_cloud->height = input_->height;
167 // for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
168 // {
169 // (*debug_cloud)[point_i].intensity = diffs[scales_[scale_i].second][point_i];
170 // (*debug_cloud)[point_i].x = (*input_)[point_i].x;
171 // (*debug_cloud)[point_i].y = (*input_)[point_i].y;
172 // (*debug_cloud)[point_i].z = (*input_)[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
initCompute()182 pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::initCompute ()
183 {
184 PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n");
185 if ( !Keypoint<PointT, PointT>::initCompute ())
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_->size () != normals_->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 (std::size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
209 {
210 if (clouds_[cloud_i]->size () != input_->size ())
211 {
212 PCL_ERROR("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %zu does not have "
213 "the same number of points as the input cloud\n",
214 cloud_i);
215 return false;
216 }
217
218 if (cloud_normals_[cloud_i]->size () != input_->size ())
219 {
220 PCL_ERROR("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %zu "
221 "do not have the same number of points as the input cloud\n",
222 cloud_i);
223 return false;
224 }
225 }
226
227 // Add the input cloud as the last index
228 scales_.push_back (std::pair<float, std::size_t> (input_scale_, scales_.size ()));
229 clouds_.push_back (input_);
230 cloud_normals_.push_back (normals_);
231 cloud_trees_.push_back (tree_);
232 // Sort the clouds by their scales
233 sort (scales_.begin (), scales_.end (), compareScalesFunction);
234
235 // Find the index of the input after sorting
236 for (std::size_t i = 0; i < scales_.size (); ++i)
237 if (scales_[i].second == scales_.size () - 1)
238 {
239 input_index_ = i;
240 break;
241 }
242
243 PCL_INFO ("Scales: ");
244 for (std::size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
245 PCL_INFO ("\n");
246
247 return (true);
248 }
249
250
251 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>;
252
253
254 #endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */
255