Point Cloud Library (PCL)  1.8.0
hv_go.h
1 /*
2  * go.h
3  *
4  * Created on: Jun 4, 2012
5  * Author: aitor
6  */
7 
8 #ifndef GO_H_
9 #define GO_H_
10 
11 #include <pcl/pcl_macros.h>
12 #include <pcl/recognition/hv/hypotheses_verification.h>
13 #include <pcl/common/common.h>
14 #include <metslib/mets.hh>
15 #include <pcl/features/normal_3d.h>
16 #include <boost/graph/graph_traits.hpp>
17 #include <boost/graph/adjacency_list.hpp>
18 
19 namespace pcl
20 {
21 
22  /** \brief A hypothesis verification method proposed in
23  * "A Global Hypotheses Verification Method for 3D Object Recognition", A. Aldoma and F. Tombari and L. Di Stefano and Markus Vincze, ECCV 2012
24  * \author Aitor Aldoma
25  */
26  template<typename ModelT, typename SceneT>
27  class PCL_EXPORTS GlobalHypothesesVerification: public HypothesisVerification<ModelT, SceneT>
28  {
29  private:
30 
31  //Helper classes
32  struct RecognitionModel
33  {
34  public:
35  std::vector<int> explained_; //indices vector referencing explained_by_RM_
36  std::vector<float> explained_distances_; //closest distances to the scene for point i
37  std::vector<int> unexplained_in_neighborhood; //indices vector referencing unexplained_by_RM_neighboorhods
38  std::vector<float> unexplained_in_neighborhood_weights; //weights for the points not being explained in the neighborhood of a hypothesis
39  std::vector<int> outlier_indices_; //outlier indices of this model
40  std::vector<int> complete_cloud_occupancy_indices_;
41  typename pcl::PointCloud<ModelT>::Ptr cloud_;
42  typename pcl::PointCloud<ModelT>::Ptr complete_cloud_;
43  int bad_information_;
44  float outliers_weight_;
46  int id_;
47  };
48 
50  class SAModel: public mets::evaluable_solution
51  {
52  public:
53  std::vector<bool> solution_;
54  SAOptimizerT * opt_;
55  mets::gol_type cost_;
56 
57  //Evaluates the current solution
58  mets::gol_type cost_function() const
59  {
60  return cost_;
61  }
62 
63  void copy_from(const mets::copyable& o)
64  {
65  const SAModel& s = dynamic_cast<const SAModel&> (o);
66  solution_ = s.solution_;
67  opt_ = s.opt_;
68  cost_ = s.cost_;
69  }
70 
71  mets::gol_type what_if(int /*index*/, bool /*val*/) const
72  {
73  /*std::vector<bool> tmp (solution_);
74  tmp[index] = val;
75  mets::gol_type sol = opt_->evaluateSolution (solution_, index); //evaluate without updating status
76  return sol;*/
77  return static_cast<mets::gol_type>(0);
78  }
79 
80  mets::gol_type apply_and_evaluate(int index, bool val)
81  {
82  solution_[index] = val;
83  mets::gol_type sol = opt_->evaluateSolution (solution_, index); //this will update the state of the solution
84  cost_ = sol;
85  return sol;
86  }
87 
88  void apply(int /*index*/, bool /*val*/)
89  {
90 
91  }
92 
93  void unapply(int index, bool val)
94  {
95  solution_[index] = val;
96  //update optimizer solution
97  cost_ = opt_->evaluateSolution (solution_, index); //this will udpate the cost function in opt_
98  }
99  void setSolution(std::vector<bool> & sol)
100  {
101  solution_ = sol;
102  }
103 
104  void setOptimizer(SAOptimizerT * opt)
105  {
106  opt_ = opt;
107  }
108  };
109 
110  /*
111  * Represents a move, deactivate a hypothesis
112  */
113 
114  class move: public mets::move
115  {
116  int index_;
117  public:
118  move(int i) :
119  index_ (i)
120  {
121  }
122 
123  mets::gol_type evaluate(const mets::feasible_solution& /*cs*/) const
124  {
125  return static_cast<mets::gol_type>(0);
126  }
127 
128  mets::gol_type apply_and_evaluate(mets::feasible_solution& cs)
129  {
130  SAModel& model = dynamic_cast<SAModel&> (cs);
131  return model.apply_and_evaluate (index_, !model.solution_[index_]);
132  }
133 
134  void apply(mets::feasible_solution& /*s*/) const
135  {
136  }
137 
138  void unapply(mets::feasible_solution& s) const
139  {
140  SAModel& model = dynamic_cast<SAModel&> (s);
141  model.unapply (index_, !model.solution_[index_]);
142  }
143  };
144 
145  class move_manager
146  {
147  public:
148  std::vector<move*> moves_m;
149  typedef typename std::vector<move*>::iterator iterator;
150  iterator begin()
151  {
152  return moves_m.begin ();
153  }
154  iterator end()
155  {
156  return moves_m.end ();
157  }
158 
159  move_manager(int problem_size)
160  {
161  for (int ii = 0; ii != problem_size; ++ii)
162  moves_m.push_back (new move (ii));
163  }
164 
165  ~move_manager()
166  {
167  // delete all moves
168  for (iterator ii = begin (); ii != end (); ++ii)
169  delete (*ii);
170  }
171 
172  void refresh(mets::feasible_solution& /*s*/)
173  {
174  std::random_shuffle (moves_m.begin (), moves_m.end ());
175  }
176 
177  };
178 
179  //inherited class attributes
187 
188  //class attributes
190  pcl::PointCloud<pcl::Normal>::Ptr scene_normals_;
191  pcl::PointCloud<pcl::PointXYZI>::Ptr clusters_cloud_;
192 
193  std::vector<int> complete_cloud_occupancy_by_RM_;
194  float res_occupancy_grid_;
195  float w_occupied_multiple_cm_;
196 
197  std::vector<int> explained_by_RM_; //represents the points of scene_cloud_ that are explained by the recognition models
198  std::vector<float> explained_by_RM_distance_weighted; //represents the points of scene_cloud_ that are explained by the recognition models
199  std::vector<float> unexplained_by_RM_neighboorhods; //represents the points of scene_cloud_ that are not explained by the active hypotheses in the neighboorhod of the recognition models
200  std::vector<boost::shared_ptr<RecognitionModel> > recognition_models_;
201  std::vector<size_t> indices_;
202 
203  float regularizer_;
204  float clutter_regularizer_;
205  bool detect_clutter_;
206  float radius_neighborhood_GO_;
207  float radius_normals_;
208 
209  float previous_explained_value;
210  int previous_duplicity_;
211  int previous_duplicity_complete_models_;
212  float previous_bad_info_;
213  float previous_unexplained_;
214 
215  int max_iterations_; //max iterations without improvement
216  SAModel best_seen_;
217  float initial_temp_;
218 
219  int n_cc_;
220  std::vector<std::vector<int> > cc_;
221 
222  void setPreviousBadInfo(float f)
223  {
224  previous_bad_info_ = f;
225  }
226 
227  float getPreviousBadInfo()
228  {
229  return previous_bad_info_;
230  }
231 
232  void setPreviousExplainedValue(float v)
233  {
234  previous_explained_value = v;
235  }
236 
237  void setPreviousDuplicity(int v)
238  {
239  previous_duplicity_ = v;
240  }
241 
242  void setPreviousDuplicityCM(int v)
243  {
244  previous_duplicity_complete_models_ = v;
245  }
246 
247  void setPreviousUnexplainedValue(float v)
248  {
249  previous_unexplained_ = v;
250  }
251 
252  float getPreviousUnexplainedValue()
253  {
254  return previous_unexplained_;
255  }
256 
257  float getExplainedValue()
258  {
259  return previous_explained_value;
260  }
261 
262  int getDuplicity()
263  {
264  return previous_duplicity_;
265  }
266 
267  int getDuplicityCM()
268  {
269  return previous_duplicity_complete_models_;
270  }
271 
272  void updateUnexplainedVector(std::vector<int> & unexplained_, std::vector<float> & unexplained_distances, std::vector<float> & unexplained_by_RM,
273  std::vector<int> & explained, std::vector<int> & explained_by_RM, float val)
274  {
275  {
276 
277  float add_to_unexplained = 0.f;
278 
279  for (size_t i = 0; i < unexplained_.size (); i++)
280  {
281 
282  bool prev_unexplained = (unexplained_by_RM[unexplained_[i]] > 0) && (explained_by_RM[unexplained_[i]] == 0);
283  unexplained_by_RM[unexplained_[i]] += val * unexplained_distances[i];
284 
285  if (val < 0) //the hypothesis is being removed
286  {
287  if (prev_unexplained)
288  {
289  //decrease by 1
290  add_to_unexplained -= unexplained_distances[i];
291  }
292  } else //the hypothesis is being added and unexplains unexplained_[i], so increase by 1 unless its explained by another hypothesis
293  {
294  if (explained_by_RM[unexplained_[i]] == 0)
295  add_to_unexplained += unexplained_distances[i];
296  }
297  }
298 
299  for (size_t i = 0; i < explained.size (); i++)
300  {
301  if (val < 0)
302  {
303  //the hypothesis is being removed, check that there are no points that become unexplained and have clutter unexplained hypotheses
304  if ((explained_by_RM[explained[i]] == 0) && (unexplained_by_RM[explained[i]] > 0))
305  {
306  add_to_unexplained += unexplained_by_RM[explained[i]]; //the points become unexplained
307  }
308  } else
309  {
310  //std::cout << "being added..." << add_to_unexplained << " " << unexplained_by_RM[explained[i]] << std::endl;
311  if ((explained_by_RM[explained[i]] == 1) && (unexplained_by_RM[explained[i]] > 0))
312  { //the only hypothesis explaining that point
313  add_to_unexplained -= unexplained_by_RM[explained[i]]; //the points are not unexplained any longer because this hypothesis explains them
314  }
315  }
316  }
317 
318  //std::cout << add_to_unexplained << std::endl;
319  previous_unexplained_ += add_to_unexplained;
320  }
321  }
322 
323  void updateExplainedVector(std::vector<int> & vec, std::vector<float> & vec_float, std::vector<int> & explained_,
324  std::vector<float> & explained_by_RM_distance_weighted, float sign)
325  {
326  float add_to_explained = 0.f;
327  int add_to_duplicity_ = 0;
328 
329  for (size_t i = 0; i < vec.size (); i++)
330  {
331  bool prev_dup = explained_[vec[i]] > 1;
332 
333  explained_[vec[i]] += static_cast<int> (sign);
334  explained_by_RM_distance_weighted[vec[i]] += vec_float[i] * sign;
335 
336  add_to_explained += vec_float[i] * sign;
337 
338  if ((explained_[vec[i]] > 1) && prev_dup)
339  { //its still a duplicate, we are adding
340  add_to_duplicity_ += static_cast<int> (sign); //so, just add or remove one
341  } else if ((explained_[vec[i]] == 1) && prev_dup)
342  { //if was duplicate before, now its not, remove 2, we are removing the hypothesis
343  add_to_duplicity_ -= 2;
344  } else if ((explained_[vec[i]] > 1) && !prev_dup)
345  { //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point
346  add_to_duplicity_ += 2;
347  }
348  }
349 
350  //update explained and duplicity values...
351  previous_explained_value += add_to_explained;
352  previous_duplicity_ += add_to_duplicity_;
353  }
354 
355  void updateCMDuplicity(std::vector<int> & vec, std::vector<int> & occupancy_vec, float sign) {
356  int add_to_duplicity_ = 0;
357  for (size_t i = 0; i < vec.size (); i++)
358  {
359  bool prev_dup = occupancy_vec[vec[i]] > 1;
360  occupancy_vec[vec[i]] += static_cast<int> (sign);
361  if ((occupancy_vec[vec[i]] > 1) && prev_dup)
362  { //its still a duplicate, we are adding
363  add_to_duplicity_ += static_cast<int> (sign); //so, just add or remove one
364  } else if ((occupancy_vec[vec[i]] == 1) && prev_dup)
365  { //if was duplicate before, now its not, remove 2, we are removing the hypothesis
366  add_to_duplicity_ -= 2;
367  } else if ((occupancy_vec[vec[i]] > 1) && !prev_dup)
368  { //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point
369  add_to_duplicity_ += 2;
370  }
371  }
372 
373  previous_duplicity_complete_models_ += add_to_duplicity_;
374  }
375 
376  float getTotalExplainedInformation(std::vector<int> & explained_, std::vector<float> & explained_by_RM_distance_weighted, int * duplicity_)
377  {
378  float explained_info = 0;
379  int duplicity = 0;
380 
381  for (size_t i = 0; i < explained_.size (); i++)
382  {
383  if (explained_[i] > 0)
384  explained_info += explained_by_RM_distance_weighted[i];
385 
386  if (explained_[i] > 1)
387  duplicity += explained_[i];
388  }
389 
390  *duplicity_ = duplicity;
391 
392  return explained_info;
393  }
394 
395  float getTotalBadInformation(std::vector<boost::shared_ptr<RecognitionModel> > & recog_models)
396  {
397  float bad_info = 0;
398  for (size_t i = 0; i < recog_models.size (); i++)
399  bad_info += recog_models[i]->outliers_weight_ * static_cast<float> (recog_models[i]->bad_information_);
400 
401  return bad_info;
402  }
403 
404  float getUnexplainedInformationInNeighborhood(std::vector<float> & unexplained, std::vector<int> & explained)
405  {
406  float unexplained_sum = 0.f;
407  for (size_t i = 0; i < unexplained.size (); i++)
408  {
409  if (unexplained[i] > 0 && explained[i] == 0)
410  unexplained_sum += unexplained[i];
411  }
412 
413  return unexplained_sum;
414  }
415 
416  //Performs smooth segmentation of the scene cloud and compute the model cues
417  void
418  initialize();
419 
420  mets::gol_type
421  evaluateSolution(const std::vector<bool> & active, int changed);
422 
423  bool
424  addModel(typename pcl::PointCloud<ModelT>::ConstPtr & model, typename pcl::PointCloud<ModelT>::ConstPtr & complete_model,
425  boost::shared_ptr<RecognitionModel> & recog_model);
426 
427  void
428  computeClutterCue(boost::shared_ptr<RecognitionModel> & recog_model);
429 
430  void
431  SAOptimize(std::vector<int> & cc_indices, std::vector<bool> & sub_solution);
432 
433  public:
435  {
436  resolution_ = 0.005f;
437  max_iterations_ = 5000;
438  regularizer_ = 1.f;
439  radius_normals_ = 0.01f;
440  initial_temp_ = 1000;
441  detect_clutter_ = true;
442  radius_neighborhood_GO_ = 0.03f;
443  clutter_regularizer_ = 5.f;
444  res_occupancy_grid_ = 0.01f;
445  w_occupied_multiple_cm_ = 4.f;
446  }
447 
448  void
449  verify();
450 
451  void setRadiusNormals(float r)
452  {
453  radius_normals_ = r;
454  }
455 
456  void setMaxIterations(int i)
457  {
458  max_iterations_ = i;
459  }
460 
461  void setInitialTemp(float t)
462  {
463  initial_temp_ = t;
464  }
465 
466  void setRegularizer(float r)
467  {
468  regularizer_ = r;
469  }
470 
471  void setRadiusClutter(float r)
472  {
473  radius_neighborhood_GO_ = r;
474  }
475 
476  void setClutterRegularizer(float cr)
477  {
478  clutter_regularizer_ = cr;
479  }
480 
481  void setDetectClutter(bool d)
482  {
483  detect_clutter_ = d;
484  }
485  };
486 }
487 
488 #ifdef PCL_NO_PRECOMPILE
489 #include <pcl/recognition/impl/hv/hv_go.hpp>
490 #endif
491 
492 #endif /* GO_H_ */
void setClutterRegularizer(float cr)
Definition: hv_go.h:476
void setRadiusNormals(float r)
Definition: hv_go.h:451
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:199
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void setRadiusClutter(float r)
Definition: hv_go.h:471
void setInitialTemp(float t)
Definition: hv_go.h:461
void setRegularizer(float r)
Definition: hv_go.h:466
Abstract class for hypotheses verification methods.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...
Definition: hv_go.h:27