Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Alexandru-Eugen Ichim 00005 * Willow Garage, Inc 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * $Id: ppf_registration.h 3042 2011-11-01 04:44:47Z svn $ 00036 */ 00037 00038 00039 #ifndef PCL_PPF_REGISTRATION_H_ 00040 #define PCL_PPF_REGISTRATION_H_ 00041 00042 #include "pcl/registration/registration.h" 00043 #include <pcl/features/ppf.h> 00044 #include <boost/unordered_map.hpp> 00045 00046 namespace pcl 00047 { 00048 class PCL_EXPORTS PPFHashMapSearch 00049 { 00050 public: 00056 struct HashKeyStruct : public std::pair <int, std::pair <int, std::pair <int, int> > > 00057 { 00058 HashKeyStruct(int a, int b, int c, int d) 00059 { 00060 this->first = a; 00061 this->second.first = b; 00062 this->second.second.first = c; 00063 this->second.second.second = d; 00064 } 00065 }; 00066 typedef boost::unordered_multimap<HashKeyStruct, std::pair<size_t, size_t> > FeatureHashMapType; 00067 typedef boost::shared_ptr<FeatureHashMapType> FeatureHashMapTypePtr; 00068 typedef boost::shared_ptr<PPFHashMapSearch> Ptr; 00069 00070 00075 PPFHashMapSearch (float angle_discretization_step = 12.0 / 180 * M_PI, 00076 float distance_discretization_step = 0.01) 00077 : angle_discretization_step_ (angle_discretization_step), 00078 distance_discretization_step_ (distance_discretization_step) 00079 { 00080 feature_hash_map_ = FeatureHashMapTypePtr (new FeatureHashMapType); 00081 internals_initialized_ = false; 00082 max_dist_ = -1.0; 00083 } 00084 00088 void 00089 setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud); 00090 00099 void 00100 nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4, 00101 std::vector<std::pair<size_t, size_t> > &indices); 00102 00104 Ptr 00105 makeShared() { return Ptr (new PPFHashMapSearch (*this)); } 00106 00108 inline float 00109 getAngleDiscretizationStep () { return angle_discretization_step_; } 00110 00112 inline float 00113 getDistanceDiscretizationStep () { return distance_discretization_step_; } 00114 00116 inline float 00117 getModelDiameter () { return max_dist_; } 00118 00119 std::vector <std::vector <float> > alpha_m_; 00120 private: 00121 FeatureHashMapTypePtr feature_hash_map_; 00122 bool internals_initialized_; 00123 00124 float angle_discretization_step_, distance_discretization_step_; 00125 float max_dist_; 00126 }; 00127 00139 template <typename PointSource, typename PointTarget> 00140 class PPFRegistration : public Registration<PointSource, PointTarget> 00141 { 00142 public: 00147 struct PoseWithVotes 00148 { 00149 PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes) 00150 : pose (a_pose), 00151 votes (a_votes) 00152 {} 00153 00154 Eigen::Affine3f pose; 00155 unsigned int votes; 00156 }; 00157 typedef std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> > PoseWithVotesList; 00158 00160 using Registration<PointSource, PointTarget>::input_; 00162 using Registration<PointSource, PointTarget>::target_; 00163 using Registration<PointSource, PointTarget>::converged_; 00164 using Registration<PointSource, PointTarget>::final_transformation_; 00165 using Registration<PointSource, PointTarget>::transformation_; 00166 00167 typedef pcl::PointCloud<PointSource> PointCloudSource; 00168 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00169 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00170 00171 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00172 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00173 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00174 00175 00177 PPFRegistration () 00178 : Registration<PointSource, PointTarget> (), 00179 search_method_ (), 00180 scene_reference_point_sampling_rate_ (5), 00181 clustering_position_diff_threshold_ (0.01), 00182 clustering_rotation_diff_threshold_ (20.0 / 180 * M_PI) 00183 {} 00184 00189 inline void 00190 setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; } 00191 00196 inline float 00197 getPositionClusteringThreshold () { return clustering_position_diff_threshold_; } 00198 00203 inline void 00204 setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; } 00205 00208 inline float 00209 getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; } 00210 00214 inline void 00215 setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; } 00216 00218 inline unsigned int 00219 getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; } 00220 00226 inline void 00227 setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; } 00228 00230 inline PPFHashMapSearch::Ptr 00231 getSearchMethod () { return search_method_; } 00232 00236 void 00237 setInputTarget (const PointCloudTargetConstPtr &cloud); 00238 00239 00240 private: 00242 void 00243 computeTransformation (PointCloudSource &output); 00244 00245 00247 PPFHashMapSearch::Ptr search_method_; 00248 00250 unsigned int scene_reference_point_sampling_rate_; 00251 00254 float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_; 00255 00257 typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_; 00258 00261 static bool 00262 poseWithVotesCompareFunction (const PoseWithVotes &a, 00263 const PoseWithVotes &b); 00264 00267 static bool 00268 clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a, 00269 const std::pair<size_t, unsigned int> &b); 00270 00273 void 00274 clusterPoses (PoseWithVotesList &poses, 00275 PoseWithVotesList &result); 00276 00279 bool 00280 posesWithinErrorBounds (Eigen::Affine3f &pose1, 00281 Eigen::Affine3f &pose2); 00282 }; 00283 } 00284 00285 #include "pcl/registration/impl/ppf_registration.hpp" 00286 00287 #endif // PCL_PPF_REGISTRATION_H_