38 #ifndef PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ 39 #define PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ 41 #include <pcl/surface/surfel_smoothing.h> 45 template <
typename Po
intT,
typename Po
intNT>
bool 53 PCL_ERROR (
"SurfelSmoothing: normal cloud not set\n");
57 if (input_->points.size () != normals_->points.size ())
59 PCL_ERROR (
"SurfelSmoothing: number of input points different from the number of given normals\n");
66 if (input_->isOrganized ())
81 template <
typename Po
intT,
typename Po
intNT>
float 88 output_positions->points.resize (interm_cloud_->points.size ());
90 output_normals->points.resize (interm_cloud_->points.size ());
92 std::vector<int> nn_indices;
93 std::vector<float> nn_distances;
95 std::vector<float> diffs (interm_cloud_->points.size ());
96 float total_residual = 0.0f;
98 for (
size_t i = 0; i < interm_cloud_->points.size (); ++i)
100 Eigen::Vector4f smoothed_point = Eigen::Vector4f::Zero ();
101 Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero ();
105 tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances);
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)
112 float theta_i = expf ( (-1) * dist / scale_squared_);
113 theta_normalization_factor += theta_i;
115 smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
117 theta[nn_index_i] = theta_i;
120 smoothed_normal /= theta_normalization_factor;
121 smoothed_normal(3) = 0.0f;
122 smoothed_normal.normalize ();
127 smoothed_point = interm_cloud_->points[i].getVector4fMap ();
131 smoothed_point(3) = 0.0f;
132 for (
size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
134 Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();
136 float dot_product = smoothed_normal.dot (neighbor - smoothed_point);
137 e_residual += theta[nn_index_i] * dot_product;
139 e_residual /= theta_normalization_factor;
140 if (e_residual < 1e-5)
break;
142 smoothed_point = smoothed_point + e_residual * smoothed_normal;
145 total_residual += e_residual;
147 output_positions->points[i].getVector4fMap () = smoothed_point;
148 output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();
153 return total_residual;
157 template <
typename Po
intT,
typename Po
intNT>
void 160 PointNT &output_normal)
162 Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
163 Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap ();
164 result_point(3) = 0.0f;
167 float error_residual_threshold_ = 1e-3f;
168 float error_residual = error_residual_threshold_ + 1;
169 float last_error_residual = error_residual + 100.0f;
171 std::vector<int> nn_indices;
172 std::vector<float> nn_distances;
174 int big_iterations = 0;
175 int max_big_iterations = 500;
177 while (fabs (error_residual) < fabs (last_error_residual) -error_residual_threshold_ &&
178 big_iterations < max_big_iterations)
180 average_normal = Eigen::Vector4f::Zero ();
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);
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)
189 float dist = nn_distances[nn_index_i];
190 float theta_i = expf ( (-1) * dist / scale_squared_);
191 theta_normalization_factor += theta_i;
193 average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
194 theta[nn_index_i] = theta_i;
196 average_normal /= theta_normalization_factor;
197 average_normal(3) = 0.0f;
198 average_normal.normalize ();
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)
209 e_residual_along_normal = 0.0f;
210 for (
size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
212 Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();
214 float dot_product = average_normal.dot (neighbor - result_point);
215 e_residual_along_normal += theta[nn_index_i] * dot_product;
217 e_residual_along_normal /= theta_normalization_factor;
218 if (e_residual_along_normal < 1e-3)
break;
220 result_point = result_point + e_residual_along_normal * average_normal;
226 last_error_residual = error_residual;
227 error_residual = e_residual_along_normal;
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];
237 if (big_iterations == max_big_iterations)
238 PCL_DEBUG (
"[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations);
243 template <
typename Po
intT,
typename Po
intNT>
void 249 PCL_ERROR (
"[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n");
253 tree_->setInputCloud (input_);
255 output_positions->header = input_->header;
256 output_positions->height = input_->height;
257 output_positions->width = input_->width;
259 output_normals->header = input_->header;
260 output_normals->height = input_->height;
261 output_normals->width = input_->width;
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)
267 smoothPoint (i, output_positions->points[i], output_normals->points[i]);
272 template <
typename Po
intT,
typename Po
intNT>
void 275 boost::shared_ptr<std::vector<int> > &output_features)
277 if (interm_cloud_->points.size () != cloud2->points.size () ||
278 cloud2->points.size () != cloud2_normals->points.size ())
280 PCL_ERROR (
"[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n");
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 ());
289 std::vector<int> nn_indices;
290 std::vector<float> nn_distances;
292 output_features->resize (cloud2->points.size ());
293 for (
int point_i = 0; point_i < static_cast<int> (cloud2->points.size ()); ++point_i)
296 tree_->radiusSearch (point_i, scale_, nn_indices, nn_distances);
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)
302 if (diffs[point_i] < diffs[*nn_index_it])
308 if (largest ==
true || smallest ==
true)
309 (*output_features)[point_i] = point_i;
315 #define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing<PointT, PointNT>;
void smoothPoint(size_t &point_index, PointT &output_point, PointNT &output_normal)
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
void computeSmoothedCloud(PointCloudInPtr &output_positions, NormalCloudPtr &output_normals)
Define standard C methods to do distance calculations.
float smoothCloudIteration(PointCloudInPtr &output_positions, NormalCloudPtr &output_normals)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PointCloud< PointT >::Ptr PointCloudInPtr
void extractSalientFeaturesBetweenScales(PointCloudInPtr &cloud2, NormalCloudPtr &cloud2_normals, boost::shared_ptr< std::vector< int > > &output_features)
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
A point structure representing Euclidean xyz coordinates, and the RGB color.
pcl::PointCloud< PointNT >::Ptr NormalCloudPtr