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>
23 #include <metslib/mets.hh>
24 #include <pcl/features/normal_3d.h>
35 template<
typename ModelT,
typename SceneT>
41 struct RecognitionModel
44 std::vector<int> explained_;
45 std::vector<float> explained_distances_;
46 std::vector<int> unexplained_in_neighborhood;
47 std::vector<float> unexplained_in_neighborhood_weights;
48 std::vector<int> outlier_indices_;
49 std::vector<int> complete_cloud_occupancy_indices_;
53 float outliers_weight_;
58 using RecognitionModelPtr = std::shared_ptr<RecognitionModel>;
61 class SAModel:
public mets::evaluable_solution
64 std::vector<bool> solution_;
69 mets::gol_type cost_function()
const override
74 void copy_from(
const mets::copyable& o)
override
76 const SAModel& s =
dynamic_cast<const SAModel&
> (o);
77 solution_ = s.solution_;
82 mets::gol_type what_if(
int ,
bool )
const
88 return static_cast<mets::gol_type
>(0);
91 mets::gol_type apply_and_evaluate(
int index,
bool val)
93 solution_[index] = val;
94 mets::gol_type sol = opt_->evaluateSolution (solution_, index);
99 void apply(
int ,
bool )
104 void unapply(
int index,
bool val)
106 solution_[index] = val;
108 cost_ = opt_->evaluateSolution (solution_, index);
110 void setSolution(std::vector<bool> & sol)
125 class move:
public mets::move
134 mets::gol_type evaluate(
const mets::feasible_solution& )
const override
136 return static_cast<mets::gol_type
>(0);
139 mets::gol_type apply_and_evaluate(mets::feasible_solution& cs)
141 SAModel& model =
dynamic_cast<SAModel&
> (cs);
142 return model.apply_and_evaluate (index_, !model.solution_[index_]);
145 void apply(mets::feasible_solution& )
const override
149 void unapply(mets::feasible_solution& s)
const
151 SAModel& model =
dynamic_cast<SAModel&
> (s);
152 model.unapply (index_, !model.solution_[index_]);
159 std::vector<move*> moves_m;
160 using iterator =
typename std::vector<move *>::iterator;
163 return moves_m.begin ();
167 return moves_m.end ();
170 move_manager(
int problem_size)
172 for (
int ii = 0; ii != problem_size; ++ii)
173 moves_m.push_back (
new move (ii));
179 for (iterator ii = begin (); ii != end (); ++ii)
183 void refresh(mets::feasible_solution& )
185 std::shuffle (moves_m.begin (), moves_m.end (), std::mt19937(std::random_device()()));
204 std::vector<int> complete_cloud_occupancy_by_RM_;
205 float res_occupancy_grid_;
206 float w_occupied_multiple_cm_;
208 std::vector<int> explained_by_RM_;
209 std::vector<float> explained_by_RM_distance_weighted;
210 std::vector<float> unexplained_by_RM_neighboorhods;
211 std::vector<RecognitionModelPtr> recognition_models_;
212 std::vector<std::size_t> indices_;
215 float clutter_regularizer_;
216 bool detect_clutter_;
217 float radius_neighborhood_GO_;
218 float radius_normals_;
220 float previous_explained_value;
221 int previous_duplicity_;
222 int previous_duplicity_complete_models_;
223 float previous_bad_info_;
224 float previous_unexplained_;
231 std::vector<std::vector<int> > cc_;
233 void setPreviousBadInfo(
float f)
235 previous_bad_info_ = f;
238 float getPreviousBadInfo()
240 return previous_bad_info_;
243 void setPreviousExplainedValue(
float v)
245 previous_explained_value = v;
248 void setPreviousDuplicity(
int v)
250 previous_duplicity_ = v;
253 void setPreviousDuplicityCM(
int v)
255 previous_duplicity_complete_models_ = v;
258 void setPreviousUnexplainedValue(
float v)
260 previous_unexplained_ = v;
263 float getPreviousUnexplainedValue()
265 return previous_unexplained_;
268 float getExplainedValue()
270 return previous_explained_value;
275 return previous_duplicity_;
280 return previous_duplicity_complete_models_;
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)
288 float add_to_unexplained = 0.f;
290 for (std::size_t i = 0; i < unexplained_.size (); i++)
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];
298 if (prev_unexplained)
301 add_to_unexplained -= unexplained_distances[i];
305 if (explained_by_RM[unexplained_[i]] == 0)
306 add_to_unexplained += unexplained_distances[i];
310 for (
const int &i : explained)
315 if ((explained_by_RM[i] == 0) && (unexplained_by_RM[i] > 0))
317 add_to_unexplained += unexplained_by_RM[i];
322 if ((explained_by_RM[i] == 1) && (unexplained_by_RM[i] > 0))
324 add_to_unexplained -= unexplained_by_RM[i];
330 previous_unexplained_ += add_to_unexplained;
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)
337 float add_to_explained = 0.f;
338 int add_to_duplicity_ = 0;
340 for (std::size_t i = 0; i < vec.size (); i++)
342 bool prev_dup = explained_[vec[i]] > 1;
344 explained_[vec[i]] +=
static_cast<int> (sign);
345 explained_by_RM_distance_weighted[vec[i]] += vec_float[i] * sign;
347 add_to_explained += vec_float[i] * sign;
349 if ((explained_[vec[i]] > 1) && prev_dup)
351 add_to_duplicity_ +=
static_cast<int> (sign);
352 }
else if ((explained_[vec[i]] == 1) && prev_dup)
354 add_to_duplicity_ -= 2;
355 }
else if ((explained_[vec[i]] > 1) && !prev_dup)
357 add_to_duplicity_ += 2;
362 previous_explained_value += add_to_explained;
363 previous_duplicity_ += add_to_duplicity_;
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)
370 bool prev_dup = occupancy_vec[i] > 1;
371 occupancy_vec[i] +=
static_cast<int> (sign);
372 if ((occupancy_vec[i] > 1) && prev_dup)
374 add_to_duplicity_ +=
static_cast<int> (sign);
375 }
else if ((occupancy_vec[i] == 1) && prev_dup)
377 add_to_duplicity_ -= 2;
378 }
else if ((occupancy_vec[i] > 1) && !prev_dup)
380 add_to_duplicity_ += 2;
384 previous_duplicity_complete_models_ += add_to_duplicity_;
387 float getTotalExplainedInformation(std::vector<int> & explained_, std::vector<float> & explained_by_RM_distance_weighted,
int * duplicity_)
389 float explained_info = 0;
392 for (std::size_t i = 0; i < explained_.size (); i++)
394 if (explained_[i] > 0)
395 explained_info += explained_by_RM_distance_weighted[i];
397 if (explained_[i] > 1)
398 duplicity += explained_[i];
401 *duplicity_ = duplicity;
403 return explained_info;
406 float getTotalBadInformation(std::vector<RecognitionModelPtr> & recog_models)
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_);
415 float getUnexplainedInformationInNeighborhood(std::vector<float> & unexplained, std::vector<int> & explained)
417 float unexplained_sum = 0.f;
418 for (std::size_t i = 0; i < unexplained.size (); i++)
420 if (unexplained[i] > 0 && explained[i] == 0)
421 unexplained_sum += unexplained[i];
424 return unexplained_sum;
432 evaluateSolution(
const std::vector<bool> & active,
int changed);
438 computeClutterCue(RecognitionModelPtr & recog_model);
441 SAOptimize(std::vector<int> & cc_indices, std::vector<bool> & sub_solution);
446 resolution_ = 0.005f;
447 max_iterations_ = 5000;
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;
463 res_occupancy_grid_ = r;
488 radius_neighborhood_GO_ = r;
493 clutter_regularizer_ = cr;
503 #ifdef PCL_NO_PRECOMPILE
504 #include <pcl/recognition/impl/hv/hv_go.hpp>