Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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 <metslib/mets.hh>
23#include <pcl/features/normal_3d.h>
24
25#include <memory>
26
27namespace pcl
28{
29
30 /** \brief A hypothesis verification method proposed in
31 * "A Global Hypotheses Verification Method for 3D Object Recognition", A. Aldoma and F. Tombari and L. Di Stefano and Markus Vincze, ECCV 2012
32 * \author Aitor Aldoma
33 * \ingroup recognition
34 */
35 template<typename ModelT, typename SceneT>
36 class PCL_EXPORTS GlobalHypothesesVerification: public HypothesisVerification<ModelT, 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 auto& 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 auto& 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 auto& 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 (auto 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
191 using HypothesisVerification<ModelT, SceneT>::mask_;
192 using HypothesisVerification<ModelT, SceneT>::scene_cloud_downsampled_;
193 using HypothesisVerification<ModelT, SceneT>::scene_downsampled_tree_;
194 using HypothesisVerification<ModelT, SceneT>::visible_models_;
195 using HypothesisVerification<ModelT, SceneT>::complete_models_;
196 using HypothesisVerification<ModelT, SceneT>::resolution_;
197 using HypothesisVerification<ModelT, SceneT>::inliers_threshold_;
198
199 //class attributes
200 using NormalEstimator_ = pcl::NormalEstimation<SceneT, pcl::Normal>;
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
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
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
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...
Definition hv_go.h:37
void setClutterRegularizer(float cr)
Definition hv_go.h:491
void setResolutionOccupancyGrid(float r)
Definition hv_go.h:461
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
shared_ptr< const PointCloud< PointT > > ConstPtr
Defines all the PCL and non-PCL macros used.