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_FEATURE_H_ 00042 #define PCL_FEATURE_H_ 00043 00044 #if defined __GNUC__ 00045 # pragma GCC system_header 00046 #endif 00047 00048 #include <boost/function.hpp> 00049 #include <boost/bind.hpp> 00050 // PCL includes 00051 #include <pcl/pcl_base.h> 00052 #include <pcl/search/search.h> 00053 00054 namespace pcl 00055 { 00056 /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares 00057 * plane normal and surface curvature. 00058 * \param covariance_matrix the 3x3 covariance matrix 00059 * \param point a point lying on the least-squares plane (SSE aligned) 00060 * \param plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0) 00061 * \param curvature the estimated surface curvature as a measure of 00062 * \f[ 00063 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 00064 * \f] 00065 * \ingroup features 00066 */ 00067 inline void 00068 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 00069 const Eigen::Vector4f &point, 00070 Eigen::Vector4f &plane_parameters, float &curvature); 00071 00072 /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares 00073 * plane normal and surface curvature. 00074 * \param covariance_matrix the 3x3 covariance matrix 00075 * \param nx the resultant X component of the plane normal 00076 * \param ny the resultant Y component of the plane normal 00077 * \param nz the resultant Z component of the plane normal 00078 * \param curvature the estimated surface curvature as a measure of 00079 * \f[ 00080 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 00081 * \f] 00082 * \ingroup features 00083 */ 00084 inline void 00085 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 00086 float &nx, float &ny, float &nz, float &curvature); 00087 00088 //////////////////////////////////////////////////////////////////////////////////////////// 00089 //////////////////////////////////////////////////////////////////////////////////////////// 00090 //////////////////////////////////////////////////////////////////////////////////////////// 00091 /** \brief Feature represents the base feature class. Some generic 3D operations that 00092 * are applicable to all features are defined here as static methods. 00093 * 00094 * \attention 00095 * The convention for a feature descriptor is: 00096 * - if the nearest neighbors for the query point at which the descriptor is to be computed cannot be 00097 * determined, the descriptor values will be set to NaN (not a number) 00098 * - it is impossible to estimate a feature descriptor for a point that doesn't have finite 3D coordinates. 00099 * Therefore, any point that has NaN data on x, y, or z, will most likely have its descriptor set to NaN. 00100 * 00101 * \author Radu B. Rusu 00102 * \ingroup features 00103 */ 00104 template <typename PointInT, typename PointOutT> 00105 class Feature : public PCLBase<PointInT> 00106 { 00107 public: 00108 using PCLBase<PointInT>::indices_; 00109 using PCLBase<PointInT>::input_; 00110 00111 typedef PCLBase<PointInT> BaseClass; 00112 00113 typedef boost::shared_ptr< Feature<PointInT, PointOutT> > Ptr; 00114 typedef boost::shared_ptr< const Feature<PointInT, PointOutT> > ConstPtr; 00115 00116 typedef typename pcl::search::Search<PointInT> KdTree; 00117 typedef typename pcl::search::Search<PointInT>::Ptr KdTreePtr; 00118 00119 typedef pcl::PointCloud<PointInT> PointCloudIn; 00120 typedef typename PointCloudIn::Ptr PointCloudInPtr; 00121 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; 00122 00123 typedef pcl::PointCloud<PointOutT> PointCloudOut; 00124 00125 typedef boost::function<int (size_t, double, std::vector<int> &, std::vector<float> &)> SearchMethod; 00126 typedef boost::function<int (const PointCloudIn &cloud, size_t index, double, std::vector<int> &, std::vector<float> &)> SearchMethodSurface; 00127 00128 public: 00129 /** \brief Empty constructor. */ 00130 Feature () : 00131 feature_name_ (), search_method_surface_ (), 00132 surface_(), tree_(), 00133 search_parameter_(0), search_radius_(0), k_(0), 00134 fake_surface_(false) 00135 {} 00136 00137 /** \brief Empty destructor */ 00138 virtual ~Feature () {} 00139 00140 /** \brief Provide a pointer to a dataset to add additional information 00141 * to estimate the features for every point in the input dataset. This 00142 * is optional, if this is not set, it will only use the data in the 00143 * input cloud to estimate the features. This is useful when you only 00144 * need to compute the features for a downsampled cloud. 00145 * \param[in] cloud a pointer to a PointCloud message 00146 */ 00147 inline void 00148 setSearchSurface (const PointCloudInConstPtr &cloud) 00149 { 00150 surface_ = cloud; 00151 fake_surface_ = false; 00152 //use_surface_ = true; 00153 } 00154 00155 /** \brief Get a pointer to the surface point cloud dataset. */ 00156 inline PointCloudInConstPtr 00157 getSearchSurface () const 00158 { 00159 return (surface_); 00160 } 00161 00162 /** \brief Provide a pointer to the search object. 00163 * \param[in] tree a pointer to the spatial search object. 00164 */ 00165 inline void 00166 setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } 00167 00168 /** \brief Get a pointer to the search method used. */ 00169 inline KdTreePtr 00170 getSearchMethod () const 00171 { 00172 return (tree_); 00173 } 00174 00175 /** \brief Get the internal search parameter. */ 00176 inline double 00177 getSearchParameter () const 00178 { 00179 return (search_parameter_); 00180 } 00181 00182 /** \brief Set the number of k nearest neighbors to use for the feature estimation. 00183 * \param[in] k the number of k-nearest neighbors 00184 */ 00185 inline void 00186 setKSearch (int k) { k_ = k; } 00187 00188 /** \brief get the number of k nearest neighbors used for the feature estimation. */ 00189 inline int 00190 getKSearch () const 00191 { 00192 return (k_); 00193 } 00194 00195 /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature 00196 * estimation. 00197 * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor 00198 */ 00199 inline void 00200 setRadiusSearch (double radius) 00201 { 00202 search_radius_ = radius; 00203 } 00204 00205 /** \brief Get the sphere radius used for determining the neighbors. */ 00206 inline double 00207 getRadiusSearch () const 00208 { 00209 return (search_radius_); 00210 } 00211 00212 /** \brief Base method for feature estimation for all points given in 00213 * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () 00214 * and the spatial locator in setSearchMethod () 00215 * \param[out] output the resultant point cloud model dataset containing the estimated features 00216 */ 00217 void 00218 compute (PointCloudOut &output); 00219 00220 protected: 00221 /** \brief The feature name. */ 00222 std::string feature_name_; 00223 00224 /** \brief The search method template for points. */ 00225 SearchMethodSurface search_method_surface_; 00226 00227 /** \brief An input point cloud describing the surface that is to be used 00228 * for nearest neighbors estimation. 00229 */ 00230 PointCloudInConstPtr surface_; 00231 00232 /** \brief A pointer to the spatial search object. */ 00233 KdTreePtr tree_; 00234 00235 /** \brief The actual search parameter (from either \a search_radius_ or \a k_). */ 00236 double search_parameter_; 00237 00238 /** \brief The nearest neighbors search radius for each point. */ 00239 double search_radius_; 00240 00241 /** \brief The number of K nearest neighbors to use for each point. */ 00242 int k_; 00243 00244 /** \brief Get a string representation of the name of this class. */ 00245 inline const std::string& 00246 getClassName () const { return (feature_name_); } 00247 00248 /** \brief This method should get called before starting the actual computation. */ 00249 virtual bool 00250 initCompute (); 00251 00252 /** \brief This method should get called after ending the actual computation. */ 00253 virtual bool 00254 deinitCompute (); 00255 00256 /** \brief If no surface is given, we use the input PointCloud as the surface. */ 00257 bool fake_surface_; 00258 00259 /** \brief Search for k-nearest neighbors using the spatial locator from 00260 * \a setSearchmethod, and the given surface from \a setSearchSurface. 00261 * \param[in] index the index of the query point 00262 * \param[in] parameter the search parameter (either k or radius) 00263 * \param[out] indices the resultant vector of indices representing the k-nearest neighbors 00264 * \param[out] distances the resultant vector of distances representing the distances from the query point to the 00265 * k-nearest neighbors 00266 * 00267 * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0. 00268 */ 00269 inline int 00270 searchForNeighbors (size_t index, double parameter, 00271 std::vector<int> &indices, std::vector<float> &distances) const 00272 { 00273 return (search_method_surface_ (*input_, index, parameter, indices, distances)); 00274 } 00275 00276 /** \brief Search for k-nearest neighbors using the spatial locator from 00277 * \a setSearchmethod, and the given surface from \a setSearchSurface. 00278 * \param[in] cloud the query point cloud 00279 * \param[in] index the index of the query point in \a cloud 00280 * \param[in] parameter the search parameter (either k or radius) 00281 * \param[out] indices the resultant vector of indices representing the k-nearest neighbors 00282 * \param[out] distances the resultant vector of distances representing the distances from the query point to the 00283 * k-nearest neighbors 00284 * 00285 * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0. 00286 */ 00287 inline int 00288 searchForNeighbors (const PointCloudIn &cloud, size_t index, double parameter, 00289 std::vector<int> &indices, std::vector<float> &distances) const 00290 { 00291 return (search_method_surface_ (cloud, index, parameter, indices, distances)); 00292 } 00293 00294 private: 00295 /** \brief Abstract feature estimation method. 00296 * \param[out] output the resultant features 00297 */ 00298 virtual void 00299 computeFeature (PointCloudOut &output) = 0; 00300 00301 public: 00302 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00303 }; 00304 00305 00306 //////////////////////////////////////////////////////////////////////////////////////////// 00307 //////////////////////////////////////////////////////////////////////////////////////////// 00308 //////////////////////////////////////////////////////////////////////////////////////////// 00309 template <typename PointInT, typename PointNT, typename PointOutT> 00310 class FeatureFromNormals : public Feature<PointInT, PointOutT> 00311 { 00312 typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn; 00313 typedef typename PointCloudIn::Ptr PointCloudInPtr; 00314 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; 00315 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00316 00317 public: 00318 typedef typename pcl::PointCloud<PointNT> PointCloudN; 00319 typedef typename PointCloudN::Ptr PointCloudNPtr; 00320 typedef typename PointCloudN::ConstPtr PointCloudNConstPtr; 00321 00322 typedef boost::shared_ptr< FeatureFromNormals<PointInT, PointNT, PointOutT> > Ptr; 00323 typedef boost::shared_ptr< const FeatureFromNormals<PointInT, PointNT, PointOutT> > ConstPtr; 00324 00325 // Members derived from the base class 00326 using Feature<PointInT, PointOutT>::input_; 00327 using Feature<PointInT, PointOutT>::surface_; 00328 using Feature<PointInT, PointOutT>::getClassName; 00329 00330 /** \brief Empty constructor. */ 00331 FeatureFromNormals () : normals_ () {} 00332 00333 /** \brief Empty destructor */ 00334 virtual ~FeatureFromNormals () {} 00335 00336 /** \brief Provide a pointer to the input dataset that contains the point normals of 00337 * the XYZ dataset. 00338 * In case of search surface is set to be different from the input cloud, 00339 * normals should correspond to the search surface, not the input cloud! 00340 * \param[in] normals the const boost shared pointer to a PointCloud of normals. 00341 * By convention, L2 norm of each normal should be 1. 00342 */ 00343 inline void 00344 setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; } 00345 00346 /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */ 00347 inline PointCloudNConstPtr 00348 getInputNormals () const { return (normals_); } 00349 00350 protected: 00351 /** \brief A pointer to the input dataset that contains the point normals of the XYZ 00352 * dataset. 00353 */ 00354 PointCloudNConstPtr normals_; 00355 00356 /** \brief This method should get called before starting the actual computation. */ 00357 virtual bool 00358 initCompute (); 00359 00360 public: 00361 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00362 }; 00363 00364 //////////////////////////////////////////////////////////////////////////////////////////// 00365 //////////////////////////////////////////////////////////////////////////////////////////// 00366 //////////////////////////////////////////////////////////////////////////////////////////// 00367 template <typename PointInT, typename PointLT, typename PointOutT> 00368 class FeatureFromLabels : public Feature<PointInT, PointOutT> 00369 { 00370 typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn; 00371 typedef typename PointCloudIn::Ptr PointCloudInPtr; 00372 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; 00373 00374 typedef typename pcl::PointCloud<PointLT> PointCloudL; 00375 typedef typename PointCloudL::Ptr PointCloudNPtr; 00376 typedef typename PointCloudL::ConstPtr PointCloudLConstPtr; 00377 00378 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00379 00380 public: 00381 typedef boost::shared_ptr< FeatureFromLabels<PointInT, PointLT, PointOutT> > Ptr; 00382 typedef boost::shared_ptr< const FeatureFromLabels<PointInT, PointLT, PointOutT> > ConstPtr; 00383 00384 // Members derived from the base class 00385 using Feature<PointInT, PointOutT>::input_; 00386 using Feature<PointInT, PointOutT>::surface_; 00387 using Feature<PointInT, PointOutT>::getClassName; 00388 using Feature<PointInT, PointOutT>::k_; 00389 00390 /** \brief Empty constructor. */ 00391 FeatureFromLabels () : labels_ () 00392 { 00393 k_ = 1; // Search tree is not always used here. 00394 } 00395 00396 /** \brief Empty destructor */ 00397 virtual ~FeatureFromLabels () {} 00398 00399 /** \brief Provide a pointer to the input dataset that contains the point labels of 00400 * the XYZ dataset. 00401 * In case of search surface is set to be different from the input cloud, 00402 * labels should correspond to the search surface, not the input cloud! 00403 * \param[in] labels the const boost shared pointer to a PointCloud of labels. 00404 */ 00405 inline void 00406 setInputLabels (const PointCloudLConstPtr &labels) 00407 { 00408 labels_ = labels; 00409 } 00410 00411 /** \brief Get a pointer to the labels of the input XYZ point cloud dataset. */ 00412 inline PointCloudLConstPtr 00413 getInputLabels () const 00414 { 00415 return (labels_); 00416 } 00417 00418 protected: 00419 /** \brief A pointer to the input dataset that contains the point labels of the XYZ 00420 * dataset. 00421 */ 00422 PointCloudLConstPtr labels_; 00423 00424 /** \brief This method should get called before starting the actual computation. */ 00425 virtual bool 00426 initCompute (); 00427 00428 public: 00429 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00430 }; 00431 00432 //////////////////////////////////////////////////////////////////////////////////////////// 00433 //////////////////////////////////////////////////////////////////////////////////////////// 00434 //////////////////////////////////////////////////////////////////////////////////////////// 00435 /** \brief FeatureWithLocalReferenceFrames provides a public interface for descriptor 00436 * extractor classes which need a local reference frame at each input keypoint. 00437 * 00438 * \attention 00439 * This interface is for backward compatibility with existing code and in the future it could be 00440 * merged with pcl::Feature. Subclasses should call the protected method initLocalReferenceFrames () 00441 * to correctly initialize the frames_ member. 00442 * 00443 * \author Nicola Fioraio 00444 * \ingroup features 00445 */ 00446 template <typename PointInT, typename PointRFT> 00447 class FeatureWithLocalReferenceFrames 00448 { 00449 public: 00450 typedef pcl::PointCloud<PointRFT> PointCloudLRF; 00451 typedef typename PointCloudLRF::Ptr PointCloudLRFPtr; 00452 typedef typename PointCloudLRF::ConstPtr PointCloudLRFConstPtr; 00453 00454 /** \brief Empty constructor. */ 00455 FeatureWithLocalReferenceFrames () : frames_ (), frames_never_defined_ (true) {} 00456 00457 /** \brief Empty destructor. */ 00458 virtual ~FeatureWithLocalReferenceFrames () {} 00459 00460 /** \brief Provide a pointer to the input dataset that contains the local 00461 * reference frames of the XYZ dataset. 00462 * In case of search surface is set to be different from the input cloud, 00463 * local reference frames should correspond to the input cloud, not the search surface! 00464 * \param[in] frames the const boost shared pointer to a PointCloud of reference frames. 00465 */ 00466 inline void 00467 setInputReferenceFrames (const PointCloudLRFConstPtr &frames) 00468 { 00469 frames_ = frames; 00470 frames_never_defined_ = false; 00471 } 00472 00473 /** \brief Get a pointer to the local reference frames. */ 00474 inline PointCloudLRFConstPtr 00475 getInputReferenceFrames () const 00476 { 00477 return (frames_); 00478 } 00479 00480 protected: 00481 /** \brief A boost shared pointer to the local reference frames. */ 00482 PointCloudLRFConstPtr frames_; 00483 /** \brief The user has never set the frames. */ 00484 bool frames_never_defined_; 00485 00486 /** \brief Check if frames_ has been correctly initialized and compute it if needed. 00487 * \param input the subclass' input cloud dataset. 00488 * \param lrf_estimation a pointer to a local reference frame estimation class to be used as default. 00489 * \return true if frames_ has been correctly initialized. 00490 */ 00491 typedef typename Feature<PointInT, PointRFT>::Ptr LRFEstimationPtr; 00492 virtual bool 00493 initLocalReferenceFrames (const size_t& indices_size, 00494 const LRFEstimationPtr& lrf_estimation = LRFEstimationPtr()); 00495 }; 00496 } 00497 00498 #include <pcl/features/impl/feature.hpp> 00499 00500 #endif //#ifndef PCL_FEATURE_H_