39#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
40#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
44namespace registration {
46template <
typename SourceT,
typename TargetT>
57 PCL_ERROR(
"[pcl::registration::%s::getRemainingCorrespondences] No source was "
58 "input! Returning all input correspondences.\n",
59 getClassName().
c_str());
64 PCL_ERROR(
"[pcl::registration::%s::getRemainingCorrespondences] No target was "
65 "input! Returning all input correspondences.\n",
66 getClassName().
c_str());
71 if (cardinality_ < 2) {
72 PCL_ERROR(
"[pcl::registration::%s::getRemainingCorrespondences] Polygon "
73 "cardinality too low!. Returning all input correspondences.\n",
74 getClassName().
c_str());
83 PCL_ERROR(
"[pcl::registration::%s::getRemainingCorrespondences] Number of "
84 "correspondences smaller than polygon cardinality! Returning all input "
86 getClassName().
c_str());
93 "[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length "
94 "similarity - must be in [0,1]!. Returning all input correspondences.\n",
95 getClassName().
c_str());
100 similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_;
111 for (
int i = 0; i < iterations_; ++i) {
113 const std::vector<int> idx =
119 for (
int j = 0; j < cardinality_; ++j) {
126 for (
int j = 0; j < cardinality_; ++j)
143 const std::vector<int> histogram =
147 const int cut_idx = findThresholdOtsu(histogram);
156template <
typename SourceT,
typename TargetT>
159 const std::vector<float>& data,
float lower,
float upper,
int bins)
169 for (
const float& value : data)
175template <
typename SourceT,
typename TargetT>
178 const std::vector<int>& histogram)
181 const double eps = std::numeric_limits<double>::epsilon();
184 const int nbins =
static_cast<int>(histogram.size());
189 for (
int i = 0; i <
nbins; ++i) {
190 mean +=
static_cast<double>(i * histogram[i]);
191 sum_inv +=
static_cast<double>(histogram[i]);
206 for (
int i = 0; i <
nbins; ++i) {
210 const double prob_i =
static_cast<double>(histogram[i]) *
sum_inv;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
std::vector< int > computeHistogram(const std::vector< float > &data, float lower, float upper, int bins)
Compute a linear histogram.
int findThresholdOtsu(const std::vector< int > &histogram)
Find the optimal value for binary histogram thresholding using Otsu's method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences