Point Cloud Library (PCL)  1.9.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 
75  std::vector<int> consensus_set;
76  std::vector<bool> taken_corresps (model_scene_corrs_->size (), false);
77 
78  Eigen::Vector3f dist_ref, dist_trg;
79 
80  //temp copy of scene cloud with the type cast to ModelT in order to use Ransac
81  PointCloudPtr temp_scene_cloud_ptr (new PointCloud ());
82  pcl::copyPointCloud<PointSceneT, PointModelT> (*scene_, *temp_scene_cloud_ptr);
83 
85  corr_rejector.setMaximumIterations (10000);
86  corr_rejector.setInlierThreshold (gc_size_);
87  corr_rejector.setInputSource(input_);
88  corr_rejector.setInputTarget (temp_scene_cloud_ptr);
89 
90  for (size_t i = 0; i < model_scene_corrs_->size (); ++i)
91  {
92  if (taken_corresps[i])
93  continue;
94 
95  consensus_set.clear ();
96  consensus_set.push_back (static_cast<int> (i));
97 
98  for (size_t j = 0; j < model_scene_corrs_->size (); ++j)
99  {
100  if ( j != i && !taken_corresps[j])
101  {
102  //Let's check if j fits into the current consensus set
103  bool is_a_good_candidate = true;
104  for (size_t k = 0; k < consensus_set.size (); ++k)
105  {
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;
110 
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 ();
115 
116  dist_ref = scene_point_k - scene_point_j;
117  dist_trg = model_point_k - model_point_j;
118 
119  double distance = fabs (dist_ref.norm () - dist_trg.norm ());
120 
121  if (distance > gc_size_)
122  {
123  is_a_good_candidate = false;
124  break;
125  }
126  }
127 
128  if (is_a_good_candidate)
129  consensus_set.push_back (static_cast<int> (j));
130  }
131  }
132 
133  if (static_cast<int> (consensus_set.size ()) > gc_threshold_)
134  {
135  Correspondences temp_corrs, filtered_corrs;
136  for (size_t j = 0; j < consensus_set.size (); j++)
137  {
138  temp_corrs.push_back (model_scene_corrs_->at (consensus_set[j]));
139  taken_corresps[ consensus_set[j] ] = true;
140  }
141  //ransac filtering
142  corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
143  //save transformations for recognize
144  found_transformations_.push_back (corr_rejector.getBestTransformation ());
145 
146  model_instances.push_back (filtered_corrs);
147  }
148  }
149 }
150 
151 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
152 template<typename PointModelT, typename PointSceneT> bool
154  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
155 {
156  std::vector<pcl::Correspondences> model_instances;
157  return (this->recognize (transformations, model_instances));
158 }
159 
160 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
161 template<typename PointModelT, typename PointSceneT> bool
163  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
164 {
165  transformations.clear ();
166  if (!this->initCompute ())
167  {
168  PCL_ERROR(
169  "[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
170  return (false);
171  }
172 
173  clusterCorrespondences (clustered_corrs);
174 
175  transformations = found_transformations_;
176 
177  this->deinitCompute ();
178  return (true);
179 }
180 
181 #define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping<T,ST>;
182 
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)
Definition: geometry.h:61
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.