Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/features/include/pcl/features/pfh.h
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_PFH_H_
00042 #define PCL_PFH_H_
00043 
00044 #include <pcl/point_types.h>
00045 #include <pcl/features/feature.h>
00046 #include <pcl/features/pfh_tools.h>
00047 #include <map>
00048 
00049 namespace pcl
00050 {
00051   /** \brief PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
00052     * containing points and normals.
00053     *
00054     * A commonly used type for PointOutT is pcl::PFHSignature125.
00055     *
00056     * \note If you use this code in any academic work, please cite:
00057     *
00058     *   - R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
00059     *     Aligning Point Cloud Views using Persistent Feature Histograms.
00060     *     In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
00061     *     Nice, France, September 22-26 2008.
00062     *   - R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
00063     *     Learning Informative Point Classes for the Acquisition of Object Model Maps.
00064     *     In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV),
00065     *     Hanoi, Vietnam, December 17-20 2008.
00066     *
00067     * \attention 
00068     * The convention for PFH features is:
00069     *   - if a query point's nearest neighbors cannot be estimated, the PFH feature will be set to NaN 
00070     *     (not a number)
00071     *   - it is impossible to estimate a PFH descriptor for a point that
00072     *     doesn't have finite 3D coordinates. Therefore, any point that contains
00073     *     NaN data on x, y, or z, will have its PFH feature property set to NaN.
00074     *
00075     * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
00076     * \ref FPFHEstimationOMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
00077     *
00078     * \author Radu B. Rusu
00079     * \ingroup features
00080     */
00081   template <typename PointInT, typename PointNT, typename PointOutT = pcl::PFHSignature125>
00082   class PFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
00083   {
00084     public:
00085       typedef boost::shared_ptr<PFHEstimation<PointInT, PointNT, PointOutT> > Ptr;
00086       typedef boost::shared_ptr<const PFHEstimation<PointInT, PointNT, PointOutT> > ConstPtr;
00087       using Feature<PointInT, PointOutT>::feature_name_;
00088       using Feature<PointInT, PointOutT>::getClassName;
00089       using Feature<PointInT, PointOutT>::indices_;
00090       using Feature<PointInT, PointOutT>::k_;
00091       using Feature<PointInT, PointOutT>::search_parameter_;
00092       using Feature<PointInT, PointOutT>::surface_;
00093       using Feature<PointInT, PointOutT>::input_;
00094       using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00095 
00096       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00097       typedef typename Feature<PointInT, PointOutT>::PointCloudIn  PointCloudIn;
00098 
00099       /** \brief Empty constructor. 
00100         * Sets \a use_cache_ to false, \a nr_subdiv_ to 5, and the internal maximum cache size to 1GB.
00101         */
00102       PFHEstimation () : 
00103         nr_subdiv_ (5), 
00104         pfh_histogram_ (),
00105         pfh_tuple_ (),
00106         d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI))), 
00107         feature_map_ (),
00108         key_list_ (),
00109         // Default 1GB memory size. Need to set it to something more conservative.
00110         max_cache_size_ ((1ul*1024ul*1024ul*1024ul) / sizeof (std::pair<std::pair<int, int>, Eigen::Vector4f>)),
00111         use_cache_ (false)
00112       {
00113         feature_name_ = "PFHEstimation";
00114       };
00115 
00116       /** \brief Set the maximum internal cache size. Defaults to 2GB worth of entries.
00117         * \param[in] cache_size maximum cache size 
00118         */
00119       inline void
00120       setMaximumCacheSize (unsigned int cache_size)
00121       {
00122         max_cache_size_ = cache_size;
00123       }
00124 
00125       /** \brief Get the maximum internal cache size. */
00126       inline unsigned int 
00127       getMaximumCacheSize ()
00128       {
00129         return (max_cache_size_);
00130       }
00131 
00132       /** \brief Set whether to use an internal cache mechanism for removing redundant calculations or not. 
00133         *
00134         * \note Depending on how the point cloud is ordered and how the nearest
00135         * neighbors are estimated, using a cache could have a positive or a
00136         * negative influence. Please test with and without a cache on your
00137         * data, and choose whatever works best!
00138         *
00139         * See \ref setMaximumCacheSize for setting the maximum cache size
00140         *
00141         * \param[in] use_cache set to true to use the internal cache, false otherwise
00142         */
00143       inline void
00144       setUseInternalCache (bool use_cache)
00145       {
00146         use_cache_ = use_cache;
00147       }
00148 
00149       /** \brief Get whether the internal cache is used or not for computing the PFH features. */
00150       inline bool
00151       getUseInternalCache ()
00152       {
00153         return (use_cache_);
00154       }
00155 
00156       /** \brief Compute the 4-tuple representation containing the three angles and one distance between two points
00157         * represented by Cartesian coordinates and normals.
00158         * \note For explanations about the features, please see the literature mentioned above (the order of the
00159         * features might be different).
00160         * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points
00161         * \param[in] normals the dataset containing the surface normals (assuming normalized vectors) at each point in cloud
00162         * \param[in] p_idx the index of the first point (source)
00163         * \param[in] q_idx the index of the second point (target)
00164         * \param[out] f1 the first angular feature (angle between the projection of nq_idx and u)
00165         * \param[out] f2 the second angular feature (angle between nq_idx and v)
00166         * \param[out] f3 the third angular feature (angle between np_idx and |p_idx - q_idx|)
00167         * \param[out] f4 the distance feature (p_idx - q_idx)
00168         * \note For efficiency reasons, we assume that the point data passed to the method is finite.
00169         */
00170       bool 
00171       computePairFeatures (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals, 
00172                            int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4);
00173 
00174       /** \brief Estimate the PFH (Point Feature Histograms) individual signatures of the three angular (f1, f2, f3)
00175         * features for a given point based on its spatial neighborhood of 3D points with normals
00176         * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points
00177         * \param[in] normals the dataset containing the surface normals at each point in \a cloud
00178         * \param[in] indices the k-neighborhood point indices in the dataset
00179         * \param[in] nr_split the number of subdivisions for each angular feature interval
00180         * \param[out] pfh_histogram the resultant (combinatorial) PFH histogram representing the feature at the query point
00181         */
00182       void 
00183       computePointPFHSignature (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals, 
00184                                 const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram);
00185 
00186     protected:
00187       /** \brief Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by
00188         * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
00189         * setSearchMethod ()
00190         * \param[out] output the resultant point cloud model dataset that contains the PFH feature estimates
00191         */
00192       void 
00193       computeFeature (PointCloudOut &output);
00194 
00195       /** \brief The number of subdivisions for each angular feature interval. */
00196       int nr_subdiv_;
00197 
00198       /** \brief Placeholder for a point's PFH signature. */
00199       Eigen::VectorXf pfh_histogram_;
00200 
00201       /** \brief Placeholder for a PFH 4-tuple. */
00202       Eigen::Vector4f pfh_tuple_;
00203 
00204       /** \brief Placeholder for a histogram index. */
00205       int f_index_[3];
00206 
00207       /** \brief Float constant = 1.0 / (2.0 * M_PI) */
00208       float d_pi_; 
00209 
00210       /** \brief Internal hashmap, used to optimize efficiency of redundant computations. */
00211       std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> > feature_map_;
00212 
00213       /** \brief Queue of pairs saved, used to constrain memory usage. */
00214       std::queue<std::pair<int, int> > key_list_;
00215 
00216       /** \brief Maximum size of internal cache memory. */
00217       unsigned int max_cache_size_;
00218 
00219       /** \brief Set to true to use the internal cache for removing redundant computations. */
00220       bool use_cache_;
00221   };
00222 }
00223 
00224 #ifdef PCL_NO_PRECOMPILE
00225 #include <pcl/features/impl/pfh.hpp>
00226 #endif
00227 
00228 #endif  //#ifndef PCL_PFH_H_
00229