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);
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;
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,...
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 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)
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
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...
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!)
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)
Get a list of valid correspondences after rejection from the original set of correspondences.