surfel_smoothing.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Alexandru-Eugen Ichim
00005  *                      Willow Garage, Inc
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * $Id$
00036  */
00037 
00038 #ifndef PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_
00039 #define PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_
00040 
00041 #include <pcl/surface/surfel_smoothing.h>
00042 #include <pcl/common/distances.h>
00043 
00045 template <typename PointT, typename PointNT> bool
00046 pcl::SurfelSmoothing<PointT, PointNT>::initCompute ()
00047 {
00048   if (!PCLBase<PointT>::initCompute ())
00049     return false;
00050 
00051   if (!normals_)
00052   {
00053     PCL_ERROR ("SurfelSmoothing: normal cloud not set\n");
00054     return false;
00055   }
00056 
00057   if (input_->points.size () != normals_->points.size ())
00058   {
00059     PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n");
00060     return false;
00061   }
00062 
00063   // Initialize the spatial locator
00064   if (!tree_)
00065   {
00066     if (input_->isOrganized ())
00067       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00068     else
00069       tree_.reset (new pcl::search::KdTree<PointT> (false));
00070   }
00071 
00072   // create internal copies of the input - these will be modified
00073   interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_));
00074   interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_));
00075 
00076   return (true);
00077 }
00078 
00079 
00081 template <typename PointT, typename PointNT> float
00082 pcl::SurfelSmoothing<PointT, PointNT>::smoothCloudIteration (PointCloudInPtr &output_positions,
00083                                                              NormalCloudPtr &output_normals)
00084 {
00085 //  PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n");
00086 
00087   output_positions = PointCloudInPtr (new PointCloudIn);
00088   output_positions->points.resize (interm_cloud_->points.size ());
00089   output_normals = NormalCloudPtr (new NormalCloud);
00090   output_normals->points.resize (interm_cloud_->points.size ());
00091 
00092   std::vector<int> nn_indices;
00093   std::vector<float> nn_distances;
00094 
00095   std::vector<float> diffs (interm_cloud_->points.size ());
00096   float total_residual = 0.0f;
00097 
00098   for (size_t i = 0; i < interm_cloud_->points.size (); ++i)
00099   {
00100     Eigen::Vector4f smoothed_point  = Eigen::Vector4f::Zero ();
00101     Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero (); 
00102 
00103     // get neighbors
00104     // @todo using 5x the scale for searching instead of all the points to avoid O(N^2)
00105     tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances);
00106 
00107     float theta_normalization_factor = 0.0;
00108     std::vector<float> theta (nn_indices.size ());
00109     for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00110     {
00111       float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], input_->points[nn_indices[nn_index_i]]);//interm_cloud_->points[nn_indices[nn_index_i]]);
00112       float theta_i = expf ( (-1) * dist / scale_squared_);
00113       theta_normalization_factor += theta_i;
00114 
00115       smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
00116 
00117       theta[nn_index_i] = theta_i;
00118     }
00119 
00120     smoothed_normal /= theta_normalization_factor;
00121     smoothed_normal(3) = 0.0f;
00122     smoothed_normal.normalize ();
00123 
00124 
00125     // find minimum along the normal
00126     float e_residual;
00127     smoothed_point = interm_cloud_->points[i].getVector4fMap ();
00128     while (1)
00129     {
00130       e_residual = 0.0f;
00131       smoothed_point(3) = 0.0f;
00132       for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00133       {
00134         Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();//interm_cloud_->points[nn_indices[nn_index_i]].getVector4fMap ();
00135         neighbor(3) = 0.0f;
00136         float dot_product = smoothed_normal.dot (neighbor - smoothed_point);
00137         e_residual += theta[nn_index_i] * dot_product;// * dot_product;
00138       }
00139       e_residual /= theta_normalization_factor;
00140       if (e_residual < 1e-5) break;
00141 
00142       smoothed_point = smoothed_point + e_residual * smoothed_normal;
00143     }
00144 
00145     total_residual += e_residual;
00146 
00147     output_positions->points[i].getVector4fMap () = smoothed_point;
00148     output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();//smoothed_normal;
00149   }
00150 
00151 //  std::cerr << "Total residual after iteration: " << total_residual << std::endl;
00152 //  PCL_INFO("SurfelSmoothing done iteration\n");
00153   return total_residual;
00154 }
00155 
00156 
00157 template <typename PointT, typename PointNT> void
00158 pcl::SurfelSmoothing<PointT, PointNT>::smoothPoint (size_t &point_index,
00159                                                     PointT &output_point,
00160                                                     PointNT &output_normal)
00161 {
00162   Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
00163   Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap ();
00164   result_point(3) = 0.0f;
00165 
00166   // @todo parameter
00167   float error_residual_threshold_ = 1e-3f;
00168   float error_residual = error_residual_threshold_ + 1;
00169   float last_error_residual = error_residual + 100.0f;
00170 
00171   std::vector<int> nn_indices;
00172   std::vector<float> nn_distances;
00173 
00174   int big_iterations = 0;
00175   int max_big_iterations = 500;
00176 
00177   while (fabs (error_residual) < fabs (last_error_residual) -error_residual_threshold_ &&
00178          big_iterations < max_big_iterations)
00179   {
00180     average_normal = Eigen::Vector4f::Zero ();
00181     big_iterations ++;
00182     PointT aux_point; aux_point.x = result_point(0); aux_point.y = result_point(1); aux_point.z = result_point(2);
00183     tree_->radiusSearch (aux_point, 5*scale_, nn_indices, nn_distances);
00184 
00185     float theta_normalization_factor = 0.0;
00186     std::vector<float> theta (nn_indices.size ());
00187     for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00188     {
00189       float dist = nn_distances[nn_index_i];
00190       float theta_i = expf ( (-1) * dist / scale_squared_);
00191       theta_normalization_factor += theta_i;
00192 
00193       average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
00194       theta[nn_index_i] = theta_i;
00195     }
00196     average_normal /= theta_normalization_factor;
00197     average_normal(3) = 0.0f;
00198     average_normal.normalize ();
00199 
00200     // find minimum along the normal
00201     float e_residual_along_normal = 2, last_e_residual_along_normal = 3;
00202     int small_iterations = 0;
00203     int max_small_iterations = 10;
00204     while ( fabs (e_residual_along_normal) < fabs (last_e_residual_along_normal) &&
00205         small_iterations < max_small_iterations)
00206     {
00207       small_iterations ++;
00208 
00209       e_residual_along_normal = 0.0f;
00210       for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00211       {
00212         Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();
00213         neighbor(3) = 0.0f;
00214         float dot_product = average_normal.dot (neighbor - result_point);
00215         e_residual_along_normal += theta[nn_index_i] * dot_product;
00216       }
00217       e_residual_along_normal /= theta_normalization_factor;
00218       if (e_residual_along_normal < 1e-3) break;
00219 
00220       result_point = result_point + e_residual_along_normal * average_normal;
00221     }
00222 
00223 //    if (small_iterations == max_small_iterations)
00224 //      PCL_INFO ("passed the number of small iterations %d\n", small_iterations);
00225 
00226     last_error_residual = error_residual;
00227     error_residual = e_residual_along_normal;
00228 
00229 //    PCL_INFO ("last %f    current %f\n", last_error_residual, error_residual);
00230   }
00231 
00232   output_point.x = result_point(0);
00233   output_point.y = result_point(1);
00234   output_point.z = result_point(2);
00235   output_normal = normals_->points[point_index];
00236 
00237   if (big_iterations == max_big_iterations)
00238     PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations);
00239 }
00240 
00241 
00243 template <typename PointT, typename PointNT> void
00244 pcl::SurfelSmoothing<PointT, PointNT>::computeSmoothedCloud (PointCloudInPtr &output_positions,
00245                                                              NormalCloudPtr &output_normals)
00246 {
00247   if (!initCompute ())
00248   {
00249     PCL_ERROR ("[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n");
00250     return;
00251   }
00252 
00253   tree_->setInputCloud (input_);
00254 
00255   output_positions->header = input_->header;
00256   output_positions->height = input_->height;
00257   output_positions->width = input_->width;
00258 
00259   output_normals->header = input_->header;
00260   output_normals->height = input_->height;
00261   output_normals->width = input_->width;
00262 
00263   output_positions->points.resize (input_->points.size ());
00264   output_normals->points.resize (input_->points.size ());
00265   for (size_t i = 0; i < input_->points.size (); ++i)
00266   {
00267     smoothPoint (i, output_positions->points[i], output_normals->points[i]);
00268   }
00269 }
00270 
00272 template <typename PointT, typename PointNT> void
00273 pcl::SurfelSmoothing<PointT, PointNT>::extractSalientFeaturesBetweenScales (PointCloudInPtr &cloud2,
00274                                                                             NormalCloudPtr &cloud2_normals,
00275                                                                             boost::shared_ptr<std::vector<int> > &output_features)
00276 {
00277   if (interm_cloud_->points.size () != cloud2->points.size () || 
00278       cloud2->points.size () != cloud2_normals->points.size ())
00279   {
00280     PCL_ERROR ("[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n");
00281     return;
00282   }
00283 
00284   std::vector<float> diffs (cloud2->points.size ());
00285   for (size_t i = 0; i < cloud2->points.size (); ++i)
00286     diffs[i] = cloud2_normals->points[i].getNormalVector4fMap ().dot (cloud2->points[i].getVector4fMap () - 
00287                                                                       interm_cloud_->points[i].getVector4fMap ());
00288 
00289   std::vector<int> nn_indices;
00290   std::vector<float> nn_distances;
00291 
00292   output_features->resize (cloud2->points.size ());
00293   for (int point_i = 0; point_i < static_cast<int> (cloud2->points.size ()); ++point_i)
00294   {
00295     // Get neighbors
00296     tree_->radiusSearch (point_i, scale_, nn_indices, nn_distances);
00297 
00298     bool largest = true;
00299     bool smallest = true;
00300     for (std::vector<int>::iterator nn_index_it = nn_indices.begin (); nn_index_it != nn_indices.end (); ++nn_index_it)
00301     {
00302       if (diffs[point_i] < diffs[*nn_index_it])
00303         largest = false;
00304       else 
00305         smallest = false;
00306     }
00307 
00308     if (largest == true || smallest == true)
00309       (*output_features)[point_i] = point_i;
00310   }
00311 }
00312 
00313 
00314 
00315 #define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing<PointT, PointNT>;
00316 
00317 #endif /* PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:34:11