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