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_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ 00039 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ 00040 00041 /////////////////////////////////////////////////////////////////////////////////////////// 00042 template <typename SourceT, typename TargetT> void 00043 pcl::registration::CorrespondenceRejectorPoly<SourceT, TargetT>::getRemainingCorrespondences ( 00044 const pcl::Correspondences& original_correspondences, 00045 pcl::Correspondences& remaining_correspondences) 00046 { 00047 // This is reset after all the checks below 00048 remaining_correspondences = original_correspondences; 00049 00050 // Check source/target 00051 if (!input_) 00052 { 00053 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No source was input! Returning all input correspondences.\n", 00054 getClassName ().c_str ()); 00055 return; 00056 } 00057 00058 if (!target_) 00059 { 00060 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No target was input! Returning all input correspondences.\n", 00061 getClassName ().c_str ()); 00062 return; 00063 } 00064 00065 // Check cardinality 00066 if (cardinality_ < 2) 00067 { 00068 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Polygon cardinality too low!. Returning all input correspondences.\n", 00069 getClassName ().c_str() ); 00070 return; 00071 } 00072 00073 // Number of input correspondences 00074 const int nr_correspondences = static_cast<int> (original_correspondences.size ()); 00075 00076 // Not enough correspondences for polygonal rejections 00077 if (cardinality_ >= nr_correspondences) 00078 { 00079 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Number of correspondences smaller than polygon cardinality! Returning all input correspondences.\n", 00080 getClassName ().c_str() ); 00081 return; 00082 } 00083 00084 // Check similarity 00085 if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f) 00086 { 00087 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length similarity - must be in [0,1]!. Returning all input correspondences.\n", 00088 getClassName ().c_str() ); 00089 return; 00090 } 00091 00092 // Similarity, squared 00093 similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_; 00094 00095 // Initialization of result 00096 remaining_correspondences.clear (); 00097 remaining_correspondences.reserve (nr_correspondences); 00098 00099 // Number of times a correspondence is sampled and number of times it was accepted 00100 std::vector<int> num_samples (nr_correspondences, 0); 00101 std::vector<int> num_accepted (nr_correspondences, 0); 00102 00103 // Main loop 00104 for (int i = 0; i < iterations_; ++i) 00105 { 00106 // Sample cardinality_ correspondences without replacement 00107 const std::vector<int> idx = getUniqueRandomIndices (nr_correspondences, cardinality_); 00108 00109 // Verify the polygon similarity 00110 if (thresholdPolygon (original_correspondences, idx)) 00111 { 00112 // Increment sample counter and accept counter 00113 for (int j = 0; j < cardinality_; ++j) 00114 { 00115 ++num_samples[ idx[j] ]; 00116 ++num_accepted[ idx[j] ]; 00117 } 00118 } 00119 else 00120 { 00121 // Not accepted, only increment sample counter 00122 for (int j = 0; j < cardinality_; ++j) 00123 ++num_samples[ idx[j] ]; 00124 } 00125 } 00126 00127 // Now calculate the acceptance rate of each correspondence 00128 std::vector<float> accept_rate (nr_correspondences, 0.0f); 00129 for (int i = 0; i < nr_correspondences; ++i) 00130 { 00131 const int numsi = num_samples[i]; 00132 if (numsi == 0) 00133 accept_rate[i] = 0.0f; 00134 else 00135 accept_rate[i] = static_cast<float> (num_accepted[i]) / static_cast<float> (numsi); 00136 } 00137 00138 // Compute a histogram in range [0,1] for acceptance rates 00139 const int hist_size = nr_correspondences / 2; // TODO: Optimize this 00140 const std::vector<int> histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size); 00141 00142 // Find the cut point between outliers and inliers using Otsu's thresholding method 00143 const int cut_idx = findThresholdOtsu (histogram); 00144 const float cut = static_cast<float> (cut_idx) / static_cast<float> (hist_size); 00145 00146 // Threshold 00147 for (int i = 0; i < nr_correspondences; ++i) 00148 if (accept_rate[i] > cut) 00149 remaining_correspondences.push_back (original_correspondences[i]); 00150 } 00151 00152 ////////////////////////////////////////////////////////////////////////////////////////////// 00153 template <typename SourceT, typename TargetT> std::vector<int> 00154 pcl::registration::CorrespondenceRejectorPoly<SourceT, TargetT>::computeHistogram (const std::vector<float>& data, 00155 float lower, float upper, int bins) 00156 { 00157 // Result 00158 std::vector<int> result (bins, 0); 00159 00160 // Last index into result and increment factor from data value --> index 00161 const int last_idx = bins - 1; 00162 const float idx_per_val = static_cast<float> (bins) / (upper - lower); 00163 00164 // Accumulate 00165 for (std::vector<float>::const_iterator it = data.begin (); it != data.end (); ++it) 00166 ++result[ std::min (last_idx, int ((*it)*idx_per_val)) ]; 00167 00168 return (result); 00169 } 00170 00171 ////////////////////////////////////////////////////////////////////////////////////////////// 00172 template <typename SourceT, typename TargetT> int 00173 pcl::registration::CorrespondenceRejectorPoly<SourceT, TargetT>::findThresholdOtsu (const std::vector<int>& histogram) 00174 { 00175 // Precision 00176 const double eps = std::numeric_limits<double>::epsilon(); 00177 00178 // Histogram dimension 00179 const int nbins = static_cast<int> (histogram.size ()); 00180 00181 // Mean and inverse of the number of data points 00182 double mean = 0.0; 00183 double sum_inv = 0.0; 00184 for (int i = 0; i < nbins; ++i) 00185 { 00186 mean += static_cast<double> (i * histogram[i]); 00187 sum_inv += static_cast<double> (histogram[i]); 00188 } 00189 sum_inv = 1.0/sum_inv; 00190 mean *= sum_inv; 00191 00192 // Probability and mean of class 1 (data to the left of threshold) 00193 double class_mean1 = 0.0; 00194 double class_prob1 = 0.0; 00195 double class_prob2 = 1.0; 00196 00197 // Maximized between class variance and associated bin value 00198 double between_class_variance_max = 0.0; 00199 int result = 0; 00200 00201 // Loop over all bin values 00202 for (int i = 0; i < nbins; ++i) 00203 { 00204 class_mean1 *= class_prob1; 00205 00206 // Probability of bin i 00207 const double prob_i = static_cast<double> (histogram[i]) * sum_inv; 00208 00209 // Class probability 1: sum of probabilities from 0 to i 00210 class_prob1 += prob_i; 00211 00212 // Class probability 2: sum of probabilities from i+1 to nbins-1 00213 class_prob2 -= prob_i; 00214 00215 // Avoid division by zero below 00216 if (std::min (class_prob1,class_prob2) < eps || std::max (class_prob1,class_prob2) > 1.0-eps) 00217 continue; 00218 00219 // Class mean 1: sum of probabilities from 0 to i, weighted by bin value 00220 class_mean1 = (class_mean1 + static_cast<double> (i) * prob_i) / class_prob1; 00221 00222 // Class mean 2: sum of probabilities from i+1 to nbins-1, weighted by bin value 00223 const double class_mean2 = (mean - class_prob1*class_mean1) / class_prob2; 00224 00225 // Between class variance 00226 const double between_class_variance = class_prob1 * class_prob2 00227 * (class_mean1 - class_mean2) 00228 * (class_mean1 - class_mean2); 00229 00230 // If between class variance is maximized, update result 00231 if (between_class_variance > between_class_variance_max) 00232 { 00233 between_class_variance_max = between_class_variance; 00234 result = i; 00235 } 00236 } 00237 00238 return (result); 00239 } 00240 00241 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_