Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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>
45#include <pcl/console/print.h> // for PCL_ERROR, PCL_DEBUG
46
47//////////////////////////////////////////////////////////////////////////////////////////////
48template <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//////////////////////////////////////////////////////////////////////////////////////////////
84template <typename PointT, typename PointNT> float
87{
88// PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n");
89
91 output_positions->points.resize (interm_cloud_->size ());
93 output_normals->points.resize (interm_cloud_->size ());
94
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_);
117
118 smoothed_normal += theta_i * (*interm_normals_)[nn_indices[nn_index_i]].getNormalVector4fMap ();
119
121 }
122
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 }
143 if (e_residual < 1e-5) break;
144
146 }
147
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
160template <typename PointT, typename PointNT> void
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;
172 float last_error_residual = error_residual + 100.0f;
173
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_ &&
182 {
183 average_normal = Eigen::Vector4f::Zero ();
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 {
193 float theta_i = std::exp ( (-1) * dist / scale_squared_);
195
196 average_normal += theta_i * (*normals_)[nn_indices[nn_index_i]].getNormalVector4fMap ();
198 }
200 average_normal(3) = 0.0f;
201 average_normal.normalize ();
202
203 // find minimum along the normal
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) &&
209 {
211
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);
219 }
221 if (e_residual_along_normal < 1e-3) break;
222
224 }
225
226// if (small_iterations == max_small_iterations)
227// PCL_INFO ("passed the number of small iterations %d\n", small_iterations);
228
231
232// PCL_INFO ("last %f current %f\n", last_error_residual, error_residual);
233 }
234
238 output_normal = (*normals_)[point_index];
239
241 PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations);
242}
243
244
245//////////////////////////////////////////////////////////////////////////////////////////////
246template <typename PointT, typename PointNT> void
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//////////////////////////////////////////////////////////////////////////////////////////////
275template <typename PointT, typename PointNT> void
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
293 std::vector<float> nn_distances;
294
295 output_features->resize (cloud2->size ());
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_ */
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
PCL base class.
Definition pcl_base.h:70
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
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
A point structure representing Euclidean xyz coordinates, and the RGB color.