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-2011, 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 */ 00038 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_ 00039 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_ 00040 00041 #include <pcl/registration/correspondence_rejection.h> 00042 #include <pcl/point_cloud.h> 00043 00044 namespace pcl 00045 { 00046 namespace registration 00047 { 00048 /** \brief CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and 00049 * pose-invariant geometric constraints between two point sets by forming virtual polygons of a user-specifiable 00050 * cardinality on each model using the input correspondences. 00051 * These polygons are then checked in a pose-invariant manner (i.e. the side lengths must be approximately equal), 00052 * and rejection is performed by thresholding these edge lengths. 00053 * 00054 * If you use this in academic work, please cite: 00055 * 00056 * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger. 00057 * Pose Estimation using Local Structure-Specific Shape and Appearance Context. 00058 * International Conference on Robotics and Automation (ICRA), 2013. 00059 * 00060 * \author Anders Glent Buch 00061 * \ingroup registration 00062 */ 00063 template <typename SourceT, typename TargetT> 00064 class PCL_EXPORTS CorrespondenceRejectorPoly: public CorrespondenceRejector 00065 { 00066 using CorrespondenceRejector::input_correspondences_; 00067 using CorrespondenceRejector::rejection_name_; 00068 using CorrespondenceRejector::getClassName; 00069 00070 public: 00071 typedef boost::shared_ptr<CorrespondenceRejectorPoly> Ptr; 00072 typedef boost::shared_ptr<const CorrespondenceRejectorPoly> ConstPtr; 00073 00074 typedef pcl::PointCloud<SourceT> PointCloudSource; 00075 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00076 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00077 00078 typedef pcl::PointCloud<TargetT> PointCloudTarget; 00079 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00080 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00081 00082 /** \brief Empty constructor */ 00083 CorrespondenceRejectorPoly () 00084 : iterations_ (10000) 00085 , cardinality_ (3) 00086 , similarity_threshold_ (0.75f) 00087 , similarity_threshold_squared_ (0.75f * 0.75f) 00088 { 00089 rejection_name_ = "CorrespondenceRejectorPoly"; 00090 } 00091 00092 /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. 00093 * \param[in] original_correspondences the set of initial correspondences given 00094 * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences 00095 */ 00096 void 00097 getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 00098 pcl::Correspondences& remaining_correspondences); 00099 00100 /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. 00101 * \param[in] cloud a cloud containing XYZ data 00102 */ 00103 inline void 00104 setInputSource (const PointCloudSourceConstPtr &cloud) 00105 { 00106 input_ = cloud; 00107 } 00108 00109 /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. 00110 * \param[in] cloud a cloud containing XYZ data 00111 */ 00112 inline void 00113 setInputCloud (const PointCloudSourceConstPtr &cloud) 00114 { 00115 PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", 00116 getClassName ().c_str ()); 00117 input_ = cloud; 00118 } 00119 00120 /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. 00121 * \param[in] target a cloud containing XYZ data 00122 */ 00123 inline void 00124 setInputTarget (const PointCloudTargetConstPtr &target) 00125 { 00126 target_ = target; 00127 } 00128 00129 /** \brief Set the polygon cardinality 00130 * \param cardinality polygon cardinality 00131 */ 00132 inline void 00133 setCardinality (int cardinality) 00134 { 00135 cardinality_ = cardinality; 00136 } 00137 00138 /** \brief Get the polygon cardinality 00139 * \return polygon cardinality 00140 */ 00141 inline int 00142 getCardinality () 00143 { 00144 return (cardinality_); 00145 } 00146 00147 /** \brief Set the similarity threshold in [0,1[ between edge lengths, 00148 * where 1 is a perfect match 00149 * \param similarity similarity threshold 00150 */ 00151 inline void 00152 setSimilarityThreshold (float similarity_threshold) 00153 { 00154 similarity_threshold_ = similarity_threshold; 00155 similarity_threshold_squared_ = similarity_threshold * similarity_threshold; 00156 } 00157 00158 /** \brief Get the similarity threshold between edge lengths 00159 * \return similarity threshold 00160 */ 00161 inline float 00162 getSimilarityThreshold () 00163 { 00164 return (similarity_threshold_); 00165 } 00166 00167 /** \brief Set the number of iterations 00168 * \param iterations number of iterations 00169 */ 00170 inline void 00171 setIterations (int iterations) 00172 { 00173 iterations_ = iterations; 00174 } 00175 00176 /** \brief Get the number of iterations 00177 * \return number of iterations 00178 */ 00179 inline int 00180 getIterations () 00181 { 00182 return (iterations_); 00183 } 00184 00185 /** \brief Polygonal rejection of a single polygon, indexed by a subset of correspondences 00186 * \param corr all correspondences into \ref input_ and \ref target_ 00187 * \param idx sampled indices into \b correspondences, must have a size equal to \ref cardinality_ 00188 * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_ 00189 */ 00190 inline bool 00191 thresholdPolygon (const pcl::Correspondences& corr, const std::vector<int>& idx) 00192 { 00193 if (cardinality_ == 2) // Special case: when two points are considered, we only have one edge 00194 { 00195 return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query, 00196 corr[ idx[0] ].index_match, corr[ idx[1] ].index_match, 00197 cardinality_)); 00198 } 00199 else 00200 { // Otherwise check all edges 00201 for (int i = 0; i < cardinality_; ++i) 00202 if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query, 00203 corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match, 00204 similarity_threshold_squared_)) 00205 return (false); 00206 00207 return (true); 00208 } 00209 } 00210 00211 /** \brief Polygonal rejection of a single polygon, indexed by two point index vectors 00212 * \param source_indices indices of polygon points in \ref input_, must have a size equal to \ref cardinality_ 00213 * \param target_indices corresponding indices of polygon points in \ref target_, must have a size equal to \ref cardinality_ 00214 * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_ 00215 */ 00216 inline bool 00217 thresholdPolygon (const std::vector<int>& source_indices, const std::vector<int>& target_indices) 00218 { 00219 // Convert indices to correspondences and an index vector pointing to each element 00220 pcl::Correspondences corr (cardinality_); 00221 std::vector<int> idx (cardinality_); 00222 for (int i = 0; i < cardinality_; ++i) 00223 { 00224 corr[i].index_query = source_indices[i]; 00225 corr[i].index_match = target_indices[i]; 00226 idx[i] = i; 00227 } 00228 00229 return (thresholdPolygon (corr, idx)); 00230 } 00231 00232 protected: 00233 /** \brief Apply the rejection algorithm. 00234 * \param[out] correspondences the set of resultant correspondences. 00235 */ 00236 inline void 00237 applyRejection (pcl::Correspondences &correspondences) 00238 { 00239 getRemainingCorrespondences (*input_correspondences_, correspondences); 00240 } 00241 00242 /** \brief Get k unique random indices in range {0,...,n-1} (sampling without replacement) 00243 * \note No check is made to ensure that k <= n. 00244 * \param n upper index range, exclusive 00245 * \param k number of unique indices to sample 00246 * \return k unique random indices in range {0,...,n-1} 00247 */ 00248 inline std::vector<int> 00249 getUniqueRandomIndices (int n, int k) 00250 { 00251 // Marked sampled indices and sample counter 00252 std::vector<bool> sampled (n, false); 00253 int samples = 0; 00254 // Resulting unique indices 00255 std::vector<int> result; 00256 result.reserve (k); 00257 do 00258 { 00259 // Pick a random index in the range 00260 const int idx = (std::rand () % n); 00261 // If unique 00262 if (!sampled[idx]) 00263 { 00264 // Mark as sampled and increment result counter 00265 sampled[idx] = true; 00266 ++samples; 00267 // Store 00268 result.push_back (idx); 00269 } 00270 } 00271 while (samples < k); 00272 00273 return (result); 00274 } 00275 00276 /** \brief Squared Euclidean distance between two points using the members x, y and z 00277 * \param p1 first point 00278 * \param p2 second point 00279 * \return squared Euclidean distance 00280 */ 00281 inline float 00282 computeSquaredDistance (const SourceT& p1, const TargetT& p2) 00283 { 00284 const float dx = p2.x - p1.x; 00285 const float dy = p2.y - p1.y; 00286 const float dz = p2.z - p1.z; 00287 00288 return (dx*dx + dy*dy + dz*dz); 00289 } 00290 00291 /** \brief Edge length similarity thresholding 00292 * \param index_query_1 index of first source vertex 00293 * \param index_query_2 index of second source vertex 00294 * \param index_match_1 index of first target vertex 00295 * \param index_match_2 index of second target vertex 00296 * \param simsq squared similarity threshold in [0,1] 00297 * \return true if edge length ratio is larger than or equal to threshold 00298 */ 00299 inline bool 00300 thresholdEdgeLength (int index_query_1, 00301 int index_query_2, 00302 int index_match_1, 00303 int index_match_2, 00304 float simsq) 00305 { 00306 // Distance between source points 00307 const float dist_src = computeSquaredDistance ((*input_)[index_query_1], (*input_)[index_query_2]); 00308 // Distance between target points 00309 const float dist_tgt = computeSquaredDistance ((*target_)[index_match_1], (*target_)[index_match_2]); 00310 // Edge length similarity [0,1] where 1 is a perfect match 00311 const float edge_sim = (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src); 00312 00313 return (edge_sim >= simsq); 00314 } 00315 00316 /** \brief Compute a linear histogram. This function is equivalent to the MATLAB function \b histc, with the 00317 * edges set as follows: <b> lower:(upper-lower)/bins:upper </b> 00318 * \param data input samples 00319 * \param lower lower bound of input samples 00320 * \param upper upper bound of input samples 00321 * \param bins number of bins in output 00322 * \return linear histogram 00323 */ 00324 std::vector<int> 00325 computeHistogram (const std::vector<float>& data, float lower, float upper, int bins); 00326 00327 /** \brief Find the optimal value for binary histogram thresholding using Otsu's method 00328 * \param histogram input histogram 00329 * \return threshold value according to Otsu's criterion 00330 */ 00331 int 00332 findThresholdOtsu (const std::vector<int>& histogram); 00333 00334 /** \brief The input point cloud dataset */ 00335 PointCloudSourceConstPtr input_; 00336 00337 /** \brief The input point cloud dataset target */ 00338 PointCloudTargetConstPtr target_; 00339 00340 /** \brief Number of iterations to run */ 00341 int iterations_; 00342 00343 /** \brief The polygon cardinality used during rejection */ 00344 int cardinality_; 00345 00346 /** \brief Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect match */ 00347 float similarity_threshold_; 00348 00349 /** \brief Squared value if \ref similarity_threshold_, only for internal use */ 00350 float similarity_threshold_squared_; 00351 }; 00352 } 00353 } 00354 00355 #include <pcl/registration/impl/correspondence_rejection_poly.hpp> 00356 00357 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_