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;
74 PCL_DEBUG_STREAM(
"[pcl::GeometricConsistencyGrouping::clusterCorrespondences] Five best correspondences: ");
75 for(std::size_t i=0; i<std::min<std::size_t>(model_scene_corrs_->size(), 5); ++i)
76 PCL_DEBUG_STREAM(
"[" << (*input_)[(*model_scene_corrs_)[i].index_query] <<
" " << (*scene_)[(*model_scene_corrs_)[i].index_match] <<
" " << (*model_scene_corrs_)[i].distance <<
"] ");
77 PCL_DEBUG_STREAM(std::endl);
79 std::vector<int> consensus_set;
80 std::vector<bool> taken_corresps (model_scene_corrs_->size (),
false);
82 Eigen::Vector3f dist_ref, dist_trg;
94 for (std::size_t i = 0; i < model_scene_corrs_->size (); ++i)
96 if (taken_corresps[i])
99 consensus_set.clear ();
100 consensus_set.push_back (static_cast<int> (i));
102 for (std::size_t j = 0; j < model_scene_corrs_->size (); ++j)
104 if ( j != i && !taken_corresps[j])
107 bool is_a_good_candidate =
true;
108 for (
const int &k : consensus_set)
110 int scene_index_k = model_scene_corrs_->at (k).index_match;
111 int model_index_k = model_scene_corrs_->at (k).index_query;
112 int scene_index_j = model_scene_corrs_->at (j).index_match;
113 int model_index_j = model_scene_corrs_->at (j).index_query;
115 const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap ();
116 const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap ();
117 const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap ();
118 const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap ();
120 dist_ref = scene_point_k - scene_point_j;
121 dist_trg = model_point_k - model_point_j;
123 double distance = std::abs (dist_ref.norm () - dist_trg.norm ());
125 if (distance > gc_size_)
127 is_a_good_candidate =
false;
132 if (is_a_good_candidate)
133 consensus_set.push_back (static_cast<int> (j));
137 if (static_cast<int> (consensus_set.size ()) > gc_threshold_)
140 for (
const int &j : consensus_set)
142 temp_corrs.push_back (model_scene_corrs_->at (j));
143 taken_corresps[ j ] =
true;
150 model_instances.push_back (filtered_corrs);
156 template<
typename Po
intModelT,
typename Po
intSceneT>
bool
158 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
160 std::vector<pcl::Correspondences> model_instances;
161 return (this->recognize (transformations, model_instances));
165 template<
typename Po
intModelT,
typename Po
intSceneT>
bool
167 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
169 transformations.clear ();
170 if (!this->initCompute ())
173 "[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
177 clusterCorrespondences (clustered_corrs);
179 transformations = found_transformations_;
181 this->deinitCompute ();
185 #define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping<T,ST>;
187 #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