38 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
39 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
42 template <
typename SourceT,
typename TargetT>
void
48 remaining_correspondences = original_correspondences;
53 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] No source was input! Returning all input correspondences.\n",
54 getClassName ().c_str ());
60 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] No target was input! Returning all input correspondences.\n",
61 getClassName ().c_str ());
68 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] Polygon cardinality too low!. Returning all input correspondences.\n",
69 getClassName ().c_str() );
74 const int nr_correspondences =
static_cast<int> (original_correspondences.size ());
77 if (cardinality_ >= nr_correspondences)
79 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] Number of correspondences smaller than polygon cardinality! Returning all input correspondences.\n",
80 getClassName ().c_str() );
85 if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f)
87 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length similarity - must be in [0,1]!. Returning all input correspondences.\n",
88 getClassName ().c_str() );
93 similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_;
96 remaining_correspondences.clear ();
97 remaining_correspondences.reserve (nr_correspondences);
100 std::vector<int> num_samples (nr_correspondences, 0);
101 std::vector<int> num_accepted (nr_correspondences, 0);
104 for (
int i = 0; i < iterations_; ++i)
107 const std::vector<int> idx = getUniqueRandomIndices (nr_correspondences, cardinality_);
110 if (thresholdPolygon (original_correspondences, idx))
113 for (
int j = 0; j < cardinality_; ++j)
115 ++num_samples[ idx[j] ];
116 ++num_accepted[ idx[j] ];
122 for (
int j = 0; j < cardinality_; ++j)
123 ++num_samples[ idx[j] ];
128 std::vector<float> accept_rate (nr_correspondences, 0.0f);
129 for (
int i = 0; i < nr_correspondences; ++i)
131 const int numsi = num_samples[i];
133 accept_rate[i] = 0.0f;
135 accept_rate[i] =
static_cast<float> (num_accepted[i]) / static_cast<float> (numsi);
139 const int hist_size = nr_correspondences / 2;
140 const std::vector<int> histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size);
143 const int cut_idx = findThresholdOtsu (histogram);
144 const float cut =
static_cast<float> (cut_idx) / static_cast<float> (hist_size);
147 for (
int i = 0; i < nr_correspondences; ++i)
148 if (accept_rate[i] > cut)
149 remaining_correspondences.push_back (original_correspondences[i]);
153 template <
typename SourceT,
typename TargetT> std::vector<int>
155 float lower,
float upper,
int bins)
158 std::vector<int> result (bins, 0);
161 const int last_idx = bins - 1;
162 const float idx_per_val =
static_cast<float> (bins) / (upper - lower);
165 for (std::vector<float>::const_iterator it = data.begin (); it != data.end (); ++it)
166 ++result[ std::min (last_idx,
int ((*it)*idx_per_val)) ];
172 template <
typename SourceT,
typename TargetT>
int
176 const double eps = std::numeric_limits<double>::epsilon();
179 const int nbins =
static_cast<int> (histogram.size ());
183 double sum_inv = 0.0;
184 for (
int i = 0; i < nbins; ++i)
186 mean +=
static_cast<double> (i * histogram[i]);
187 sum_inv +=
static_cast<double> (histogram[i]);
189 sum_inv = 1.0/sum_inv;
193 double class_mean1 = 0.0;
194 double class_prob1 = 0.0;
195 double class_prob2 = 1.0;
198 double between_class_variance_max = 0.0;
202 for (
int i = 0; i < nbins; ++i)
204 class_mean1 *= class_prob1;
207 const double prob_i =
static_cast<double> (histogram[i]) * sum_inv;
210 class_prob1 += prob_i;
213 class_prob2 -= prob_i;
216 if (std::min (class_prob1,class_prob2) < eps || std::max (class_prob1,class_prob2) > 1.0-eps)
220 class_mean1 = (class_mean1 +
static_cast<double> (i) * prob_i) / class_prob1;
223 const double class_mean2 = (mean - class_prob1*class_mean1) / class_prob2;
226 const double between_class_variance = class_prob1 * class_prob2
227 * (class_mean1 - class_mean2)
228 * (class_mean1 - class_mean2);
231 if (between_class_variance > between_class_variance_max)
233 between_class_variance_max = between_class_variance;
241 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_