Point Cloud Library (PCL)  1.14.1
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_
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.
Definition: io.hpp:142
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. ...
Definition: distances.h:55
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