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