12 #include <boost/graph/graph_traits.hpp>
13 #include <boost/graph/adjacency_list.hpp>
16 #include <boost/random/linear_congruential.hpp>
17 #include <boost/random/mersenne_twister.hpp>
18 #include <boost/random/variate_generator.hpp>
21 #include <pcl/recognition/hv/hypotheses_verification.h>
22 #include <pcl/recognition/3rdparty/metslib/mets.hh>
23 #include <pcl/features/normal_3d.h>
34 template<
typename ModelT,
typename SceneT>
40 struct RecognitionModel
43 std::vector<int> explained_;
44 std::vector<float> explained_distances_;
45 std::vector<int> unexplained_in_neighborhood;
46 std::vector<float> unexplained_in_neighborhood_weights;
47 std::vector<int> outlier_indices_;
48 std::vector<int> complete_cloud_occupancy_indices_;
52 float outliers_weight_;
57 using RecognitionModelPtr = std::shared_ptr<RecognitionModel>;
60 class SAModel:
public mets::evaluable_solution
63 std::vector<bool> solution_;
68 mets::gol_type cost_function()
const override
73 void copy_from(
const mets::copyable& o)
override
75 const SAModel& s =
dynamic_cast<const SAModel&
> (o);
76 solution_ = s.solution_;
81 mets::gol_type what_if(
int ,
bool )
const
87 return static_cast<mets::gol_type
>(0);
90 mets::gol_type apply_and_evaluate(
int index,
bool val)
92 solution_[index] = val;
93 mets::gol_type sol = opt_->evaluateSolution (solution_, index);
98 void apply(
int ,
bool )
103 void unapply(
int index,
bool val)
105 solution_[index] = val;
107 cost_ = opt_->evaluateSolution (solution_, index);
109 void setSolution(std::vector<bool> & sol)
124 class move:
public mets::move
133 mets::gol_type evaluate(
const mets::feasible_solution& )
const override
135 return static_cast<mets::gol_type
>(0);
138 mets::gol_type apply_and_evaluate(mets::feasible_solution& cs)
140 SAModel& model =
dynamic_cast<SAModel&
> (cs);
141 return model.apply_and_evaluate (index_, !model.solution_[index_]);
144 void apply(mets::feasible_solution& )
const override
148 void unapply(mets::feasible_solution& s)
const
150 SAModel& model =
dynamic_cast<SAModel&
> (s);
151 model.unapply (index_, !model.solution_[index_]);
158 std::vector<move*> moves_m;
159 using iterator =
typename std::vector<move *>::iterator;
162 return moves_m.begin ();
166 return moves_m.end ();
169 move_manager(
int problem_size)
171 for (
int ii = 0; ii != problem_size; ++ii)
172 moves_m.push_back (
new move (ii));
178 for (iterator ii = begin (); ii != end (); ++ii)
182 void refresh(mets::feasible_solution& )
184 std::shuffle (moves_m.begin (), moves_m.end (), std::mt19937(std::random_device()()));
203 std::vector<int> complete_cloud_occupancy_by_RM_;
204 float res_occupancy_grid_;
205 float w_occupied_multiple_cm_;
207 std::vector<int> explained_by_RM_;
208 std::vector<float> explained_by_RM_distance_weighted;
209 std::vector<float> unexplained_by_RM_neighboorhods;
210 std::vector<RecognitionModelPtr> recognition_models_;
211 std::vector<std::size_t> indices_;
214 float clutter_regularizer_;
215 bool detect_clutter_;
216 float radius_neighborhood_GO_;
217 float radius_normals_;
219 float previous_explained_value;
220 int previous_duplicity_;
221 int previous_duplicity_complete_models_;
222 float previous_bad_info_;
223 float previous_unexplained_;
230 std::vector<std::vector<int> > cc_;
232 void setPreviousBadInfo(
float f)
234 previous_bad_info_ = f;
237 float getPreviousBadInfo()
239 return previous_bad_info_;
242 void setPreviousExplainedValue(
float v)
244 previous_explained_value = v;
247 void setPreviousDuplicity(
int v)
249 previous_duplicity_ = v;
252 void setPreviousDuplicityCM(
int v)
254 previous_duplicity_complete_models_ = v;
257 void setPreviousUnexplainedValue(
float v)
259 previous_unexplained_ = v;
262 float getPreviousUnexplainedValue()
264 return previous_unexplained_;
267 float getExplainedValue()
269 return previous_explained_value;
274 return previous_duplicity_;
279 return previous_duplicity_complete_models_;
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)
287 float add_to_unexplained = 0.f;
289 for (std::size_t i = 0; i < unexplained_.size (); i++)
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];
297 if (prev_unexplained)
300 add_to_unexplained -= unexplained_distances[i];
304 if (explained_by_RM[unexplained_[i]] == 0)
305 add_to_unexplained += unexplained_distances[i];
309 for (
const int &i : explained)
314 if ((explained_by_RM[i] == 0) && (unexplained_by_RM[i] > 0))
316 add_to_unexplained += unexplained_by_RM[i];
321 if ((explained_by_RM[i] == 1) && (unexplained_by_RM[i] > 0))
323 add_to_unexplained -= unexplained_by_RM[i];
329 previous_unexplained_ += add_to_unexplained;
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)
336 float add_to_explained = 0.f;
337 int add_to_duplicity_ = 0;
339 for (std::size_t i = 0; i < vec.size (); i++)
341 bool prev_dup = explained_[vec[i]] > 1;
343 explained_[vec[i]] +=
static_cast<int> (sign);
344 explained_by_RM_distance_weighted[vec[i]] += vec_float[i] * sign;
346 add_to_explained += vec_float[i] * sign;
348 if ((explained_[vec[i]] > 1) && prev_dup)
350 add_to_duplicity_ +=
static_cast<int> (sign);
351 }
else if ((explained_[vec[i]] == 1) && prev_dup)
353 add_to_duplicity_ -= 2;
354 }
else if ((explained_[vec[i]] > 1) && !prev_dup)
356 add_to_duplicity_ += 2;
361 previous_explained_value += add_to_explained;
362 previous_duplicity_ += add_to_duplicity_;
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)
369 bool prev_dup = occupancy_vec[i] > 1;
370 occupancy_vec[i] +=
static_cast<int> (sign);
371 if ((occupancy_vec[i] > 1) && prev_dup)
373 add_to_duplicity_ +=
static_cast<int> (sign);
374 }
else if ((occupancy_vec[i] == 1) && prev_dup)
376 add_to_duplicity_ -= 2;
377 }
else if ((occupancy_vec[i] > 1) && !prev_dup)
379 add_to_duplicity_ += 2;
383 previous_duplicity_complete_models_ += add_to_duplicity_;
386 float getTotalExplainedInformation(std::vector<int> & explained_, std::vector<float> & explained_by_RM_distance_weighted,
int * duplicity_)
388 float explained_info = 0;
391 for (std::size_t i = 0; i < explained_.size (); i++)
393 if (explained_[i] > 0)
394 explained_info += explained_by_RM_distance_weighted[i];
396 if (explained_[i] > 1)
397 duplicity += explained_[i];
400 *duplicity_ = duplicity;
402 return explained_info;
405 float getTotalBadInformation(std::vector<RecognitionModelPtr> & recog_models)
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_);
414 float getUnexplainedInformationInNeighborhood(std::vector<float> & unexplained, std::vector<int> & explained)
416 float unexplained_sum = 0.f;
417 for (std::size_t i = 0; i < unexplained.size (); i++)
419 if (unexplained[i] > 0 && explained[i] == 0)
420 unexplained_sum += unexplained[i];
423 return unexplained_sum;
431 evaluateSolution(
const std::vector<bool> & active,
int changed);
437 computeClutterCue(RecognitionModelPtr & recog_model);
440 SAOptimize(std::vector<int> & cc_indices, std::vector<bool> & sub_solution);
445 resolution_ = 0.005f;
446 max_iterations_ = 5000;
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;
462 res_occupancy_grid_ = r;
487 radius_neighborhood_GO_ = r;
492 clutter_regularizer_ = cr;
502 #ifdef PCL_NO_PRECOMPILE
503 #include <pcl/recognition/impl/hv/hv_go.hpp>