Point Cloud Library (PCL)  1.3.1
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 
00044 template <typename PointT, typename PointNT> bool
00045 pcl::SurfelSmoothing<PointT, PointNT>::initCompute ()
00046 {
00047   if (!PCLBase<PointT>::initCompute ())
00048     return false;
00049 
00050   if (!normals_)
00051   {
00052     PCL_ERROR ("SurfelSmoothing: normal cloud not set\n");
00053     return false;
00054   }
00055 
00056   if (input_->points.size () != normals_->points.size ())
00057   {
00058     PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n");
00059     return false;
00060   }
00061 
00062   // Initialize the spatial locator
00063   if (!tree_)
00064   {
00065     if (input_->isOrganized ())
00066       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00067     else
00068       tree_.reset (new pcl::search::KdTree<PointT> (false));
00069   }
00070 
00071   // create internal copies of the input - these will be modified
00072   interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_));
00073   interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_));
00074 
00075   return (true);
00076 }
00077 
00078 
00080 template <typename PointT, typename PointNT> float
00081 pcl::SurfelSmoothing<PointT, PointNT>::smoothCloudIteration (PointCloudInPtr &output_positions,
00082                                                              NormalCloudPtr &output_normals)
00083 {
00084 //  PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n");
00085 
00086   output_positions = PointCloudInPtr (new PointCloudIn);
00087   output_positions->points.resize (interm_cloud_->points.size ());
00088   output_normals = NormalCloudPtr (new NormalCloud);
00089   output_normals->points.resize (interm_cloud_->points.size ());
00090 
00091   std::vector<int> nn_indices;
00092   std::vector<float> nn_distances;
00093 
00094   std::vector<float> diffs (interm_cloud_->points.size ());
00095   float total_residual = 0.0f;
00096 
00097   for (size_t i = 0; i < interm_cloud_->points.size (); ++i)
00098   {
00099     Eigen::Vector4f smoothed_point  = Eigen::Vector4f::Zero ();
00100     Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero (); 
00101 
00102     // get neighbors
00103     // @todo using 5x the scale for searching instead of all the points to avoid O(N^2)
00104     tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances);
00105 
00106     float theta_normalization_factor = 0.0;
00107     std::vector<float> theta (nn_indices.size ());
00108     for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00109     {
00110       float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], input_->points[nn_indices[nn_index_i]]);//interm_cloud_->points[nn_indices[nn_index_i]]);
00111       float theta_i = exp ( (-1) * dist / scale_squared_);
00112       theta_normalization_factor += theta_i;
00113 
00114       smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
00115 
00116       theta[nn_index_i] = theta_i;
00117     }
00118 
00119     smoothed_normal /= theta_normalization_factor;
00120     smoothed_normal(3) = 0.0f;
00121     smoothed_normal.normalize ();
00122 
00123 
00124     // find minimum along the normal
00125     float e_residual;
00126     smoothed_point = interm_cloud_->points[i].getVector4fMap ();
00127     while (1)
00128     {
00129       e_residual = 0.0f;
00130       smoothed_point(3) = 0.0f;
00131       for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00132       {
00133         Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();//interm_cloud_->points[nn_indices[nn_index_i]].getVector4fMap ();
00134         neighbor(3) = 0.0f;
00135         float dot_product = smoothed_normal.dot (neighbor - smoothed_point);
00136         e_residual += theta[nn_index_i] * dot_product;// * dot_product;
00137       }
00138       e_residual /= theta_normalization_factor;
00139       if (e_residual < 1e-5) break;
00140 
00141       smoothed_point = smoothed_point + e_residual * smoothed_normal;
00142     }
00143 
00144     total_residual += e_residual;
00145 
00146     output_positions->points[i].getVector4fMap () = smoothed_point;
00147     output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();//smoothed_normal;
00148   }
00149 
00150 //  std::cerr << "Total residual after iteration: " << total_residual << std::endl;
00151 //  PCL_INFO("SurfelSmoothing done iteration\n");
00152   return total_residual;
00153 }
00154 
00155 
00156 template <typename PointT, typename PointNT> void
00157 pcl::SurfelSmoothing<PointT, PointNT>::smoothPoint (size_t &point_index,
00158                                                     PointT &output_point,
00159                                                     PointNT &output_normal)
00160 {
00161   Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
00162   Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap ();
00163   result_point(3) = 0.0f;
00164 
00165   // @todo parameter
00166   float error_residual_threshold_ = 1e-3;
00167   float error_residual = error_residual_threshold_ + 1;
00168   float last_error_residual = error_residual + 100;
00169 
00170   std::vector<int> nn_indices;
00171   std::vector<float> nn_distances;
00172 
00173   int big_iterations = 0;
00174   int max_big_iterations = 500;
00175 
00176   while (fabs (error_residual) < fabs (last_error_residual) -error_residual_threshold_ &&
00177          big_iterations < max_big_iterations)
00178   {
00179     average_normal = Eigen::Vector4f::Zero ();
00180     big_iterations ++;
00181     PointT aux_point; aux_point.x = result_point(0); aux_point.y = result_point(1); aux_point.z = result_point(2);
00182     tree_->radiusSearch (aux_point, 5*scale_, nn_indices, nn_distances);
00183 
00184     float theta_normalization_factor = 0.0;
00185     std::vector<float> theta (nn_indices.size ());
00186     for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00187     {
00188       float dist = nn_distances[nn_index_i];
00189       float theta_i = exp ( (-1) * dist / scale_squared_);
00190       theta_normalization_factor += theta_i;
00191 
00192       average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
00193       theta[nn_index_i] = theta_i;
00194     }
00195     average_normal /= theta_normalization_factor;
00196     average_normal(3) = 0.0f;
00197     average_normal.normalize ();
00198 
00199     // find minimum along the normal
00200     float e_residual_along_normal = 2, last_e_residual_along_normal = 3;
00201     int small_iterations = 0;
00202     int max_small_iterations = 10;
00203     while ( fabs (e_residual_along_normal) < fabs (last_e_residual_along_normal) &&
00204         small_iterations < max_small_iterations)
00205     {
00206       small_iterations ++;
00207 
00208       e_residual_along_normal = 0.0f;
00209       for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
00210       {
00211         Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();
00212         neighbor(3) = 0.0f;
00213         float dot_product = average_normal.dot (neighbor - result_point);
00214         e_residual_along_normal += theta[nn_index_i] * dot_product;
00215       }
00216       e_residual_along_normal /= theta_normalization_factor;
00217       if (e_residual_along_normal < 1e-3) break;
00218 
00219       result_point = result_point + e_residual_along_normal * average_normal;
00220     }
00221 
00222 //    if (small_iterations == max_small_iterations)
00223 //      PCL_INFO ("passed the number of small iterations %d\n", small_iterations);
00224 
00225     last_error_residual = error_residual;
00226     error_residual = e_residual_along_normal;
00227 
00228 //    PCL_INFO ("last %f    current %f\n", last_error_residual, error_residual);
00229   }
00230 
00231   output_point.x = result_point(0);
00232   output_point.y = result_point(1);
00233   output_point.z = result_point(2);
00234   output_normal = normals_->points[point_index];
00235 
00236   if (big_iterations == max_big_iterations)
00237     PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations);
00238 }
00239 
00240 
00242 template <typename PointT, typename PointNT> void
00243 pcl::SurfelSmoothing<PointT, PointNT>::computeSmoothedCloud (PointCloudInPtr &output_positions,
00244                                                              NormalCloudPtr &output_normals)
00245 {
00246   if (!initCompute ())
00247   {
00248     PCL_ERROR ("[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n");
00249     return;
00250   }
00251 
00252   tree_->setInputCloud (input_);
00253 
00254   output_positions->header = input_->header;
00255   output_positions->height = input_->height;
00256   output_positions->width = input_->width;
00257 
00258   output_normals->header = input_->header;
00259   output_normals->height = input_->height;
00260   output_normals->width = input_->width;
00261 
00262   output_positions->points.resize (input_->points.size ());
00263   output_normals->points.resize (input_->points.size ());
00264   for (size_t i = 0; i < input_->points.size (); ++i)
00265   {
00266     smoothPoint (i, output_positions->points[i], output_normals->points[i]);
00267   }
00268 }
00269 
00271 template <typename PointT, typename PointNT> void
00272 pcl::SurfelSmoothing<PointT, PointNT>::extractSalientFeaturesBetweenScales (PointCloudInPtr &cloud2,
00273                                                                             NormalCloudPtr &cloud2_normals,
00274                                                                             boost::shared_ptr<std::vector<int> > &output_features)
00275 {
00276   if (interm_cloud_->points.size () != cloud2->points.size () || 
00277       cloud2->points.size () != cloud2_normals->points.size ())
00278   {
00279     PCL_ERROR ("[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n");
00280     return;
00281   }
00282 
00283   std::vector<float> diffs (cloud2->points.size ());
00284   for (size_t i = 0; i < cloud2->points.size (); ++i)
00285     diffs[i] = cloud2_normals->points[i].getNormalVector4fMap ().dot (cloud2->points[i].getVector4fMap () - 
00286                                                                       interm_cloud_->points[i].getVector4fMap ());
00287 
00288   std::vector<int> nn_indices;
00289   std::vector<float> nn_distances;
00290 
00291   output_features->resize (cloud2->points.size ());
00292   for (size_t point_i = 0; point_i < cloud2->points.size (); ++point_i)
00293   {
00294     // Get neighbors
00295     tree_->radiusSearch (point_i, scale_, nn_indices, nn_distances);
00296 
00297     bool largest = true;
00298     bool smallest = true;
00299     for (std::vector<int>::iterator nn_index_it = nn_indices.begin (); nn_index_it != nn_indices.end (); ++nn_index_it)
00300     {
00301       if (diffs[point_i] < diffs[*nn_index_it])
00302         largest = false;
00303       else 
00304         smallest = false;
00305     }
00306 
00307     if (largest == true || smallest == true)
00308       (*output_features)[point_i] = point_i;
00309   }
00310 }
00311 
00312 
00313 
00314 #define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing<PointT, PointNT>;
00315 
00316 #endif /* PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines