59 model_instances.clear ();
60 found_transformations_.clear ();
62 if (!model_scene_corrs_)
65 "[pcl::GeometricConsistencyGrouping::clusterCorrespondences()] Error! Correspondences not set, please set them before calling again this function.\n");
71 std::sort (sorted_corrs->begin (), sorted_corrs->end (), gcCorrespSorter);
73 model_scene_corrs_ = sorted_corrs;
75 std::vector<int> consensus_set;
76 std::vector<bool> taken_corresps (model_scene_corrs_->size (),
false);
78 Eigen::Vector3f dist_ref, dist_trg;
90 for (std::size_t i = 0; i < model_scene_corrs_->size (); ++i)
92 if (taken_corresps[i])
95 consensus_set.clear ();
96 consensus_set.push_back (
static_cast<int> (i));
98 for (std::size_t j = 0; j < model_scene_corrs_->size (); ++j)
100 if ( j != i && !taken_corresps[j])
103 bool is_a_good_candidate =
true;
104 for (
const int &k : consensus_set)
106 int scene_index_k = model_scene_corrs_->at (k).index_match;
107 int model_index_k = model_scene_corrs_->at (k).index_query;
108 int scene_index_j = model_scene_corrs_->at (j).index_match;
109 int model_index_j = model_scene_corrs_->at (j).index_query;
111 const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap ();
112 const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap ();
113 const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap ();
114 const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap ();
116 dist_ref = scene_point_k - scene_point_j;
117 dist_trg = model_point_k - model_point_j;
119 double distance = std::abs (dist_ref.norm () - dist_trg.norm ());
121 if (distance > gc_size_)
123 is_a_good_candidate =
false;
128 if (is_a_good_candidate)
129 consensus_set.push_back (
static_cast<int> (j));
133 if (
static_cast<int> (consensus_set.size ()) > gc_threshold_)
136 for (
const int &j : consensus_set)
138 temp_corrs.push_back (model_scene_corrs_->at (j));
139 taken_corresps[ j ] =
true;
146 model_instances.push_back (filtered_corrs);
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.