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 1370 2011-06-19 01:06:01Z jspricke $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
00039 #define PCL_FEATURES_IMPL_FEATURE_H_
00040 
00042 inline void
00043 pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 
00044                            const Eigen::Vector4f &point,
00045                            Eigen::Vector4f &plane_parameters, float &curvature)
00046 {
00047   // Avoid getting hung on Eigen's optimizers
00048   for (int i = 0; i < 3; ++i)
00049     for (int j = 0; j < 3; ++j)
00050       if (!pcl_isfinite (covariance_matrix (i, j)))
00051       {
00052         //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
00053         plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00054         curvature = std::numeric_limits<float>::quiet_NaN ();
00055         return;
00056       }
00057 
00058   // Extract the eigenvalues and eigenvectors
00059   //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix);
00060   //EIGEN_ALIGN16 Eigen::Vector3f eigen_values  = ei_symm.eigenvalues ();
00061   //EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
00062   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00063   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00064   pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00065 
00066   // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
00067   // Note: Remember to take care of the eigen_vectors ordering
00068   //float norm = 1.0 / eigen_vectors.col (0).norm ();
00069 
00070   //plane_parameters[0] = eigen_vectors (0, 0) * norm;
00071   //plane_parameters[1] = eigen_vectors (1, 0) * norm;
00072   //plane_parameters[2] = eigen_vectors (2, 0) * norm;
00073 
00074   // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
00075   plane_parameters[0] = eigen_vectors (0, 0);
00076   plane_parameters[1] = eigen_vectors (1, 0);
00077   plane_parameters[2] = eigen_vectors (2, 0);
00078   plane_parameters[3] = 0;
00079 
00080   // Hessian form (D = nc . p_plane (centroid here) + p)
00081   plane_parameters[3] = -1 * plane_parameters.dot (point);
00082 
00083   // Compute the curvature surface change
00084   float eig_sum = eigen_values.sum ();
00085   if (eig_sum != 0)
00086     curvature = fabs ( eigen_values[0] / eig_sum );
00087   else
00088     curvature = 0;
00089 }
00090 
00092 inline void
00093 pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
00094                            float &nx, float &ny, float &nz, float &curvature)
00095 {
00096   // Avoid getting hung on Eigen's optimizers
00097   for (int i = 0; i < 3; ++i)
00098     for (int j = 0; j < 3; ++j)
00099       if (!pcl_isfinite (covariance_matrix (i, j)))
00100       {
00101         //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
00102         nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
00103         return;
00104       }
00105   // Extract the eigenvalues and eigenvectors
00106   //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix);
00107   //EIGEN_ALIGN16 Eigen::Vector3f eigen_values  = ei_symm.eigenvalues ();
00108   //EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
00109   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00110   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00111   pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00112 
00113   // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
00114   // Note: Remember to take care of the eigen_vectors ordering
00115   //float norm = 1.0 / eigen_vectors.col (0).norm ();
00116 
00117   //nx = eigen_vectors (0, 0) * norm;
00118   //ny = eigen_vectors (1, 0) * norm;
00119   //nz = eigen_vectors (2, 0) * norm;
00120   
00121   // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
00122   nx = eigen_vectors (0, 0);
00123   ny = eigen_vectors (1, 0);
00124   nz = eigen_vectors (2, 0);
00125 
00126   // Compute the curvature surface change
00127   float eig_sum = eigen_values.sum ();
00128   if (eig_sum != 0)
00129     curvature = fabs ( eigen_values[0] / eig_sum );
00130   else
00131     curvature = 0;
00132 }
00133 
00137 template <typename PointInT, typename PointOutT> void
00138 pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
00139 {
00140   // Copy the header
00141   output.header = input_->header;
00142 
00143   if (!initCompute ()) 
00144   {
00145     PCL_ERROR ("[pcl::%s::compute] Init failed.\n", getClassName ().c_str ());
00146     output.width = output.height = 0;
00147     output.points.clear ();
00148     return;
00149   }
00150 
00151   // If the dataset is empty, just return
00152   if (input_->points.empty ())
00153   {
00154     PCL_ERROR ("[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ());
00155     output.width = output.height = 0;
00156     output.points.clear ();
00157     deinitCompute ();
00158     return;
00159   }
00160 
00161   // Check if a space search locator was given
00162   if (!tree_)
00163   {
00164     PCL_ERROR ("[pcl::%s::compute] No spatial search method was given!\n", getClassName ().c_str ());
00165     output.width = output.height = 0;
00166     output.points.clear ();
00167     deinitCompute ();
00168     return;
00169   }
00170 
00171   // If no search surface has been defined, use the input dataset as the search surface itself
00172   if (!surface_)
00173   {
00174     fake_surface_ = true;
00175     surface_ = input_;
00176   }
00177 
00178   // Send the surface dataset to the spatial locator
00179   tree_->setInputCloud (surface_);
00180 
00181   // Do a fast check to see if the search parameters are well defined
00182   if (search_radius_ != 0.0)
00183   {
00184     if (k_ != 0)
00185     {
00186       PCL_ERROR ("[pcl::%s::compute] Both radius (%f) and K (%d) defined! Set one of them to zero first and then re-run compute ().\n", getClassName ().c_str (), search_radius_, k_);
00187       output.width = output.height = 0;
00188       output.points.clear ();
00189 
00190       // Cleanup
00191       deinitCompute ();
00192       // Reset the surface
00193       if (fake_surface_)
00194       {
00195         surface_.reset ();
00196         fake_surface_ = false;
00197       }
00198       return;
00199     }
00200     else                  // Use the radiusSearch () function
00201     {
00202       search_parameter_ = search_radius_;
00203       if (surface_ == input_)       // if the two surfaces are the same
00204       {
00205         // Declare the search locator definition
00206         int (KdTree::*radiusSearch)(int index, double radius, std::vector<int> &k_indices,
00207                                     std::vector<float> &k_distances, int max_nn) const = &KdTree::radiusSearch;
00208         search_method_ = boost::bind (radiusSearch, boost::ref (tree_), _1, _2, _3, _4, INT_MAX);
00209       }
00210       else
00211       {
00212         // Declare the search locator definition
00213         int (KdTree::*radiusSearchSurface)(const PointCloudIn &cloud, int index, double radius, std::vector<int> &k_indices,
00214                                             std::vector<float> &k_distances, int max_nn) const = &KdTree::radiusSearch;
00215         search_method_surface_ = boost::bind (radiusSearchSurface, boost::ref (tree_), _1, _2, _3, _4, _5, INT_MAX);
00216       }
00217     }
00218   }
00219   else
00220   {
00221     if (k_ != 0)         // Use the nearestKSearch () function
00222     {
00223       search_parameter_ = k_;
00224       if (surface_ == input_)       // if the two surfaces are the same
00225       {
00226         // Declare the search locator definition
00227         int (KdTree::*nearestKSearch)(int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = &KdTree::nearestKSearch;
00228         search_method_ = boost::bind (nearestKSearch, boost::ref (tree_), _1, _2, _3, _4);
00229       }
00230       else
00231       {
00232         // Declare the search locator definition
00233         int (KdTree::*nearestKSearchSurface)(const PointCloudIn &cloud, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = &KdTree::nearestKSearch;
00234         search_method_surface_ = boost::bind (nearestKSearchSurface, boost::ref (tree_), _1, _2, _3, _4, _5);
00235       }
00236     }
00237     else
00238     {
00239       PCL_ERROR ("[pcl::%s::compute] Neither radius nor K defined! Set one of them to a positive number first and then re-run compute ().\n", getClassName ().c_str ());
00240       output.width = output.height = 0;
00241       output.points.clear ();
00242 
00243       // Cleanup
00244       deinitCompute ();
00245       // Reset the surface
00246       if (fake_surface_)
00247       {
00248         surface_.reset ();
00249         fake_surface_ = false;
00250       }
00251       return;
00252     }
00253   }
00254 
00255   // Resize the output dataset
00256   if (output.points.size () != indices_->size ())
00257     output.points.resize (indices_->size ());
00258   // Check if the output will be computed for all points or only a subset
00259   if (indices_->size () != input_->points.size ())
00260   {
00261     output.width    = indices_->size ();
00262     output.height   = 1;
00263   }
00264   else
00265   {
00266     output.width    = input_->width;
00267     output.height   = input_->height;
00268   }
00269   output.is_dense = input_->is_dense;
00270 
00271   // Perform the actual feature computation
00272   computeFeature (output);
00273 
00274   deinitCompute ();
00275 
00276   // Reset the surface
00277   if (fake_surface_)
00278   {
00279     surface_.reset ();
00280     fake_surface_ = false;
00281   }
00282 }
00283 
00284 #endif  //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
00285