feature.hpp

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: feature.hpp 3022 2011-11-01 03:42:22Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
00039 #define PCL_FEATURES_IMPL_FEATURE_H_
00040 
00041 #include <pcl/search/pcl_search.h>
00042 
00044 inline void
00045 pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 
00046                            const Eigen::Vector4f &point,
00047                            Eigen::Vector4f &plane_parameters, float &curvature)
00048 {
00049   // Avoid getting hung on Eigen's optimizers
00050   for (int i = 0; i < 3; ++i)
00051     for (int j = 0; j < 3; ++j)
00052       if (!pcl_isfinite (covariance_matrix (i, j)))
00053       {
00054         //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
00055         plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00056         curvature = std::numeric_limits<float>::quiet_NaN ();
00057         return;
00058       }
00059 
00060   // Extract the eigenvalues and eigenvectors
00061   //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix);
00062   //EIGEN_ALIGN16 Eigen::Vector3f eigen_values  = ei_symm.eigenvalues ();
00063   //EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
00064   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00065   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00066   pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00067 
00068   // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
00069   // Note: Remember to take care of the eigen_vectors ordering
00070   //float norm = 1.0 / eigen_vectors.col (0).norm ();
00071 
00072   //plane_parameters[0] = eigen_vectors (0, 0) * norm;
00073   //plane_parameters[1] = eigen_vectors (1, 0) * norm;
00074   //plane_parameters[2] = eigen_vectors (2, 0) * norm;
00075 
00076   // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
00077   plane_parameters[0] = eigen_vectors (0, 0);
00078   plane_parameters[1] = eigen_vectors (1, 0);
00079   plane_parameters[2] = eigen_vectors (2, 0);
00080   plane_parameters[3] = 0;
00081 
00082   // Hessian form (D = nc . p_plane (centroid here) + p)
00083   plane_parameters[3] = -1 * plane_parameters.dot (point);
00084 
00085   // Compute the curvature surface change
00086   float eig_sum = eigen_values.sum ();
00087   if (eig_sum != 0)
00088     curvature = fabs ( eigen_values[0] / eig_sum );
00089   else
00090     curvature = 0;
00091 }
00092 
00094 inline void
00095 pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
00096                            float &nx, float &ny, float &nz, float &curvature)
00097 {
00098   // Avoid getting hung on Eigen's optimizers
00099   for (int i = 0; i < 3; ++i)
00100     for (int j = 0; j < 3; ++j)
00101       if (!pcl_isfinite (covariance_matrix (i, j)))
00102       {
00103         //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
00104         nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
00105         return;
00106       }
00107   // Extract the eigenvalues and eigenvectors
00108   //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix);
00109   //EIGEN_ALIGN16 Eigen::Vector3f eigen_values  = ei_symm.eigenvalues ();
00110   //EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
00111   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00112   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00113   pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00114 
00115   // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
00116   // Note: Remember to take care of the eigen_vectors ordering
00117   //float norm = 1.0 / eigen_vectors.col (0).norm ();
00118 
00119   //nx = eigen_vectors (0, 0) * norm;
00120   //ny = eigen_vectors (1, 0) * norm;
00121   //nz = eigen_vectors (2, 0) * norm;
00122   
00123   // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
00124   nx = eigen_vectors (0, 0);
00125   ny = eigen_vectors (1, 0);
00126   nz = eigen_vectors (2, 0);
00127 
00128   // Compute the curvature surface change
00129   float eig_sum = eigen_values.sum ();
00130   if (eig_sum != 0)
00131     curvature = fabs ( eigen_values[0] / eig_sum );
00132   else
00133     curvature = 0;
00134 }
00135 
00139 template <typename PointInT, typename PointOutT> bool
00140 pcl::Feature<PointInT, PointOutT>::initCompute ()
00141 {
00142   if (!PCLBase<PointInT>::initCompute ())
00143   {
00144     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00145     return (false);
00146   }
00147 
00148   // If the dataset is empty, just return
00149   if (input_->points.empty ())
00150   {
00151     PCL_ERROR ("[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ());
00152     // Cleanup
00153     deinitCompute ();
00154     return (false);
00155   }
00156 
00157   // If no search surface has been defined, use the input dataset as the search surface itself
00158   if (!surface_)
00159   {
00160     fake_surface_ = true;
00161     surface_ = input_;
00162   }
00163 
00164   // Check if a space search locator was given
00165   if (!tree_)
00166   {
00167     if (surface_->isOrganized () && input_->isOrganized ())
00168       tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
00169     else
00170       tree_.reset (new pcl::search::KdTree<PointInT> (false));
00171   }
00172   // Send the surface dataset to the spatial locator
00173   tree_->setInputCloud (surface_);
00174 
00175   // Do a fast check to see if the search parameters are well defined
00176   if (search_radius_ != 0.0)
00177   {
00178     if (k_ != 0)
00179     {
00180       PCL_ERROR ("[pcl::%s::compute] ", getClassName ().c_str ());
00181       PCL_ERROR ("Both radius (%f) and K (%d) defined! ", search_radius_, k_);
00182       PCL_ERROR ("Set one of them to zero first and then re-run compute ().\n");
00183       // Cleanup
00184       deinitCompute ();
00185       return (false);
00186     }
00187     else // Use the radiusSearch () function
00188     {
00189       search_parameter_ = search_radius_;
00190       if (surface_ == input_) // if the two surfaces are the same
00191       {
00192         // Declare the search locator definition
00193         int (KdTree::*radiusSearch)(int index, double radius, std::vector<int> &k_indices,
00194                                     std::vector<float> &k_distances, int max_nn) const = &KdTree::radiusSearch;
00195         search_method_ = boost::bind (radiusSearch, boost::ref (tree_), _1, _2, _3, _4, INT_MAX);
00196       }
00197 
00198       // Declare the search locator definition
00199       int (KdTree::*radiusSearchSurface)(const PointCloudIn &cloud, int index, double radius,
00200                                          std::vector<int> &k_indices, std::vector<float> &k_distances,
00201                                          int max_nn) = &pcl::search::Search<PointInT>::radiusSearch;
00202       search_method_surface_ = boost::bind (radiusSearchSurface, boost::ref (tree_), _1, _2, _3, _4, _5, INT_MAX);
00203     }
00204   }
00205   else
00206   {
00207     if (k_ != 0) // Use the nearestKSearch () function
00208     {
00209       search_parameter_ = k_;
00210       if (surface_ == input_) // if the two surfaces are the same
00211       {
00212         // Declare the search locator definition
00213         int (KdTree::*nearestKSearch)(int index, int k, std::vector<int> &k_indices, 
00214                                       std::vector<float> &k_distances) = &KdTree::nearestKSearch;
00215         search_method_ = boost::bind (nearestKSearch, boost::ref (tree_), _1, _2, _3, _4);
00216       }
00217       // Declare the search locator definition
00218       int (KdTree::*nearestKSearchSurface)(const PointCloudIn &cloud, int index, int k, std::vector<int> &k_indices, 
00219                                            std::vector<float> &k_distances) = &KdTree::nearestKSearch;
00220       search_method_surface_ = boost::bind (nearestKSearchSurface, boost::ref (tree_), _1, _2, _3, _4, _5);
00221     }
00222     else
00223     {
00224       PCL_ERROR ("[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
00225       PCL_ERROR ("Set one of them to a positive number first and then re-run compute ().\n");
00226       // Cleanup
00227       deinitCompute ();
00228       return (false);
00229     }
00230   }
00231   return (true);
00232 }
00233 
00235 template <typename PointInT, typename PointOutT> bool
00236 pcl::Feature<PointInT, PointOutT>::deinitCompute ()
00237 {
00238   // Reset the surface
00239   if (fake_surface_)
00240   {
00241     surface_.reset ();
00242     fake_surface_ = false;
00243   }
00244   return (true);
00245 }
00246 
00248 template <typename PointInT, typename PointOutT> void
00249 pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
00250 {
00251   if (!initCompute ())
00252   {
00253     output.width = output.height = 0;
00254     output.points.clear ();
00255     return;
00256   }
00257 
00258   // Copy the header
00259   output.header = input_->header;
00260 
00261   // Resize the output dataset
00262   if (output.points.size () != indices_->size ())
00263     output.points.resize (indices_->size ());
00264   // Check if the output will be computed for all points or only a subset
00265   if (indices_->size () != input_->points.size ())
00266   {
00267     output.width = (int) indices_->size ();
00268     output.height = 1;
00269   }
00270   else
00271   {
00272     output.width = input_->width;
00273     output.height = input_->height;
00274   }
00275   output.is_dense = input_->is_dense;
00276 
00277   // Perform the actual feature computation
00278   computeFeature (output);
00279 
00280   deinitCompute ();
00281 }
00282 
00286 template <typename PointInT, typename PointNT, typename PointOutT> bool
00287 pcl::FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()
00288 {
00289   if (!Feature<PointInT, PointOutT>::initCompute ())
00290   {
00291     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00292     return (false);
00293   }
00294 
00295   // Check if input normals are set
00296   if (!normals_)
00297   {
00298     PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
00299     Feature<PointInT, PointOutT>::deinitCompute();
00300     return (false);
00301   }
00302 
00303   // Check if the size of normals is the same as the size of the surface
00304   if (normals_->points.size () != surface_->points.size ())
00305   {
00306     PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
00307     PCL_ERROR ("The number of points in the input dataset differs from ");
00308     PCL_ERROR ("the number of points in the dataset containing the normals!\n");
00309     Feature<PointInT, PointOutT>::deinitCompute();
00310     return (false);
00311   }
00312 
00313   return (true);
00314 }
00315 
00316 #endif  //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
00317