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