1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2011, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #pragma once 42 43 #if defined __GNUC__ 44 # pragma GCC system_header 45 #endif 46 47 // PCL includes 48 #include <pcl/memory.h> 49 #include <pcl/pcl_base.h> 50 #include <pcl/pcl_macros.h> 51 #include <pcl/search/search.h> 52 53 #include <functional> 54 55 namespace pcl 56 { 57 /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares 58 * plane normal and surface curvature. 59 * \param covariance_matrix the 3x3 covariance matrix 60 * \param point a point lying on the least-squares plane (SSE aligned) 61 * \param plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0) 62 * \param curvature the estimated surface curvature as a measure of 63 * \f[ 64 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 65 * \f] 66 * \ingroup features 67 */ 68 inline void 69 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 70 const Eigen::Vector4f &point, 71 Eigen::Vector4f &plane_parameters, float &curvature); 72 73 /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares 74 * plane normal and surface curvature. 75 * \param covariance_matrix the 3x3 covariance matrix 76 * \param nx the resultant X component of the plane normal 77 * \param ny the resultant Y component of the plane normal 78 * \param nz the resultant Z component of the plane normal 79 * \param curvature the estimated surface curvature as a measure of 80 * \f[ 81 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 82 * \f] 83 * \ingroup features 84 */ 85 inline void 86 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 87 float &nx, float &ny, float &nz, float &curvature); 88 89 //////////////////////////////////////////////////////////////////////////////////////////// 90 //////////////////////////////////////////////////////////////////////////////////////////// 91 //////////////////////////////////////////////////////////////////////////////////////////// 92 /** \brief Feature represents the base feature class. Some generic 3D operations that 93 * are applicable to all features are defined here as static methods. 94 * 95 * \attention 96 * The convention for a feature descriptor is: 97 * - if the nearest neighbors for the query point at which the descriptor is to be computed cannot be 98 * determined, the descriptor values will be set to NaN (not a number) 99 * - it is impossible to estimate a feature descriptor for a point that doesn't have finite 3D coordinates. 100 * Therefore, any point that has NaN data on x, y, or z, will most likely have its descriptor set to NaN. 101 * 102 * \author Radu B. Rusu 103 * \ingroup features 104 */ 105 template <typename PointInT, typename PointOutT> 106 class Feature : public PCLBase<PointInT> 107 { 108 public: 109 using PCLBase<PointInT>::indices_; 110 using PCLBase<PointInT>::input_; 111 112 using BaseClass = PCLBase<PointInT>; 113 114 using Ptr = shared_ptr< Feature<PointInT, PointOutT> >; 115 using ConstPtr = shared_ptr< const Feature<PointInT, PointOutT> >; 116 117 using KdTree = pcl::search::Search<PointInT>; 118 using KdTreePtr = typename KdTree::Ptr; 119 120 using PointCloudIn = pcl::PointCloud<PointInT>; 121 using PointCloudInPtr = typename PointCloudIn::Ptr; 122 using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; 123 124 using PointCloudOut = pcl::PointCloud<PointOutT>; 125 126 using SearchMethod = std::function<int (std::size_t, double, pcl::Indices &, std::vector<float> &)>; 127 using SearchMethodSurface = std::function<int (const PointCloudIn &cloud, std::size_t index, double, pcl::Indices &, std::vector<float> &)>; 128 129 public: 130 /** \brief Empty constructor. */ Feature()131 Feature () : 132 feature_name_ (), search_method_surface_ (), 133 surface_(), tree_(), 134 search_parameter_(0), search_radius_(0), k_(0), 135 fake_surface_(false) 136 {} 137 138 /** \brief Empty destructor */ ~Feature()139 virtual ~Feature () {} 140 141 /** \brief Provide a pointer to a dataset to add additional information 142 * to estimate the features for every point in the input dataset. This 143 * is optional, if this is not set, it will only use the data in the 144 * input cloud to estimate the features. This is useful when you only 145 * need to compute the features for a downsampled cloud. 146 * \param[in] cloud a pointer to a PointCloud message 147 */ 148 inline void setSearchSurface(const PointCloudInConstPtr & cloud)149 setSearchSurface (const PointCloudInConstPtr &cloud) 150 { 151 surface_ = cloud; 152 fake_surface_ = false; 153 //use_surface_ = true; 154 } 155 156 /** \brief Get a pointer to the surface point cloud dataset. */ 157 inline PointCloudInConstPtr getSearchSurface()158 getSearchSurface () const 159 { 160 return (surface_); 161 } 162 163 /** \brief Provide a pointer to the search object. 164 * \param[in] tree a pointer to the spatial search object. 165 */ 166 inline void setSearchMethod(const KdTreePtr & tree)167 setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } 168 169 /** \brief Get a pointer to the search method used. */ 170 inline KdTreePtr getSearchMethod()171 getSearchMethod () const 172 { 173 return (tree_); 174 } 175 176 /** \brief Get the internal search parameter. */ 177 inline double getSearchParameter()178 getSearchParameter () const 179 { 180 return (search_parameter_); 181 } 182 183 /** \brief Set the number of k nearest neighbors to use for the feature estimation. 184 * \param[in] k the number of k-nearest neighbors 185 */ 186 inline void setKSearch(int k)187 setKSearch (int k) { k_ = k; } 188 189 /** \brief get the number of k nearest neighbors used for the feature estimation. */ 190 inline int getKSearch()191 getKSearch () const 192 { 193 return (k_); 194 } 195 196 /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature 197 * estimation. 198 * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor 199 */ 200 inline void setRadiusSearch(double radius)201 setRadiusSearch (double radius) 202 { 203 search_radius_ = radius; 204 } 205 206 /** \brief Get the sphere radius used for determining the neighbors. */ 207 inline double getRadiusSearch()208 getRadiusSearch () const 209 { 210 return (search_radius_); 211 } 212 213 /** \brief Base method for feature estimation for all points given in 214 * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () 215 * and the spatial locator in setSearchMethod () 216 * \param[out] output the resultant point cloud model dataset containing the estimated features 217 */ 218 void 219 compute (PointCloudOut &output); 220 221 protected: 222 /** \brief The feature name. */ 223 std::string feature_name_; 224 225 /** \brief The search method template for points. */ 226 SearchMethodSurface search_method_surface_; 227 228 /** \brief An input point cloud describing the surface that is to be used 229 * for nearest neighbors estimation. 230 */ 231 PointCloudInConstPtr surface_; 232 233 /** \brief A pointer to the spatial search object. */ 234 KdTreePtr tree_; 235 236 /** \brief The actual search parameter (from either \a search_radius_ or \a k_). */ 237 double search_parameter_; 238 239 /** \brief The nearest neighbors search radius for each point. */ 240 double search_radius_; 241 242 /** \brief The number of K nearest neighbors to use for each point. */ 243 int k_; 244 245 /** \brief Get a string representation of the name of this class. */ 246 inline const std::string& getClassName()247 getClassName () const { return (feature_name_); } 248 249 /** \brief This method should get called before starting the actual computation. */ 250 virtual bool 251 initCompute (); 252 253 /** \brief This method should get called after ending the actual computation. */ 254 virtual bool 255 deinitCompute (); 256 257 /** \brief If no surface is given, we use the input PointCloud as the surface. */ 258 bool fake_surface_; 259 260 /** \brief Search for k-nearest neighbors using the spatial locator from 261 * \a setSearchmethod, and the given surface from \a setSearchSurface. 262 * \param[in] index the index of the query point 263 * \param[in] parameter the search parameter (either k or radius) 264 * \param[out] indices the resultant vector of indices representing the k-nearest neighbors 265 * \param[out] distances the resultant vector of distances representing the distances from the query point to the 266 * k-nearest neighbors 267 * 268 * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0. 269 */ 270 inline int searchForNeighbors(std::size_t index,double parameter,pcl::Indices & indices,std::vector<float> & distances)271 searchForNeighbors (std::size_t index, double parameter, 272 pcl::Indices &indices, std::vector<float> &distances) const 273 { 274 return (search_method_surface_ (*input_, index, parameter, indices, distances)); 275 } 276 277 /** \brief Search for k-nearest neighbors using the spatial locator from 278 * \a setSearchmethod, and the given surface from \a setSearchSurface. 279 * \param[in] cloud the query point cloud 280 * \param[in] index the index of the query point in \a cloud 281 * \param[in] parameter the search parameter (either k or radius) 282 * \param[out] indices the resultant vector of indices representing the k-nearest neighbors 283 * \param[out] distances the resultant vector of distances representing the distances from the query point to the 284 * k-nearest neighbors 285 * 286 * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0. 287 */ 288 inline int searchForNeighbors(const PointCloudIn & cloud,std::size_t index,double parameter,pcl::Indices & indices,std::vector<float> & distances)289 searchForNeighbors (const PointCloudIn &cloud, std::size_t index, double parameter, 290 pcl::Indices &indices, std::vector<float> &distances) const 291 { 292 return (search_method_surface_ (cloud, index, parameter, indices, distances)); 293 } 294 295 private: 296 /** \brief Abstract feature estimation method. 297 * \param[out] output the resultant features 298 */ 299 virtual void 300 computeFeature (PointCloudOut &output) = 0; 301 302 public: 303 PCL_MAKE_ALIGNED_OPERATOR_NEW 304 }; 305 306 307 //////////////////////////////////////////////////////////////////////////////////////////// 308 //////////////////////////////////////////////////////////////////////////////////////////// 309 //////////////////////////////////////////////////////////////////////////////////////////// 310 template <typename PointInT, typename PointNT, typename PointOutT> 311 class FeatureFromNormals : public Feature<PointInT, PointOutT> 312 { 313 using PointCloudIn = typename Feature<PointInT, PointOutT>::PointCloudIn; 314 using PointCloudInPtr = typename PointCloudIn::Ptr; 315 using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; 316 using PointCloudOut = typename Feature<PointInT, PointOutT>::PointCloudOut; 317 318 public: 319 using PointCloudN = pcl::PointCloud<PointNT>; 320 using PointCloudNPtr = typename PointCloudN::Ptr; 321 using PointCloudNConstPtr = typename PointCloudN::ConstPtr; 322 323 using Ptr = shared_ptr< FeatureFromNormals<PointInT, PointNT, PointOutT> >; 324 using ConstPtr = shared_ptr< const FeatureFromNormals<PointInT, PointNT, PointOutT> >; 325 326 // Members derived from the base class 327 using Feature<PointInT, PointOutT>::input_; 328 using Feature<PointInT, PointOutT>::surface_; 329 using Feature<PointInT, PointOutT>::getClassName; 330 331 /** \brief Empty constructor. */ FeatureFromNormals()332 FeatureFromNormals () : normals_ () {} 333 334 /** \brief Empty destructor */ ~FeatureFromNormals()335 virtual ~FeatureFromNormals () {} 336 337 /** \brief Provide a pointer to the input dataset that contains the point normals of 338 * the XYZ dataset. 339 * In case of search surface is set to be different from the input cloud, 340 * normals should correspond to the search surface, not the input cloud! 341 * \param[in] normals the const boost shared pointer to a PointCloud of normals. 342 * By convention, L2 norm of each normal should be 1. 343 */ 344 inline void setInputNormals(const PointCloudNConstPtr & normals)345 setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; } 346 347 /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */ 348 inline PointCloudNConstPtr getInputNormals()349 getInputNormals () const { return (normals_); } 350 351 protected: 352 /** \brief A pointer to the input dataset that contains the point normals of the XYZ 353 * dataset. 354 */ 355 PointCloudNConstPtr normals_; 356 357 /** \brief This method should get called before starting the actual computation. */ 358 virtual bool 359 initCompute (); 360 361 public: 362 PCL_MAKE_ALIGNED_OPERATOR_NEW 363 }; 364 365 //////////////////////////////////////////////////////////////////////////////////////////// 366 //////////////////////////////////////////////////////////////////////////////////////////// 367 //////////////////////////////////////////////////////////////////////////////////////////// 368 template <typename PointInT, typename PointLT, typename PointOutT> 369 class FeatureFromLabels : public Feature<PointInT, PointOutT> 370 { 371 using PointCloudIn = typename Feature<PointInT, PointOutT>::PointCloudIn; 372 using PointCloudInPtr = typename PointCloudIn::Ptr; 373 using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; 374 375 using PointCloudL = pcl::PointCloud<PointLT>; 376 using PointCloudNPtr = typename PointCloudL::Ptr; 377 using PointCloudLConstPtr = typename PointCloudL::ConstPtr; 378 379 using PointCloudOut = typename Feature<PointInT, PointOutT>::PointCloudOut; 380 381 public: 382 using Ptr = shared_ptr< FeatureFromLabels<PointInT, PointLT, PointOutT> >; 383 using ConstPtr = shared_ptr< const FeatureFromLabels<PointInT, PointLT, PointOutT> >; 384 385 // Members derived from the base class 386 using Feature<PointInT, PointOutT>::input_; 387 using Feature<PointInT, PointOutT>::surface_; 388 using Feature<PointInT, PointOutT>::getClassName; 389 using Feature<PointInT, PointOutT>::k_; 390 391 /** \brief Empty constructor. */ FeatureFromLabels()392 FeatureFromLabels () : labels_ () 393 { 394 k_ = 1; // Search tree is not always used here. 395 } 396 397 /** \brief Empty destructor */ ~FeatureFromLabels()398 virtual ~FeatureFromLabels () {} 399 400 /** \brief Provide a pointer to the input dataset that contains the point labels of 401 * the XYZ dataset. 402 * In case of search surface is set to be different from the input cloud, 403 * labels should correspond to the search surface, not the input cloud! 404 * \param[in] labels the const boost shared pointer to a PointCloud of labels. 405 */ 406 inline void setInputLabels(const PointCloudLConstPtr & labels)407 setInputLabels (const PointCloudLConstPtr &labels) 408 { 409 labels_ = labels; 410 } 411 412 /** \brief Get a pointer to the labels of the input XYZ point cloud dataset. */ 413 inline PointCloudLConstPtr getInputLabels()414 getInputLabels () const 415 { 416 return (labels_); 417 } 418 419 protected: 420 /** \brief A pointer to the input dataset that contains the point labels of the XYZ 421 * dataset. 422 */ 423 PointCloudLConstPtr labels_; 424 425 /** \brief This method should get called before starting the actual computation. */ 426 virtual bool 427 initCompute (); 428 429 public: 430 PCL_MAKE_ALIGNED_OPERATOR_NEW 431 }; 432 433 //////////////////////////////////////////////////////////////////////////////////////////// 434 //////////////////////////////////////////////////////////////////////////////////////////// 435 //////////////////////////////////////////////////////////////////////////////////////////// 436 /** \brief FeatureWithLocalReferenceFrames provides a public interface for descriptor 437 * extractor classes which need a local reference frame at each input keypoint. 438 * 439 * \attention 440 * This interface is for backward compatibility with existing code and in the future it could be 441 * merged with pcl::Feature. Subclasses should call the protected method initLocalReferenceFrames () 442 * to correctly initialize the frames_ member. 443 * 444 * \author Nicola Fioraio 445 * \ingroup features 446 */ 447 template <typename PointInT, typename PointRFT> 448 class FeatureWithLocalReferenceFrames 449 { 450 public: 451 using PointCloudLRF = pcl::PointCloud<PointRFT>; 452 using PointCloudLRFPtr = typename PointCloudLRF::Ptr; 453 using PointCloudLRFConstPtr = typename PointCloudLRF::ConstPtr; 454 455 /** \brief Empty constructor. */ FeatureWithLocalReferenceFrames()456 FeatureWithLocalReferenceFrames () : frames_ (), frames_never_defined_ (true) {} 457 458 /** \brief Empty destructor. */ ~FeatureWithLocalReferenceFrames()459 virtual ~FeatureWithLocalReferenceFrames () {} 460 461 /** \brief Provide a pointer to the input dataset that contains the local 462 * reference frames of the XYZ dataset. 463 * In case of search surface is set to be different from the input cloud, 464 * local reference frames should correspond to the input cloud, not the search surface! 465 * \param[in] frames the const boost shared pointer to a PointCloud of reference frames. 466 */ 467 inline void setInputReferenceFrames(const PointCloudLRFConstPtr & frames)468 setInputReferenceFrames (const PointCloudLRFConstPtr &frames) 469 { 470 frames_ = frames; 471 frames_never_defined_ = false; 472 } 473 474 /** \brief Get a pointer to the local reference frames. */ 475 inline PointCloudLRFConstPtr getInputReferenceFrames()476 getInputReferenceFrames () const 477 { 478 return (frames_); 479 } 480 481 protected: 482 /** \brief A boost shared pointer to the local reference frames. */ 483 PointCloudLRFConstPtr frames_; 484 /** \brief The user has never set the frames. */ 485 bool frames_never_defined_; 486 487 /** \brief Check if frames_ has been correctly initialized and compute it if needed. 488 * \param input the subclass' input cloud dataset. 489 * \param lrf_estimation a pointer to a local reference frame estimation class to be used as default. 490 * \return true if frames_ has been correctly initialized. 491 */ 492 using LRFEstimationPtr = typename Feature<PointInT, PointRFT>::Ptr; 493 virtual bool 494 initLocalReferenceFrames (const std::size_t& indices_size, 495 const LRFEstimationPtr& lrf_estimation = LRFEstimationPtr()); 496 }; 497 } 498 499 #include <pcl/features/impl/feature.hpp> 500