Point Cloud Library (PCL)  1.7.1
greedy_verification.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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 
37 #ifndef PCL_RECOGNITION_HV_GREEDY_H_
38 #define PCL_RECOGNITION_HV_GREEDY_H_
39 
40 #include <pcl/pcl_macros.h>
41 #include <pcl/recognition/hv/hypotheses_verification.h>
42 #include <pcl/common/common.h>
43 
44 namespace pcl
45 {
46 
47  /**
48  * \brief A greedy hypothesis verification method
49  * \author Aitor Aldoma
50  */
51 
52  template<typename ModelT, typename SceneT>
53  class PCL_EXPORTS GreedyVerification : public HypothesisVerification<ModelT, SceneT>
54  {
61 
62  /*
63  * \brief Recognition model using during the verification
64  */
65  class RecognitionModel
66  {
67  public:
68  std::vector<int> explained_;
69  typename pcl::PointCloud<ModelT>::Ptr cloud_;
70  int bad_information_;
71  int good_information_;
72  int id_;
73  float regularizer_;
74  };
75 
76  /*
77  * \brief Sorts recognition models based on the number of explained scene points and visible outliers
78  */
79  struct sortModelsClass
80  {
81  bool
82  operator() (const boost::shared_ptr<RecognitionModel> & n1, const boost::shared_ptr<RecognitionModel> & n2)
83  {
84  float val1 = static_cast<float>(n1->good_information_) - static_cast<float>(n1->bad_information_) * n1->regularizer_;
85  float val2 = static_cast<float>(n2->good_information_) - static_cast<float>(n2->bad_information_) * n2->regularizer_;
86  return val1 > val2;
87  }
88  } sortModelsOp;
89 
90 
91  /*
92  * \brief Recognition model indices to keep track of the sorted recognition hypotheses
93  */
94  struct modelIndices
95  {
96  int index_;
97  boost::shared_ptr<RecognitionModel> model_;
98  };
99 
100  /*
101  * \brief Sorts model indices similar to sortModelsClass
102  */
103  struct sortModelIndicesClass
104  {
105  bool
106  operator() (const modelIndices & n1, const modelIndices & n2)
107  {
108  float val1 = static_cast<float>(n1.model_->good_information_) - static_cast<float>(n1.model_->bad_information_) * n1.model_->regularizer_;
109  float val2 = static_cast<float>(n2.model_->good_information_) - static_cast<float>(n2.model_->bad_information_) * n2.model_->regularizer_;
110  return val1 > val2;
111  }
112  } sortModelsIndicesOp;
113 
114  /** \brief Recognition model and indices */
115  std::vector<modelIndices> indices_models_;
116 
117  /** \brief Recognition models (hypotheses to be verified) */
118  std::vector<boost::shared_ptr<RecognitionModel> > recognition_models_;
119 
120  /** \brief Recognition models that explain a scene points. */
121  std::vector<std::vector<boost::shared_ptr<RecognitionModel> > > points_explained_by_rm_;
122 
123  /** \brief Weighting for outliers */
124  float regularizer_;
125 
126  /** \brief Initialize the data structures */
127  void
128  initialize ();
129 
130  /** \brief Sorts the hypotheses for the greedy approach */
131  void
132  sortModels ()
133  {
134  indices_models_.clear ();
135  for (size_t i = 0; i < recognition_models_.size (); i++)
136  {
137  modelIndices mi;
138  mi.index_ = static_cast<int> (i);
139  mi.model_ = recognition_models_[i];
140  indices_models_.push_back (mi);
141  }
142 
143  std::sort (indices_models_.begin (), indices_models_.end (), sortModelsIndicesOp);
144  //sort also recognition models
145  std::sort (recognition_models_.begin (), recognition_models_.end (), sortModelsOp);
146  }
147 
148  /** \brief Updates conflicting recognition hypotheses when a hypothesis is accepted */
149  void
150  updateGoodInformation (int i)
151  {
152  for (size_t k = 0; k < recognition_models_[i]->explained_.size (); k++)
153  {
154  //update good_information_ for all hypotheses that were explaining the same points as hypothesis i
155  for (size_t kk = 0; kk < points_explained_by_rm_[recognition_models_[i]->explained_[k]].size (); kk++)
156  {
157  (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->good_information_--;
158  (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->bad_information_++;
159  }
160  }
161  }
162 
163  public:
164 
165  /** \brief Constructor
166  * \param[in] Regularizer value
167  **/
168  GreedyVerification (float reg = 1.5f) :
169  HypothesisVerification<ModelT, SceneT> ()
170  {
171  regularizer_ = reg;
172  }
173 
174  /** \brief Starts verification */
175  void
176  verify ();
177  };
178 }
179 
180 #ifdef PCL_NO_PRECOMPILE
181 #include <pcl/recognition/impl/hv/greedy_verification.hpp>
182 #endif
183 
184 #endif /* PCL_RECOGNITION_HV_GREEDY_H_ */