Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/features/include/pcl/features/fpfh.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_FPFH_H_
00042 #define PCL_FPFH_H_
00043 
00044 #include <pcl/features/feature.h>
00045 #include <set>
00046 
00047 namespace pcl
00048 {
00049   /** \brief FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point 
00050     * cloud dataset containing points and normals.
00051     *
00052     * A commonly used type for PointOutT is pcl::FPFHSignature33.
00053     *
00054     * \note If you use this code in any academic work, please cite:
00055     *
00056     *   - R.B. Rusu, N. Blodow, M. Beetz.
00057     *     Fast Point Feature Histograms (FPFH) for 3D Registration.
00058     *     In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
00059     *     Kobe, Japan, May 12-17 2009.
00060     *   - R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
00061     *     Fast Geometric Point Labeling using Conditional Random Fields.
00062     *     In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
00063     *     St. Louis, MO, USA, October 11-15 2009.
00064     *
00065     * \attention 
00066     * The convention for FPFH features is:
00067     *   - if a query point's nearest neighbors cannot be estimated, the FPFH feature will be set to NaN 
00068     *     (not a number)
00069     *   - it is impossible to estimate a FPFH descriptor for a point that
00070     *     doesn't have finite 3D coordinates. Therefore, any point that contains
00071     *     NaN data on x, y, or z, will have its FPFH feature property set to NaN.
00072     *
00073     * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
00074     * \ref FPFHEstimationOMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
00075     *
00076     * \author Radu B. Rusu
00077     * \ingroup features
00078     */
00079   template <typename PointInT, typename PointNT, typename PointOutT = pcl::FPFHSignature33>
00080   class FPFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
00081   {
00082     public:
00083       typedef boost::shared_ptr<FPFHEstimation<PointInT, PointNT, PointOutT> > Ptr;
00084       typedef boost::shared_ptr<const FPFHEstimation<PointInT, PointNT, PointOutT> > ConstPtr;
00085       using Feature<PointInT, PointOutT>::feature_name_;
00086       using Feature<PointInT, PointOutT>::getClassName;
00087       using Feature<PointInT, PointOutT>::indices_;
00088       using Feature<PointInT, PointOutT>::k_;
00089       using Feature<PointInT, PointOutT>::search_parameter_;
00090       using Feature<PointInT, PointOutT>::input_;
00091       using Feature<PointInT, PointOutT>::surface_;
00092       using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00093 
00094       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00095 
00096       /** \brief Empty constructor. */
00097       FPFHEstimation () : 
00098         nr_bins_f1_ (11), nr_bins_f2_ (11), nr_bins_f3_ (11), 
00099         hist_f1_ (), hist_f2_ (), hist_f3_ (), fpfh_histogram_ (),
00100         d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI)))
00101       {
00102         feature_name_ = "FPFHEstimation";
00103       };
00104 
00105       /** \brief Compute the 4-tuple representation containing the three angles and one distance between two points
00106         * represented by Cartesian coordinates and normals.
00107         * \note For explanations about the features, please see the literature mentioned above (the order of the
00108         * features might be different).
00109         * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points
00110         * \param[in] normals the dataset containing the surface normals (assuming normalized vectors) at each point in cloud
00111         * \param[in] p_idx the index of the first point (source)
00112         * \param[in] q_idx the index of the second point (target)
00113         * \param[out] f1 the first angular feature (angle between the projection of nq_idx and u)
00114         * \param[out] f2 the second angular feature (angle between nq_idx and v)
00115         * \param[out] f3 the third angular feature (angle between np_idx and |p_idx - q_idx|)
00116         * \param[out] f4 the distance feature (p_idx - q_idx)
00117         */
00118       bool 
00119       computePairFeatures (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals, 
00120                            int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4);
00121 
00122       /** \brief Estimate the SPFH (Simple Point Feature Histograms) individual signatures of the three angular
00123         * (f1, f2, f3) features for a given point based on its spatial neighborhood of 3D points with normals
00124         * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points
00125         * \param[in] normals the dataset containing the surface normals at each point in \a cloud
00126         * \param[in] p_idx the index of the query point (source)
00127         * \param[in] row the index row in feature histogramms
00128         * \param[in] indices the k-neighborhood point indices in the dataset
00129         * \param[out] hist_f1 the resultant SPFH histogram for feature f1
00130         * \param[out] hist_f2 the resultant SPFH histogram for feature f2
00131         * \param[out] hist_f3 the resultant SPFH histogram for feature f3
00132         */
00133       void 
00134       computePointSPFHSignature (const pcl::PointCloud<PointInT> &cloud, 
00135                                  const pcl::PointCloud<PointNT> &normals, int p_idx, int row, 
00136                                  const std::vector<int> &indices, 
00137                                  Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3);
00138 
00139       /** \brief Weight the SPFH (Simple Point Feature Histograms) individual histograms to create the final FPFH
00140         * (Fast Point Feature Histogram) for a given point based on its 3D spatial neighborhood
00141         * \param[in] hist_f1 the histogram feature vector of \a f1 values over the given patch
00142         * \param[in] hist_f2 the histogram feature vector of \a f2 values over the given patch
00143         * \param[in] hist_f3 the histogram feature vector of \a f3 values over the given patch
00144         * \param[in] indices the point indices of p_idx's k-neighborhood in the point cloud
00145         * \param[in] dists the distances from p_idx to all its k-neighbors
00146         * \param[out] fpfh_histogram the resultant FPFH histogram representing the feature at the query point
00147         */
00148       void 
00149       weightPointSPFHSignature (const Eigen::MatrixXf &hist_f1, 
00150                                 const Eigen::MatrixXf &hist_f2, 
00151                                 const Eigen::MatrixXf &hist_f3, 
00152                                 const std::vector<int> &indices, 
00153                                 const std::vector<float> &dists, 
00154                                 Eigen::VectorXf &fpfh_histogram);
00155 
00156       /** \brief Set the number of subdivisions for each angular feature interval.
00157         * \param[in] nr_bins_f1 number of subdivisions for the first angular feature
00158         * \param[in] nr_bins_f2 number of subdivisions for the second angular feature
00159         * \param[in] nr_bins_f3 number of subdivisions for the third angular feature
00160         */
00161       inline void
00162       setNrSubdivisions (int nr_bins_f1, int nr_bins_f2, int nr_bins_f3)
00163       {
00164         nr_bins_f1_ = nr_bins_f1;
00165         nr_bins_f2_ = nr_bins_f2;
00166         nr_bins_f3_ = nr_bins_f3;
00167       }
00168 
00169       /** \brief Get the number of subdivisions for each angular feature interval. 
00170         * \param[out] nr_bins_f1 number of subdivisions for the first angular feature
00171         * \param[out] nr_bins_f2 number of subdivisions for the second angular feature
00172         * \param[out] nr_bins_f3 number of subdivisions for the third angular feature
00173          */
00174       inline void
00175       getNrSubdivisions (int &nr_bins_f1, int &nr_bins_f2, int &nr_bins_f3)
00176       {
00177         nr_bins_f1 = nr_bins_f1_;
00178         nr_bins_f2 = nr_bins_f2_;
00179         nr_bins_f3 = nr_bins_f3_;
00180       }
00181 
00182     protected:
00183 
00184       /** \brief Estimate the set of all SPFH (Simple Point Feature Histograms) signatures for the input cloud
00185         * \param[out] spfh_hist_lookup a lookup table for all the SPF feature indices
00186         * \param[out] hist_f1 the resultant SPFH histogram for feature f1
00187         * \param[out] hist_f2 the resultant SPFH histogram for feature f2
00188         * \param[out] hist_f3 the resultant SPFH histogram for feature f3
00189         */
00190       void 
00191       computeSPFHSignatures (std::vector<int> &spf_hist_lookup, 
00192                              Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3);
00193 
00194       /** \brief Estimate the Fast Point Feature Histograms (FPFH) descriptors at a set of points given by
00195         * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
00196         * setSearchMethod ()
00197         * \param[out] output the resultant point cloud model dataset that contains the FPFH feature estimates
00198         */
00199       void 
00200       computeFeature (PointCloudOut &output);
00201 
00202       /** \brief The number of subdivisions for each angular feature interval. */
00203       int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_;
00204 
00205       /** \brief Placeholder for the f1 histogram. */
00206       Eigen::MatrixXf hist_f1_;
00207 
00208       /** \brief Placeholder for the f2 histogram. */
00209       Eigen::MatrixXf hist_f2_;
00210 
00211       /** \brief Placeholder for the f3 histogram. */
00212       Eigen::MatrixXf hist_f3_;
00213 
00214       /** \brief Placeholder for a point's FPFH signature. */
00215       Eigen::VectorXf fpfh_histogram_;
00216 
00217       /** \brief Float constant = 1.0 / (2.0 * M_PI) */
00218       float d_pi_; 
00219   };
00220 }
00221 
00222 #ifdef PCL_NO_PRECOMPILE
00223 #include <pcl/features/impl/fpfh.hpp>
00224 #endif
00225 
00226 #endif  //#ifndef PCL_FPFH_H_