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