Point Cloud Library (PCL)  1.14.1-dev
geometric_consistency.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
41 #define PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
42 
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>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49 inline bool
50 gcCorrespSorter (pcl::Correspondence i, pcl::Correspondence j)
51 {
52  return (i.distance < j.distance);
53 }
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56 template<typename PointModelT, typename PointSceneT> void
58 {
59  model_instances.clear ();
60  found_transformations_.clear ();
61 
62  if (!model_scene_corrs_)
63  {
64  PCL_ERROR(
65  "[pcl::GeometricConsistencyGrouping::clusterCorrespondences()] Error! Correspondences not set, please set them before calling again this function.\n");
66  return;
67  }
68 
69  CorrespondencesPtr sorted_corrs (new Correspondences (*model_scene_corrs_));
70 
71  std::sort (sorted_corrs->begin (), sorted_corrs->end (), gcCorrespSorter);
72 
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);
78 
79  std::vector<int> consensus_set;
80  std::vector<bool> taken_corresps (model_scene_corrs_->size (), false);
81 
82  Eigen::Vector3f dist_ref, dist_trg;
83 
84  //temp copy of scene cloud with the type cast to ModelT in order to use Ransac
85  PointCloudPtr temp_scene_cloud_ptr (new PointCloud ());
86  pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr);
87 
89  corr_rejector.setMaximumIterations (10000);
90  corr_rejector.setInlierThreshold (gc_size_);
91  corr_rejector.setInputSource(input_);
92  corr_rejector.setInputTarget (temp_scene_cloud_ptr);
93 
94  for (std::size_t i = 0; i < model_scene_corrs_->size (); ++i)
95  {
96  if (taken_corresps[i])
97  continue;
98 
99  consensus_set.clear ();
100  consensus_set.push_back (static_cast<int> (i));
101 
102  for (std::size_t j = 0; j < model_scene_corrs_->size (); ++j)
103  {
104  if ( j != i && !taken_corresps[j])
105  {
106  //Let's check if j fits into the current consensus set
107  bool is_a_good_candidate = true;
108  for (const int &k : consensus_set)
109  {
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;
114 
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 ();
119 
120  dist_ref = scene_point_k - scene_point_j;
121  dist_trg = model_point_k - model_point_j;
122 
123  double distance = std::abs (dist_ref.norm () - dist_trg.norm ());
124 
125  if (distance > gc_size_)
126  {
127  is_a_good_candidate = false;
128  break;
129  }
130  }
131 
132  if (is_a_good_candidate)
133  consensus_set.push_back (static_cast<int> (j));
134  }
135  }
136 
137  if (static_cast<int> (consensus_set.size ()) > gc_threshold_)
138  {
139  Correspondences temp_corrs, filtered_corrs;
140  for (const int &j : consensus_set)
141  {
142  temp_corrs.push_back (model_scene_corrs_->at (j));
143  taken_corresps[ j ] = true;
144  }
145  //ransac filtering
146  corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
147  //save transformations for recognize
148  found_transformations_.push_back (corr_rejector.getBestTransformation ());
149 
150  model_instances.push_back (filtered_corrs);
151  }
152  }
153 }
154 
155 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
156 template<typename PointModelT, typename PointSceneT> bool
158  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
159 {
160  std::vector<pcl::Correspondences> model_instances;
161  return (this->recognize (transformations, model_instances));
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
165 template<typename PointModelT, typename PointSceneT> bool
167  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
168 {
169  transformations.clear ();
170  if (!this->initCompute ())
171  {
172  PCL_ERROR(
173  "[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
174  return (false);
175  }
176 
177  clusterCorrespondences (clustered_corrs);
178 
179  transformations = found_transformations_;
180 
181  this->deinitCompute ();
182  return (true);
183 }
184 
185 #define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping<T,ST>;
186 
187 #endif // PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
void clusterCorrespondences(std::vector< Correspondences > &model_instances) override
Cluster the input correspondences in order to distinguish between different instances of the model in...
typename PointCloud::Ptr PointCloudPtr
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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!)
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 setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
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.
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.
Definition: io.hpp:142
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
shared_ptr< Correspondences > CorrespondencesPtr
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Correspondence represents a match between two entities (e.g., points, descriptors,...