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-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_