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;
82 pcl::copyPointCloud<PointSceneT, PointModelT> (*scene_, *temp_scene_cloud_ptr);
86 corr_rejector.setInlierThreshold (gc_size_);
87 corr_rejector.setInputSource(input_);
88 corr_rejector.setInputTarget (temp_scene_cloud_ptr);
90 for (
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 (
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 (
size_t k = 0; k < consensus_set.size (); ++k)
106 int scene_index_k = model_scene_corrs_->at (consensus_set[k]).index_match;
107 int model_index_k = model_scene_corrs_->at (consensus_set[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 = fabs (dist_ref.norm () - dist_trg.norm ());
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 (
size_t j = 0; j < consensus_set.size (); j++)
138 temp_corrs.push_back (model_scene_corrs_->at (consensus_set[j]));
139 taken_corresps[ consensus_set[j] ] =
true;
142 corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
144 found_transformations_.push_back (corr_rejector.getBestTransformation ());
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_
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
float distance(const PointT &p1, const PointT &p2)
PointCloud::Ptr PointCloudPtr
boost::shared_ptr< Correspondences > CorrespondencesPtr
void clusterCorrespondences(std::vector< Correspondences > &model_instances)
Cluster the input correspondences in order to distinguish between different instances of the model in...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
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.