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