40 #ifndef PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
41 #define PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
43 #include <pcl/recognition/cg/geometric_consistency.h>
44 #include <pcl/registration/correspondence_types.h>
45 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
46 #include <pcl/common/io.h>
56 template<
typename Po
intModelT,
typename Po
intSceneT>
void
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);
152 template<
typename Po
intModelT,
typename Po
intSceneT>
bool
154 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
156 std::vector<pcl::Correspondences> model_instances;
157 return (this->recognize (transformations, model_instances));
161 template<
typename Po
intModelT,
typename Po
intSceneT>
bool
163 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
165 transformations.clear ();
166 if (!this->initCompute ())
169 "[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
173 clusterCorrespondences (clustered_corrs);
175 transformations = found_transformations_;
177 this->deinitCompute ();
181 #define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping<T,ST>;
183 #endif // PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
typename PointCloud::Ptr PointCloudPtr
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
Eigen::Matrix4f getBestTransformation()
Get the best transformation after RANSAC rejection.
virtual void setInputTarget(const PointCloudConstPtr &cloud)
Provide a target point cloud dataset (must contain XYZ data!)
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.
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
void clusterCorrespondences(std::vector< Correspondences > &model_instances) override
Cluster the input correspondences in order to distinguish between different instances of the model in...
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!)
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
bool recognize(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations)
The main function, recognizes instances of the model into the scene set by the user.
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.
shared_ptr< Correspondences > CorrespondencesPtr