Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/recognition/include/pcl/recognition/cg/hough_3d.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *  
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id:$
00037  *
00038  */
00039 
00040 #ifndef PCL_RECOGNITION_HOUGH_3D_H_
00041 #define PCL_RECOGNITION_HOUGH_3D_H_
00042 
00043 #include <pcl/recognition/cg/correspondence_grouping.h>
00044 #include <pcl/recognition/boost.h>
00045 
00046 namespace pcl
00047 {
00048   namespace recognition
00049   {
00050     /** \brief HoughSpace3D is a 3D voting space. Cast votes can be interpolated in order to better deal with approximations introduced by bin quantization. A weight can also be associated with each vote. 
00051       * \author Federico Tombari (original), Tommaso Cavallari (PCL port)
00052       * \ingroup recognition
00053       */
00054     class PCL_EXPORTS HoughSpace3D
00055     {
00056 
00057       public:
00058       
00059         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00060       
00061         /** \brief Constructor
00062           *
00063           * \param[in] min_coord minimum (x,y,z) coordinates of the Hough space 
00064           * \param[in] bin_size  size of each bing of the Hough space.
00065           * \param[in] max_coord maximum (x,y,z) coordinates of the Hough space.
00066           */
00067         HoughSpace3D (const Eigen::Vector3d &min_coord, const Eigen::Vector3d &bin_size, const Eigen::Vector3d &max_coord);
00068 
00069         /** \brief Reset all cast votes. */
00070         void
00071         reset ();
00072 
00073         /** \brief Casting a vote for a given position in the Hough space.
00074           * 
00075           * \param[in] single_vote_coord coordinates of the vote being cast (in absolute coordinates)
00076           * \param[in] weight weight associated with the vote.
00077           * \param[in] voter_id the numeric id of the voter. Useful to trace back the voting correspondence, if the vote is returned by findMaxima as part of a maximum of the Hough Space.
00078           * \return the index of the bin in which the vote has been cast.
00079           */
00080         int
00081         vote (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id);
00082 
00083         /** \brief Vote for a given position in the 3D space. The weight is interpolated between the bin pointed by single_vote_coord and its neighbors.
00084           * 
00085           * \param[in] single_vote_coord coordinates of the vote being cast.
00086           * \param[in] weight weight associated with the vote.
00087           * \param[in] voter_id the numeric id of the voter. Useful to trace back the voting correspondence, if the vote is returned by findMaxima as a part of a maximum of the Hough Space.
00088           * \return the index of the bin in which the vote has been cast.
00089           */
00090         int
00091         voteInt (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id);
00092 
00093         /** \brief Find the bins with most votes.
00094           * 
00095           * \param[in] min_threshold the minimum number of votes to be included in a bin in order to have its value returned. 
00096           * If set to a value between -1 and 0 the Hough space maximum_vote is found and the returned values are all the votes greater than -min_threshold * maximum_vote.
00097           * \param[out] maxima_values the list of Hough Space bin values greater than min_threshold.
00098           * \param[out] maxima_voter_ids for each value returned, a list of the voter ids who cast a vote in that position. 
00099           * \return The min_threshold used, either set by the user or found by this method.
00100           */
00101         double
00102         findMaxima (double min_threshold, std::vector<double> & maxima_values, std::vector<std::vector<int> > &maxima_voter_ids);
00103 
00104       protected:
00105 
00106         /** \brief Minimum coordinate in the Hough Space. */
00107         Eigen::Vector3d min_coord_;
00108 
00109         /** \brief Size of each bin in the Hough Space. */
00110         Eigen::Vector3d bin_size_;
00111 
00112         /** \brief Number of bins for each dimension. */
00113         Eigen::Vector3i bin_count_;
00114 
00115         /** \brief Used to access hough_space_ as if it was a matrix. */
00116         int partial_bin_products_[4];
00117 
00118         /** \brief Total number of bins in the Hough Space. */
00119         int total_bins_count_;
00120 
00121         /** \brief The Hough Space. */
00122         std::vector<double> hough_space_;
00123         //boost::unordered_map<int, double> hough_space_;
00124 
00125         /** \brief List of voters for each bin. */
00126         boost::unordered_map<int, std::vector<int> > voter_ids_;
00127     };
00128   }
00129 
00130   /** \brief Class implementing a 3D correspondence grouping algorithm that can deal with multiple instances of a model template
00131     * found into a given scene. Each correspondence casts a vote for a reference point in a 3D Hough Space.
00132   * The remaining 3 DOF are taken into account by associating each correspondence with a local Reference Frame. 
00133     * The suggested PointModelRfT is pcl::ReferenceFrame
00134     * 
00135     * \note If you use this code in any academic work, please cite the original paper:
00136     *   - F. Tombari, L. Di Stefano:
00137     *     Object recognition in 3D scenes with occlusions and clutter by Hough voting.
00138     *     2010, Fourth Pacific-Rim Symposium on Image and Video Technology
00139     *
00140     * \author Federico Tombari (original), Tommaso Cavallari (PCL port)
00141     * \ingroup recognition
00142     */
00143   template<typename PointModelT, typename PointSceneT, typename PointModelRfT = pcl::ReferenceFrame, typename PointSceneRfT = pcl::ReferenceFrame>
00144   class Hough3DGrouping : public CorrespondenceGrouping<PointModelT, PointSceneT>
00145   {
00146     public:
00147       typedef pcl::PointCloud<PointModelRfT> ModelRfCloud;
00148       typedef typename ModelRfCloud::Ptr ModelRfCloudPtr;
00149       typedef typename ModelRfCloud::ConstPtr ModelRfCloudConstPtr;
00150 
00151       typedef pcl::PointCloud<PointSceneRfT> SceneRfCloud;
00152       typedef typename SceneRfCloud::Ptr SceneRfCloudPtr;
00153       typedef typename SceneRfCloud::ConstPtr SceneRfCloudConstPtr;
00154 
00155       typedef pcl::PointCloud<PointModelT> PointCloud;
00156       typedef typename PointCloud::Ptr PointCloudPtr;
00157       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00158 
00159       typedef typename pcl::CorrespondenceGrouping<PointModelT, PointSceneT>::SceneCloudConstPtr SceneCloudConstPtr;
00160 
00161       /** \brief Constructor */
00162       Hough3DGrouping () 
00163         : input_rf_ ()
00164         , scene_rf_ ()
00165         , needs_training_ (true)
00166         , model_votes_ ()
00167         , hough_threshold_ (-1)
00168         , hough_bin_size_ (1.0)
00169         , use_interpolation_ (true)
00170         , use_distance_weight_ (false)
00171         , local_rf_normals_search_radius_ (0.0f)
00172         , local_rf_search_radius_ (0.0f)
00173         , hough_space_ ()
00174         , found_transformations_ ()
00175         , hough_space_initialized_ (false)
00176       {}
00177 
00178       /** \brief Provide a pointer to the input dataset.
00179         * \param[in] cloud the const boost shared pointer to a PointCloud message.
00180         */
00181       inline void
00182       setInputCloud (const PointCloudConstPtr &cloud)
00183       {
00184         PCLBase<PointModelT>::setInputCloud (cloud);
00185         needs_training_ = true;
00186         hough_space_initialized_ = false;
00187         input_rf_.reset();
00188       }
00189 
00190       /** \brief Provide a pointer to the input dataset's reference frames. 
00191         * Each point in the reference frame cloud should be the reference frame of
00192         * the correspondent point in the input dataset.
00193         * 
00194         * \param[in] input_rf the pointer to the input cloud's reference frames.
00195         */
00196       inline void
00197       setInputRf (const ModelRfCloudConstPtr &input_rf)
00198       {
00199         input_rf_ = input_rf;
00200         needs_training_ = true;
00201         hough_space_initialized_ = false;
00202       }
00203 
00204       /** \brief Getter for the input dataset's reference frames. 
00205         * Each point in the reference frame cloud should be the reference frame of
00206         * the correspondent point in the input dataset.
00207         * 
00208         * \return the pointer to the input cloud's reference frames.
00209         */
00210       inline ModelRfCloudConstPtr
00211       getInputRf () const
00212       {
00213         return (input_rf_);
00214       }
00215       
00216       /** \brief Provide a pointer to the scene dataset (i.e. the cloud in which the algorithm has to search for instances of the input model)
00217         * 
00218         * \param[in] scene the const boost shared pointer to a PointCloud message.
00219         */
00220       inline void
00221       setSceneCloud (const SceneCloudConstPtr &scene)
00222       {
00223         scene_ = scene;
00224         hough_space_initialized_ = false;
00225         scene_rf_.reset();
00226       }
00227 
00228       /** \brief Provide a pointer to the scene dataset's reference frames. 
00229         * Each point in the reference frame cloud should be the reference frame of
00230         * the correspondent point in the scene dataset.
00231         * 
00232         * \param[in] scene_rf the pointer to the scene cloud's reference frames.
00233         */
00234       inline void
00235       setSceneRf (const SceneRfCloudConstPtr &scene_rf)
00236       {
00237         scene_rf_ = scene_rf;
00238         hough_space_initialized_ = false;
00239       }
00240 
00241       /** \brief Getter for the scene dataset's reference frames. 
00242         * Each point in the reference frame cloud should be the reference frame of
00243         * the correspondent point in the scene dataset.
00244         * 
00245         * \return the pointer to the scene cloud's reference frames.
00246         */
00247       inline SceneRfCloudConstPtr
00248       getSceneRf () const
00249       {
00250         return (scene_rf_);
00251       }
00252 
00253       /** \brief Provide a pointer to the precomputed correspondences between points in the input dataset and 
00254         * points in the scene dataset. The correspondences are going to be clustered into different model instances
00255         * by the algorithm.
00256         * 
00257         * \param[in] corrs the correspondences between the model and the scene.
00258         */
00259       inline void
00260       setModelSceneCorrespondences (const CorrespondencesConstPtr &corrs)
00261       {
00262         model_scene_corrs_ = corrs;
00263         hough_space_initialized_ = false;
00264       }
00265 
00266       /** \brief Sets the minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud.
00267         * 
00268         * \param[in] threshold the threshold for the Hough space voting, if set between -1 and 0 the maximum vote in the
00269         * entire space is automatically calculated and -threshold the maximum value is used as a threshold. This means
00270         * that a value between -1 and 0 should be used only if at least one instance of the model is always present in
00271         * the scene, or if this false positive can be filtered later.
00272         */
00273       inline void
00274       setHoughThreshold (double threshold)
00275       {
00276         hough_threshold_ = threshold;
00277       }
00278 
00279       /** \brief Gets the minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud.
00280         * 
00281         * \return the threshold for the Hough space voting.
00282         */
00283       inline double
00284       getHoughThreshold () const
00285       {
00286         return (hough_threshold_);
00287       }
00288 
00289       /** \brief Sets the size of each bin into the Hough space.
00290         * 
00291         * \param[in] bin_size the size of each Hough space's bin.
00292         */
00293       inline void
00294       setHoughBinSize (double bin_size)
00295       {
00296         hough_bin_size_ = bin_size;
00297         hough_space_initialized_ = false;
00298       }
00299 
00300       /** \brief Gets the size of each bin into the Hough space.
00301         * 
00302         * \return the size of each Hough space's bin.
00303         */
00304       inline double
00305       getHoughBinSize () const
00306       {
00307         return (hough_bin_size_);
00308       }
00309 
00310       /** \brief Sets whether the vote casting procedure interpolates
00311         * the score between neighboring bins of the Hough space or not.
00312         * 
00313         * \param[in] use_interpolation the algorithm should interpolate the vote score between neighboring bins.
00314         */
00315       inline void
00316       setUseInterpolation (bool use_interpolation)
00317       {
00318         use_interpolation_ = use_interpolation;
00319         hough_space_initialized_ = false;
00320       }
00321 
00322       /** \brief Gets whether the vote casting procedure interpolates
00323         * the score between neighboring bins of the Hough space or not.
00324         * 
00325         * \return if the algorithm should interpolate the vote score between neighboring bins.
00326         */
00327       inline bool
00328       getUseInterpolation () const
00329       {
00330         return (use_interpolation_);
00331       }
00332 
00333       /** \brief Sets whether the vote casting procedure uses the correspondence's distance as a score.
00334         * 
00335         * \param[in] use_distance_weight the algorithm should use the weighted distance when calculating the Hough voting score.
00336         */
00337       inline void
00338       setUseDistanceWeight (bool use_distance_weight)
00339       {
00340         use_distance_weight_ = use_distance_weight;
00341         hough_space_initialized_ = false;
00342       }
00343 
00344       /** \brief Gets whether the vote casting procedure uses the correspondence's distance as a score.
00345         * 
00346         * \return if the algorithm should use the weighted distance when calculating the Hough voting score.
00347         */
00348       inline bool
00349       getUseDistanceWeight () const
00350       {
00351         return (use_distance_weight_);
00352       } 
00353 
00354       /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
00355         * this algorithm makes the computation itself but needs a suitable search radius to compute the normals
00356         * in order to subsequently compute the RF (if not set a default 15 nearest neighbors search is performed).
00357         *
00358         * \param[in] local_rf_normals_search_radius the normals search radius for the local reference frame calculation.
00359         */
00360       inline void
00361       setLocalRfNormalsSearchRadius (float local_rf_normals_search_radius)
00362       {
00363         local_rf_normals_search_radius_ = local_rf_normals_search_radius;
00364         needs_training_ = true;
00365         hough_space_initialized_ = false;
00366       }
00367 
00368       /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
00369         * this algorithm makes the computation itself but needs a suitable search radius to compute the normals
00370         * in order to subsequently compute the RF (if not set a default 15 nearest neighbors search is performed).
00371         *
00372         * \return the normals search radius for the local reference frame calculation.
00373         */
00374       inline float
00375       getLocalRfNormalsSearchRadius () const
00376       {
00377         return (local_rf_normals_search_radius_);
00378       }
00379 
00380       /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
00381         * this algorithm makes the computation itself but needs a suitable search radius to do so.
00382         * \attention This parameter NEEDS to be set if the reference frames are not precomputed externally, 
00383         * otherwise the recognition results won't be correct.
00384         *
00385         * \param[in] local_rf_search_radius the search radius for the local reference frame calculation.
00386         */
00387       inline void
00388       setLocalRfSearchRadius (float local_rf_search_radius)
00389       {
00390         local_rf_search_radius_ = local_rf_search_radius;
00391         needs_training_ = true;
00392         hough_space_initialized_ = false;
00393       }
00394 
00395       /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
00396         * this algorithm makes the computation itself but needs a suitable search radius to do so.
00397         * \attention This parameter NEEDS to be set if the reference frames are not precomputed externally, 
00398         * otherwise the recognition results won't be correct.
00399         *
00400         * \return the search radius for the local reference frame calculation.
00401         */
00402       inline float
00403       getLocalRfSearchRadius () const
00404       {
00405         return (local_rf_search_radius_);
00406       }
00407 
00408       /** \brief Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform an off line training of the algorithm. This might be useful if one wants to perform once and for all a pre-computation of votes that only concern the models, increasing the on-line efficiency of the grouping algorithm. 
00409         * The algorithm is automatically trained on the first invocation of the recognize method or the cluster method if this training function has not been manually invoked.
00410         * 
00411         * \return true if the training had been successful or false if errors have occurred.
00412         */
00413       bool
00414       train ();
00415 
00416       /** \brief The main function, recognizes instances of the model into the scene set by the user.
00417         * 
00418         * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene.
00419         *
00420         * \return true if the recognition had been successful or false if errors have occurred.
00421         */
00422       bool
00423       recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations);
00424 
00425       /** \brief The main function, recognizes instances of the model into the scene set by the user.
00426         * 
00427         * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene.
00428         * \param[out] clustered_corrs a vector containing the correspondences for each instance of the model found within the input data (the same output of clusterCorrespondences).
00429         *
00430         * \return true if the recognition had been successful or false if errors have occurred.
00431         */
00432       bool
00433       recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs);
00434 
00435     protected:
00436       using CorrespondenceGrouping<PointModelT, PointSceneT>::input_;
00437       using CorrespondenceGrouping<PointModelT, PointSceneT>::scene_;
00438       using CorrespondenceGrouping<PointModelT, PointSceneT>::model_scene_corrs_;
00439 
00440       /** \brief The input Rf cloud. */
00441       ModelRfCloudConstPtr input_rf_;
00442 
00443       /** \brief The scene Rf cloud. */
00444       SceneRfCloudConstPtr scene_rf_;
00445 
00446       /** \brief If the training of the Hough space is needed; set on change of either the input cloud or the input_rf. */
00447       bool needs_training_;
00448 
00449       /** \brief The result of the training. The vector between each model point and the centroid of the model adjusted by its local reference frame.*/
00450       std::vector<Eigen::Vector3f> model_votes_;
00451 
00452       /** \brief The minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud. */
00453       double hough_threshold_;
00454 
00455       /** \brief The size of each bin of the hough space. */
00456       double hough_bin_size_;
00457 
00458       /** \brief Use the interpolation between neighboring Hough bins when casting votes. */
00459       bool use_interpolation_;
00460 
00461       /** \brief Use the weighted correspondence distance when casting votes. */
00462       bool use_distance_weight_;
00463 
00464       /** \brief Normals search radius for the potential Rf calculation. */
00465       float local_rf_normals_search_radius_;
00466 
00467       /** \brief Search radius for the potential Rf calculation. */
00468       float local_rf_search_radius_;
00469 
00470       /** \brief The Hough space. */
00471       boost::shared_ptr<pcl::recognition::HoughSpace3D> hough_space_;
00472 
00473       /** \brief Transformations found by clusterCorrespondences method. */
00474       std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > found_transformations_;
00475 
00476       /** \brief Whether the Hough space already contains the correct votes for the current input parameters and so the cluster and recognize calls don't need to recompute each value.
00477         * Reset on the change of any parameter except the hough_threshold.
00478         */
00479       bool hough_space_initialized_;
00480 
00481       /** \brief Cluster the input correspondences in order to distinguish between different instances of the model into the scene.
00482         * 
00483         * \param[out] model_instances a vector containing the clustered correspondences for each model found on the scene.
00484         * \return true if the clustering had been successful or false if errors have occurred.
00485         */ 
00486       void
00487       clusterCorrespondences (std::vector<Correspondences> &model_instances);
00488 
00489       ///** \brief Finds the transformation matrix between the input and the scene cloud for a set of correspondences using a RANSAC algorithm.
00490       //  * 
00491       //  * \param[in] the scene cloud in which the PointSceneT has been converted to PointModelT.
00492       //  * \param[in] corrs a set of correspondences.
00493       //  * \param[out] transform the transformation matrix between the input cloud and the scene cloud that aligns the found correspondences.
00494       //  * \return true if the recognition had been successful or false if errors have occurred.
00495       //  */
00496       //bool
00497       //getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform);
00498 
00499       /** \brief The Hough space voting procedure.
00500         *
00501         * \return true if the voting had been successful or false if errors have occurred.
00502         */
00503       bool
00504       houghVoting ();
00505 
00506       /** \brief Computes the reference frame for an input cloud.
00507         * 
00508         * \param[in] input the input cloud.
00509         * \param[out] rf the resulting reference frame.
00510         */
00511       template<typename PointType, typename PointRfType> void
00512       computeRf (const boost::shared_ptr<const pcl::PointCloud<PointType> > &input, pcl::PointCloud<PointRfType> &rf);
00513   };
00514 }
00515 
00516 #ifdef PCL_NO_PRECOMPILE
00517 #include <pcl/recognition/impl/cg/hough_3d.hpp>
00518 #endif
00519 
00520 #endif // PCL_RECOGNITION_HOUGH_3D_H_