Point Cloud Library (PCL)  1.9.1
geometric_consistency.h
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_H_
41 #define PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_H_
42 
43 #include <pcl/recognition/cg/correspondence_grouping.h>
44 #include <pcl/point_cloud.h>
45 
46 namespace pcl
47 {
48 
49  /** \brief Class implementing a 3D correspondence grouping enforcing geometric consistency among feature correspondences
50  *
51  * \author Federico Tombari, Tommaso Cavallari, Aitor Aldoma
52  * \ingroup recognition
53  */
54  template<typename PointModelT, typename PointSceneT>
55  class GeometricConsistencyGrouping : public CorrespondenceGrouping<PointModelT, PointSceneT>
56  {
57  public:
59  typedef typename PointCloud::Ptr PointCloudPtr;
61 
63 
64  /** \brief Constructor */
66  : gc_threshold_ (3)
67  , gc_size_ (1.0)
69  {}
70 
71 
72  /** \brief Sets the minimum cluster size
73  * \param[in] threshold the minimum cluster size
74  */
75  inline void
76  setGCThreshold (int threshold)
77  {
78  gc_threshold_ = threshold;
79  }
80 
81  /** \brief Gets the minimum cluster size.
82  *
83  * \return the minimum cluster size used by GC.
84  */
85  inline int
86  getGCThreshold () const
87  {
88  return (gc_threshold_);
89  }
90 
91  /** \brief Sets the consensus set resolution. This should be in metric units.
92  *
93  * \param[in] gc_size consensus set resolution.
94  */
95  inline void
96  setGCSize (double gc_size)
97  {
98  gc_size_ = gc_size;
99  }
100 
101  /** \brief Gets the consensus set resolution.
102  *
103  * \return the consensus set resolution.
104  */
105  inline double
106  getGCSize () const
107  {
108  return (gc_size_);
109  }
110 
111  /** \brief The main function, recognizes instances of the model into the scene set by the user.
112  *
113  * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene.
114  *
115  * \return true if the recognition had been successful or false if errors have occurred.
116  */
117  bool
118  recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations);
119 
120  /** \brief The main function, recognizes instances of the model into the scene set by the user.
121  *
122  * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene.
123  * \param[out] clustered_corrs a vector containing the correspondences for each instance of the model found within the input data (the same output of clusterCorrespondences).
124  *
125  * \return true if the recognition had been successful or false if errors have occurred.
126  */
127  bool
128  recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs);
129 
130  protected:
134 
135  /** \brief Minimum cluster size. It shouldn't be less than 3, since at least 3 correspondences are needed to compute the 6DOF pose */
137 
138  /** \brief Resolution of the consensus set used to cluster correspondences together*/
139  double gc_size_;
140 
141  /** \brief Transformations found by clusterCorrespondences method. */
142  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > found_transformations_;
143 
144  /** \brief Cluster the input correspondences in order to distinguish between different instances of the model into the scene.
145  *
146  * \param[out] model_instances a vector containing the clustered correspondences for each model found on the scene.
147  * \return true if the clustering had been successful or false if errors have occurred.
148  */
149  void
150  clusterCorrespondences (std::vector<Correspondences> &model_instances);
151  };
152 }
153 
154 #ifdef PCL_NO_PRECOMPILE
155 #include <pcl/recognition/impl/cg/geometric_consistency.hpp>
156 #endif
157 
158 #endif // PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_H_
double gc_size_
Resolution of the consensus set used to cluster correspondences together.
int gc_threshold_
Minimum cluster size.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void setGCThreshold(int threshold)
Sets the minimum cluster size.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void setGCSize(double gc_size)
Sets the consensus set resolution.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::CorrespondenceGrouping< PointModelT, PointSceneT >::SceneCloudConstPtr SceneCloudConstPtr
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. ...
Class implementing a 3D correspondence grouping enforcing geometric consistency among feature corresp...
SceneCloud::ConstPtr SceneCloudConstPtr
Abstract base class for Correspondence Grouping algorithms.
int getGCThreshold() const
Gets the minimum cluster size.
double getGCSize() const
Gets the consensus set resolution.
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.
std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > found_transformations_
Transformations found by clusterCorrespondences method.
pcl::PointCloud< PointModelT > PointCloud