Point Cloud Library (PCL)
1.3.1
|
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