Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Alexandru-Eugen Ichim 00005 * 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 00042 #ifndef PCL_PPF_REGISTRATION_H_ 00043 #define PCL_PPF_REGISTRATION_H_ 00044 00045 #include <pcl/registration/boost.h> 00046 #include <pcl/registration/registration.h> 00047 #include <pcl/features/ppf.h> 00048 00049 namespace pcl 00050 { 00051 class PCL_EXPORTS PPFHashMapSearch 00052 { 00053 public: 00054 /** \brief Data structure to hold the information for the key in the feature hash map of the 00055 * PPFHashMapSearch class 00056 * \note It uses multiple pair levels in order to enable the usage of the boost::hash function 00057 * which has the std::pair implementation (i.e., does not require a custom hash function) 00058 */ 00059 struct HashKeyStruct : public std::pair <int, std::pair <int, std::pair <int, int> > > 00060 { 00061 HashKeyStruct(int a, int b, int c, int d) 00062 { 00063 this->first = a; 00064 this->second.first = b; 00065 this->second.second.first = c; 00066 this->second.second.second = d; 00067 } 00068 }; 00069 typedef boost::unordered_multimap<HashKeyStruct, std::pair<size_t, size_t> > FeatureHashMapType; 00070 typedef boost::shared_ptr<FeatureHashMapType> FeatureHashMapTypePtr; 00071 typedef boost::shared_ptr<PPFHashMapSearch> Ptr; 00072 00073 00074 /** \brief Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data structure 00075 * \param angle_discretization_step the step value between each bin of the hash map for the angular values 00076 * \param distance_discretization_step the step value between each bin of the hash map for the distance values 00077 */ 00078 PPFHashMapSearch (float angle_discretization_step = 12.0f / 180.0f * static_cast<float> (M_PI), 00079 float distance_discretization_step = 0.01f) 00080 : alpha_m_ () 00081 , feature_hash_map_ (new FeatureHashMapType) 00082 , internals_initialized_ (false) 00083 , angle_discretization_step_ (angle_discretization_step) 00084 , distance_discretization_step_ (distance_discretization_step) 00085 , max_dist_ (-1.0f) 00086 { 00087 } 00088 00089 /** \brief Method that sets the feature cloud to be inserted in the hash map 00090 * \param feature_cloud a const smart pointer to the PPFSignature feature cloud 00091 */ 00092 void 00093 setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud); 00094 00095 /** \brief Function for finding the nearest neighbors for the given feature inside the discretized hash map 00096 * \param f1 The 1st value describing the query PPFSignature feature 00097 * \param f2 The 2nd value describing the query PPFSignature feature 00098 * \param f3 The 3rd value describing the query PPFSignature feature 00099 * \param f4 The 4th value describing the query PPFSignature feature 00100 * \param indices a vector of pair indices representing the feature pairs that have been found in the bin 00101 * corresponding to the query feature 00102 */ 00103 void 00104 nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4, 00105 std::vector<std::pair<size_t, size_t> > &indices); 00106 00107 /** \brief Convenience method for returning a copy of the class instance as a boost::shared_ptr */ 00108 Ptr 00109 makeShared() { return Ptr (new PPFHashMapSearch (*this)); } 00110 00111 /** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */ 00112 inline float 00113 getAngleDiscretizationStep () { return angle_discretization_step_; } 00114 00115 /** \brief Returns the distance discretization step parameter (the step value between each bin of the hash map for the distance values) */ 00116 inline float 00117 getDistanceDiscretizationStep () { return distance_discretization_step_; } 00118 00119 /** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */ 00120 inline float 00121 getModelDiameter () { return max_dist_; } 00122 00123 std::vector <std::vector <float> > alpha_m_; 00124 private: 00125 FeatureHashMapTypePtr feature_hash_map_; 00126 bool internals_initialized_; 00127 00128 float angle_discretization_step_, distance_discretization_step_; 00129 float max_dist_; 00130 }; 00131 00132 /** \brief Class that registers two point clouds based on their sets of PPFSignatures. 00133 * Please refer to the following publication for more details: 00134 * B. Drost, M. Ulrich, N. Navab, S. Ilic 00135 * Model Globally, Match Locally: Efficient and Robust 3D Object Recognition 00136 * 2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 00137 * 13-18 June 2010, San Francisco, CA 00138 * 00139 * \note This class works in tandem with the PPFEstimation class 00140 * 00141 * \author Alexandru-Eugen Ichim 00142 */ 00143 template <typename PointSource, typename PointTarget> 00144 class PPFRegistration : public Registration<PointSource, PointTarget> 00145 { 00146 public: 00147 /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes 00148 * \note initially used std::pair<Eigen::Affine3f, unsigned int>, but it proved problematic 00149 * because of the Eigen structures alignment problems - std::pair does not have a custom allocator 00150 */ 00151 struct PoseWithVotes 00152 { 00153 PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes) 00154 : pose (a_pose), 00155 votes (a_votes) 00156 {} 00157 00158 Eigen::Affine3f pose; 00159 unsigned int votes; 00160 }; 00161 typedef std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> > PoseWithVotesList; 00162 00163 /// input_ is the model cloud 00164 using Registration<PointSource, PointTarget>::input_; 00165 /// target_ is the scene cloud 00166 using Registration<PointSource, PointTarget>::target_; 00167 using Registration<PointSource, PointTarget>::converged_; 00168 using Registration<PointSource, PointTarget>::final_transformation_; 00169 using Registration<PointSource, PointTarget>::transformation_; 00170 00171 typedef pcl::PointCloud<PointSource> PointCloudSource; 00172 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00173 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00174 00175 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00176 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00177 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00178 00179 00180 /** \brief Empty constructor that initializes all the parameters of the algorithm with default values */ 00181 PPFRegistration () 00182 : Registration<PointSource, PointTarget> (), 00183 search_method_ (), 00184 scene_reference_point_sampling_rate_ (5), 00185 clustering_position_diff_threshold_ (0.01f), 00186 clustering_rotation_diff_threshold_ (20.0f / 180.0f * static_cast<float> (M_PI)) 00187 {} 00188 00189 /** \brief Method for setting the position difference clustering parameter 00190 * \param clustering_position_diff_threshold distance threshold below which two poses are 00191 * considered close enough to be in the same cluster (for the clustering phase of the algorithm) 00192 */ 00193 inline void 00194 setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; } 00195 00196 /** \brief Returns the parameter defining the position difference clustering parameter - 00197 * distance threshold below which two poses are considered close enough to be in the same cluster 00198 * (for the clustering phase of the algorithm) 00199 */ 00200 inline float 00201 getPositionClusteringThreshold () { return clustering_position_diff_threshold_; } 00202 00203 /** \brief Method for setting the rotation clustering parameter 00204 * \param clustering_rotation_diff_threshold rotation difference threshold below which two 00205 * poses are considered to be in the same cluster (for the clustering phase of the algorithm) 00206 */ 00207 inline void 00208 setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; } 00209 00210 /** \brief Returns the parameter defining the rotation clustering threshold 00211 */ 00212 inline float 00213 getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; } 00214 00215 /** \brief Method for setting the scene reference point sampling rate 00216 * \param scene_reference_point_sampling_rate sampling rate for the scene reference point 00217 */ 00218 inline void 00219 setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; } 00220 00221 /** \brief Returns the parameter for the scene reference point sampling rate of the algorithm */ 00222 inline unsigned int 00223 getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; } 00224 00225 /** \brief Function that sets the search method for the algorithm 00226 * \note Right now, the only available method is the one initially proposed by 00227 * the authors - by using a hash map with discretized feature vectors 00228 * \param search_method smart pointer to the search method to be set 00229 */ 00230 inline void 00231 setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; } 00232 00233 /** \brief Getter function for the search method of the class */ 00234 inline PPFHashMapSearch::Ptr 00235 getSearchMethod () { return search_method_; } 00236 00237 /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) 00238 * \param cloud the input point cloud target 00239 */ 00240 void 00241 setInputTarget (const PointCloudTargetConstPtr &cloud); 00242 00243 00244 private: 00245 /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */ 00246 void 00247 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess); 00248 00249 00250 /** \brief the search method that is going to be used to find matching feature pairs */ 00251 PPFHashMapSearch::Ptr search_method_; 00252 00253 /** \brief parameter for the sampling rate of the scene reference points */ 00254 unsigned int scene_reference_point_sampling_rate_; 00255 00256 /** \brief position and rotation difference thresholds below which two 00257 * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */ 00258 float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_; 00259 00260 /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */ 00261 typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_; 00262 00263 /** \brief static method used for the std::sort function to order two PoseWithVotes 00264 * instances by their number of votes*/ 00265 static bool 00266 poseWithVotesCompareFunction (const PoseWithVotes &a, 00267 const PoseWithVotes &b); 00268 00269 /** \brief static method used for the std::sort function to order two pairs <index, votes> 00270 * by the number of votes (unsigned integer value) */ 00271 static bool 00272 clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a, 00273 const std::pair<size_t, unsigned int> &b); 00274 00275 /** \brief Method that clusters a set of given poses by using the clustering thresholds 00276 * and their corresponding number of votes (see publication for more details) */ 00277 void 00278 clusterPoses (PoseWithVotesList &poses, 00279 PoseWithVotesList &result); 00280 00281 /** \brief Method that checks whether two poses are close together - based on the clustering threshold parameters 00282 * of the class */ 00283 bool 00284 posesWithinErrorBounds (Eigen::Affine3f &pose1, 00285 Eigen::Affine3f &pose2); 00286 }; 00287 } 00288 00289 #include <pcl/registration/impl/ppf_registration.hpp> 00290 00291 #endif // PCL_PPF_REGISTRATION_H_