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 * 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_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_H_ 00042 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_H_ 00043 00044 #include <pcl/registration/registration.h> 00045 #include <pcl/registration/transformation_estimation_svd.h> 00046 #include <pcl/registration/transformation_validation.h> 00047 #include <pcl/registration/correspondence_rejection_poly.h> 00048 00049 namespace pcl 00050 { 00051 /** \brief Pose estimation and alignment class using a prerejective RANSAC routine. 00052 * 00053 * This class inserts a simple, yet effective "prerejection" step into the standard 00054 * RANSAC pose estimation loop in order to avoid verification of pose hypotheses 00055 * that are likely to be wrong. This is achieved by local pose-invariant geometric 00056 * constraints, as also implemented in the class 00057 * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly". 00058 * 00059 * In order to robustly align partial/occluded models, this routine does not 00060 * try to minimize the fit error, but instead tries to maximize the inlier rate, 00061 * above a threshold specifiable using \ref setInlierFraction(). 00062 * 00063 * The amount of prerejection or "greedyness" of the algorithm can be specified 00064 * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled, 00065 * and 1 is maximally rejective. 00066 * 00067 * If you use this in academic work, please cite: 00068 * 00069 * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger. 00070 * Pose Estimation using Local Structure-Specific Shape and Appearance Context. 00071 * International Conference on Robotics and Automation (ICRA), 2013. 00072 * 00073 * \author Anders Glent Buch (andersgb1@gmail.com) 00074 * \ingroup registration 00075 */ 00076 template <typename PointSource, typename PointTarget, typename FeatureT> 00077 class SampleConsensusPrerejective : public Registration<PointSource, PointTarget> 00078 { 00079 public: 00080 typedef typename Registration<PointSource, PointTarget>::Matrix4 Matrix4; 00081 00082 using Registration<PointSource, PointTarget>::reg_name_; 00083 using Registration<PointSource, PointTarget>::getClassName; 00084 using Registration<PointSource, PointTarget>::input_; 00085 using Registration<PointSource, PointTarget>::target_; 00086 using Registration<PointSource, PointTarget>::tree_; 00087 using Registration<PointSource, PointTarget>::max_iterations_; 00088 using Registration<PointSource, PointTarget>::corr_dist_threshold_; 00089 using Registration<PointSource, PointTarget>::transformation_; 00090 using Registration<PointSource, PointTarget>::final_transformation_; 00091 using Registration<PointSource, PointTarget>::transformation_estimation_; 00092 using Registration<PointSource, PointTarget>::getFitnessScore; 00093 using Registration<PointSource, PointTarget>::converged_; 00094 00095 typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource; 00096 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00097 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00098 00099 typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget; 00100 00101 typedef PointIndices::Ptr PointIndicesPtr; 00102 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00103 00104 typedef pcl::PointCloud<FeatureT> FeatureCloud; 00105 typedef typename FeatureCloud::Ptr FeatureCloudPtr; 00106 typedef typename FeatureCloud::ConstPtr FeatureCloudConstPtr; 00107 00108 typedef boost::shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > Ptr; 00109 typedef boost::shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > ConstPtr; 00110 00111 typedef typename KdTreeFLANN<FeatureT>::Ptr FeatureKdTreePtr; 00112 00113 typedef pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget> CorrespondenceRejectorPoly; 00114 typedef typename CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr; 00115 typedef typename CorrespondenceRejectorPoly::ConstPtr CorrespondenceRejectorPolyConstPtr; 00116 00117 /** \brief Constructor */ 00118 SampleConsensusPrerejective () 00119 : input_features_ () 00120 , target_features_ () 00121 , nr_samples_(3) 00122 , k_correspondences_ (2) 00123 , feature_tree_ (new pcl::KdTreeFLANN<FeatureT>) 00124 , correspondence_rejector_poly_ (new CorrespondenceRejectorPoly) 00125 , inlier_fraction_ (0.0f) 00126 { 00127 reg_name_ = "SampleConsensusPrerejective"; 00128 correspondence_rejector_poly_->setSimilarityThreshold (0.6f); 00129 max_iterations_ = 5000; 00130 transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>); 00131 }; 00132 00133 /** \brief Destructor */ 00134 virtual ~SampleConsensusPrerejective () 00135 { 00136 } 00137 00138 /** \brief Provide a boost shared pointer to the source point cloud's feature descriptors 00139 * \param features the source point cloud's features 00140 */ 00141 void 00142 setSourceFeatures (const FeatureCloudConstPtr &features); 00143 00144 /** \brief Get a pointer to the source point cloud's features */ 00145 inline const FeatureCloudConstPtr 00146 getSourceFeatures () const 00147 { 00148 return (input_features_); 00149 } 00150 00151 /** \brief Provide a boost shared pointer to the target point cloud's feature descriptors 00152 * \param features the target point cloud's features 00153 */ 00154 void 00155 setTargetFeatures (const FeatureCloudConstPtr &features); 00156 00157 /** \brief Get a pointer to the target point cloud's features */ 00158 inline const FeatureCloudConstPtr 00159 getTargetFeatures () const 00160 { 00161 return (target_features_); 00162 } 00163 00164 /** \brief Set the number of samples to use during each iteration 00165 * \param nr_samples the number of samples to use during each iteration 00166 */ 00167 inline void 00168 setNumberOfSamples (int nr_samples) 00169 { 00170 nr_samples_ = nr_samples; 00171 } 00172 00173 /** \brief Get the number of samples to use during each iteration, as set by the user */ 00174 inline int 00175 getNumberOfSamples () const 00176 { 00177 return (nr_samples_); 00178 } 00179 00180 /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will 00181 * add more randomness to the feature matching. 00182 * \param k the number of neighbors to use when selecting a random feature correspondence. 00183 */ 00184 inline void 00185 setCorrespondenceRandomness (int k) 00186 { 00187 k_correspondences_ = k; 00188 } 00189 00190 /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */ 00191 inline int 00192 getCorrespondenceRandomness () const 00193 { 00194 return (k_correspondences_); 00195 } 00196 00197 /** \brief Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object, 00198 * where 1 is a perfect match 00199 * \param similarity_threshold edge length similarity threshold 00200 */ 00201 inline void 00202 setSimilarityThreshold (float similarity_threshold) 00203 { 00204 correspondence_rejector_poly_->setSimilarityThreshold (similarity_threshold); 00205 } 00206 00207 /** \brief Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object, 00208 * \return edge length similarity threshold 00209 */ 00210 inline float 00211 getSimilarityThreshold () const 00212 { 00213 return correspondence_rejector_poly_->getSimilarityThreshold (); 00214 } 00215 00216 /** \brief Set the required inlier fraction (of the input) 00217 * \param inlier_fraction required inlier fraction, must be in [0,1] 00218 */ 00219 inline void 00220 setInlierFraction (float inlier_fraction) 00221 { 00222 inlier_fraction_ = inlier_fraction; 00223 } 00224 00225 /** \brief Get the required inlier fraction 00226 * \return required inlier fraction in [0,1] 00227 */ 00228 inline float 00229 getInlierFraction () const 00230 { 00231 return inlier_fraction_; 00232 } 00233 00234 /** \brief Get the inlier indices of the source point cloud under the final transformation 00235 * @return inlier indices 00236 */ 00237 inline const std::vector<int>& 00238 getInliers () const 00239 { 00240 return inliers_; 00241 } 00242 00243 protected: 00244 /** \brief Choose a random index between 0 and n-1 00245 * \param n the number of possible indices to choose from 00246 */ 00247 inline int 00248 getRandomIndex (int n) const 00249 { 00250 return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0)))); 00251 }; 00252 00253 /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are 00254 * greater than a user-defined minimum distance, \a min_sample_distance. 00255 * \param cloud the input point cloud 00256 * \param nr_samples the number of samples to select 00257 * \param sample_indices the resulting sample indices 00258 */ 00259 void 00260 selectSamples (const PointCloudSource &cloud, int nr_samples, 00261 std::vector<int> &sample_indices); 00262 00263 /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to 00264 * the sample points' features. From these, select one randomly which will be considered that sample point's 00265 * correspondence. 00266 * \param input_features a cloud of feature descriptors 00267 * \param sample_indices the indices of each sample point 00268 * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud 00269 */ 00270 void 00271 findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices, 00272 std::vector<int> &corresponding_indices); 00273 00274 /** \brief Rigid transformation computation method. 00275 * \param output the transformed input point cloud dataset using the rigid transformation found 00276 */ 00277 void 00278 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess); 00279 00280 /** \brief Obtain the fitness of a transformation 00281 * The following metrics are calculated, based on 00282 * \b final_transformation_ and \b corr_dist_threshold_: 00283 * - Inliers: the number of transformed points which are closer than threshold to NN 00284 * - Error score: the MSE of the inliers 00285 * \param inliers indices of source point cloud inliers 00286 * \param fitness_score output fitness score as RMSE 00287 */ 00288 void 00289 getFitness (std::vector<int>& inliers, float& fitness_score); 00290 00291 /** \brief The source point cloud's feature descriptors. */ 00292 FeatureCloudConstPtr input_features_; 00293 00294 /** \brief The target point cloud's feature descriptors. */ 00295 FeatureCloudConstPtr target_features_; 00296 00297 /** \brief The number of samples to use during each iteration. */ 00298 int nr_samples_; 00299 00300 /** \brief The number of neighbors to use when selecting a random feature correspondence. */ 00301 int k_correspondences_; 00302 00303 /** \brief The KdTree used to compare feature descriptors. */ 00304 FeatureKdTreePtr feature_tree_; 00305 00306 /** \brief The polygonal correspondence rejector used for prerejection */ 00307 CorrespondenceRejectorPolyPtr correspondence_rejector_poly_; 00308 00309 /** \brief The fraction [0,1] of inlier points required for accepting a transformation */ 00310 float inlier_fraction_; 00311 00312 /** \brief Inlier points of final transformation as indices into source */ 00313 std::vector<int> inliers_; 00314 }; 00315 } 00316 00317 #include <pcl/registration/impl/sample_consensus_prerejective.hpp> 00318 00319 #endif