Point Cloud Library (PCL)  1.7.1
surfel_smoothing.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_SURFACE_IMPL_SURFEL_SMOOTHING_H_
39 #define PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_
40 
41 #include <pcl/surface/surfel_smoothing.h>
42 #include <pcl/common/distances.h>
43 
44 //////////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointT, typename PointNT> bool
47 {
49  return false;
50 
51  if (!normals_)
52  {
53  PCL_ERROR ("SurfelSmoothing: normal cloud not set\n");
54  return false;
55  }
56 
57  if (input_->points.size () != normals_->points.size ())
58  {
59  PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n");
60  return false;
61  }
62 
63  // Initialize the spatial locator
64  if (!tree_)
65  {
66  if (input_->isOrganized ())
67  tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
68  else
69  tree_.reset (new pcl::search::KdTree<PointT> (false));
70  }
71 
72  // create internal copies of the input - these will be modified
73  interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_));
74  interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_));
75 
76  return (true);
77 }
78 
79 
80 //////////////////////////////////////////////////////////////////////////////////////////////
81 template <typename PointT, typename PointNT> float
83  NormalCloudPtr &output_normals)
84 {
85 // PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n");
86 
87  output_positions = PointCloudInPtr (new PointCloudIn);
88  output_positions->points.resize (interm_cloud_->points.size ());
89  output_normals = NormalCloudPtr (new NormalCloud);
90  output_normals->points.resize (interm_cloud_->points.size ());
91 
92  std::vector<int> nn_indices;
93  std::vector<float> nn_distances;
94 
95  std::vector<float> diffs (interm_cloud_->points.size ());
96  float total_residual = 0.0f;
97 
98  for (size_t i = 0; i < interm_cloud_->points.size (); ++i)
99  {
100  Eigen::Vector4f smoothed_point = Eigen::Vector4f::Zero ();
101  Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero ();
102 
103  // get neighbors
104  // @todo using 5x the scale for searching instead of all the points to avoid O(N^2)
105  tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances);
106 
107  float theta_normalization_factor = 0.0;
108  std::vector<float> theta (nn_indices.size ());
109  for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
110  {
111  float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], input_->points[nn_indices[nn_index_i]]);//interm_cloud_->points[nn_indices[nn_index_i]]);
112  float theta_i = expf ( (-1) * dist / scale_squared_);
113  theta_normalization_factor += theta_i;
114 
115  smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
116 
117  theta[nn_index_i] = theta_i;
118  }
119 
120  smoothed_normal /= theta_normalization_factor;
121  smoothed_normal(3) = 0.0f;
122  smoothed_normal.normalize ();
123 
124 
125  // find minimum along the normal
126  float e_residual;
127  smoothed_point = interm_cloud_->points[i].getVector4fMap ();
128  while (1)
129  {
130  e_residual = 0.0f;
131  smoothed_point(3) = 0.0f;
132  for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
133  {
134  Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();//interm_cloud_->points[nn_indices[nn_index_i]].getVector4fMap ();
135  neighbor(3) = 0.0f;
136  float dot_product = smoothed_normal.dot (neighbor - smoothed_point);
137  e_residual += theta[nn_index_i] * dot_product;// * dot_product;
138  }
139  e_residual /= theta_normalization_factor;
140  if (e_residual < 1e-5) break;
141 
142  smoothed_point = smoothed_point + e_residual * smoothed_normal;
143  }
144 
145  total_residual += e_residual;
146 
147  output_positions->points[i].getVector4fMap () = smoothed_point;
148  output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();//smoothed_normal;
149  }
150 
151 // std::cerr << "Total residual after iteration: " << total_residual << std::endl;
152 // PCL_INFO("SurfelSmoothing done iteration\n");
153  return total_residual;
154 }
155 
156 
157 template <typename PointT, typename PointNT> void
159  PointT &output_point,
160  PointNT &output_normal)
161 {
162  Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
163  Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap ();
164  result_point(3) = 0.0f;
165 
166  // @todo parameter
167  float error_residual_threshold_ = 1e-3f;
168  float error_residual = error_residual_threshold_ + 1;
169  float last_error_residual = error_residual + 100.0f;
170 
171  std::vector<int> nn_indices;
172  std::vector<float> nn_distances;
173 
174  int big_iterations = 0;
175  int max_big_iterations = 500;
176 
177  while (fabs (error_residual) < fabs (last_error_residual) -error_residual_threshold_ &&
178  big_iterations < max_big_iterations)
179  {
180  average_normal = Eigen::Vector4f::Zero ();
181  big_iterations ++;
182  PointT aux_point; aux_point.x = result_point(0); aux_point.y = result_point(1); aux_point.z = result_point(2);
183  tree_->radiusSearch (aux_point, 5*scale_, nn_indices, nn_distances);
184 
185  float theta_normalization_factor = 0.0;
186  std::vector<float> theta (nn_indices.size ());
187  for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
188  {
189  float dist = nn_distances[nn_index_i];
190  float theta_i = expf ( (-1) * dist / scale_squared_);
191  theta_normalization_factor += theta_i;
192 
193  average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
194  theta[nn_index_i] = theta_i;
195  }
196  average_normal /= theta_normalization_factor;
197  average_normal(3) = 0.0f;
198  average_normal.normalize ();
199 
200  // find minimum along the normal
201  float e_residual_along_normal = 2, last_e_residual_along_normal = 3;
202  int small_iterations = 0;
203  int max_small_iterations = 10;
204  while ( fabs (e_residual_along_normal) < fabs (last_e_residual_along_normal) &&
205  small_iterations < max_small_iterations)
206  {
207  small_iterations ++;
208 
209  e_residual_along_normal = 0.0f;
210  for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
211  {
212  Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();
213  neighbor(3) = 0.0f;
214  float dot_product = average_normal.dot (neighbor - result_point);
215  e_residual_along_normal += theta[nn_index_i] * dot_product;
216  }
217  e_residual_along_normal /= theta_normalization_factor;
218  if (e_residual_along_normal < 1e-3) break;
219 
220  result_point = result_point + e_residual_along_normal * average_normal;
221  }
222 
223 // if (small_iterations == max_small_iterations)
224 // PCL_INFO ("passed the number of small iterations %d\n", small_iterations);
225 
226  last_error_residual = error_residual;
227  error_residual = e_residual_along_normal;
228 
229 // PCL_INFO ("last %f current %f\n", last_error_residual, error_residual);
230  }
231 
232  output_point.x = result_point(0);
233  output_point.y = result_point(1);
234  output_point.z = result_point(2);
235  output_normal = normals_->points[point_index];
236 
237  if (big_iterations == max_big_iterations)
238  PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations);
239 }
240 
241 
242 //////////////////////////////////////////////////////////////////////////////////////////////
243 template <typename PointT, typename PointNT> void
245  NormalCloudPtr &output_normals)
246 {
247  if (!initCompute ())
248  {
249  PCL_ERROR ("[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n");
250  return;
251  }
252 
253  tree_->setInputCloud (input_);
254 
255  output_positions->header = input_->header;
256  output_positions->height = input_->height;
257  output_positions->width = input_->width;
258 
259  output_normals->header = input_->header;
260  output_normals->height = input_->height;
261  output_normals->width = input_->width;
262 
263  output_positions->points.resize (input_->points.size ());
264  output_normals->points.resize (input_->points.size ());
265  for (size_t i = 0; i < input_->points.size (); ++i)
266  {
267  smoothPoint (i, output_positions->points[i], output_normals->points[i]);
268  }
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointT, typename PointNT> void
274  NormalCloudPtr &cloud2_normals,
275  boost::shared_ptr<std::vector<int> > &output_features)
276 {
277  if (interm_cloud_->points.size () != cloud2->points.size () ||
278  cloud2->points.size () != cloud2_normals->points.size ())
279  {
280  PCL_ERROR ("[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n");
281  return;
282  }
283 
284  std::vector<float> diffs (cloud2->points.size ());
285  for (size_t i = 0; i < cloud2->points.size (); ++i)
286  diffs[i] = cloud2_normals->points[i].getNormalVector4fMap ().dot (cloud2->points[i].getVector4fMap () -
287  interm_cloud_->points[i].getVector4fMap ());
288 
289  std::vector<int> nn_indices;
290  std::vector<float> nn_distances;
291 
292  output_features->resize (cloud2->points.size ());
293  for (int point_i = 0; point_i < static_cast<int> (cloud2->points.size ()); ++point_i)
294  {
295  // Get neighbors
296  tree_->radiusSearch (point_i, scale_, nn_indices, nn_distances);
297 
298  bool largest = true;
299  bool smallest = true;
300  for (std::vector<int>::iterator nn_index_it = nn_indices.begin (); nn_index_it != nn_indices.end (); ++nn_index_it)
301  {
302  if (diffs[point_i] < diffs[*nn_index_it])
303  largest = false;
304  else
305  smallest = false;
306  }
307 
308  if (largest == true || smallest == true)
309  (*output_features)[point_i] = point_i;
310  }
311 }
312 
313 
314 
315 #define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing<PointT, PointNT>;
316 
317 #endif /* PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ */