40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
47 namespace registration
50 template <
typename SourceT,
typename TargetT>
void
56 remaining_correspondences = original_correspondences;
61 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] No source was input! Returning all input correspondences.\n",
62 getClassName ().c_str ());
68 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] No target was input! Returning all input correspondences.\n",
69 getClassName ().c_str ());
76 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] Polygon cardinality too low!. Returning all input correspondences.\n",
77 getClassName ().c_str() );
82 const int nr_correspondences =
static_cast<int> (original_correspondences.size ());
85 if (cardinality_ >= nr_correspondences)
87 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] Number of correspondences smaller than polygon cardinality! Returning all input correspondences.\n",
88 getClassName ().c_str() );
93 if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f)
95 PCL_ERROR (
"[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length similarity - must be in [0,1]!. Returning all input correspondences.\n",
96 getClassName ().c_str() );
101 similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_;
104 remaining_correspondences.clear ();
105 remaining_correspondences.reserve (nr_correspondences);
108 std::vector<int> num_samples (nr_correspondences, 0);
109 std::vector<int> num_accepted (nr_correspondences, 0);
112 for (
int i = 0; i < iterations_; ++i)
115 const std::vector<int> idx = getUniqueRandomIndices (nr_correspondences, cardinality_);
118 if (thresholdPolygon (original_correspondences, idx))
121 for (
int j = 0; j < cardinality_; ++j)
123 ++num_samples[ idx[j] ];
124 ++num_accepted[ idx[j] ];
130 for (
int j = 0; j < cardinality_; ++j)
131 ++num_samples[ idx[j] ];
136 std::vector<float> accept_rate (nr_correspondences, 0.0f);
137 for (
int i = 0; i < nr_correspondences; ++i)
139 const int numsi = num_samples[i];
141 accept_rate[i] = 0.0f;
143 accept_rate[i] =
static_cast<float> (num_accepted[i]) /
static_cast<float> (numsi);
147 const int hist_size = nr_correspondences / 2;
148 const std::vector<int> histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size);
151 const int cut_idx = findThresholdOtsu (histogram);
152 const float cut =
static_cast<float> (cut_idx) /
static_cast<float> (hist_size);
155 for (
int i = 0; i < nr_correspondences; ++i)
156 if (accept_rate[i] > cut)
157 remaining_correspondences.push_back (original_correspondences[i]);
161 template <
typename SourceT,
typename TargetT> std::vector<int>
163 float lower,
float upper,
int bins)
166 std::vector<int> result (bins, 0);
169 const int last_idx = bins - 1;
170 const float idx_per_val =
static_cast<float> (bins) / (upper - lower);
173 for (
const float &value : data)
174 ++result[ std::min (last_idx,
int (value*idx_per_val)) ];
180 template <
typename SourceT,
typename TargetT>
int
184 const double eps = std::numeric_limits<double>::epsilon();
187 const int nbins =
static_cast<int> (histogram.size ());
191 double sum_inv = 0.0;
192 for (
int i = 0; i < nbins; ++i)
194 mean +=
static_cast<double> (i * histogram[i]);
195 sum_inv +=
static_cast<double> (histogram[i]);
197 sum_inv = 1.0/sum_inv;
201 double class_mean1 = 0.0;
202 double class_prob1 = 0.0;
203 double class_prob2 = 1.0;
206 double between_class_variance_max = 0.0;
210 for (
int i = 0; i < nbins; ++i)
212 class_mean1 *= class_prob1;
215 const double prob_i =
static_cast<double> (histogram[i]) * sum_inv;
218 class_prob1 += prob_i;
221 class_prob2 -= prob_i;
224 if (std::min (class_prob1,class_prob2) < eps || std::max (class_prob1,class_prob2) > 1.0-eps)
228 class_mean1 = (class_mean1 +
static_cast<double> (i) * prob_i) / class_prob1;
231 const double class_mean2 = (mean - class_prob1*class_mean1) / class_prob2;
234 const double between_class_variance = class_prob1 * class_prob2
235 * (class_mean1 - class_mean2)
236 * (class_mean1 - class_mean2);
239 if (between_class_variance > between_class_variance_max)
241 between_class_variance_max = between_class_variance;
252 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_