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 ();
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_;
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)
339 for (std::size_t i = 0; i <
vec.
size (); i++)
343 explained_[
vec[i]] +=
static_cast<int> (
sign);
367 for (
const int &i :
vec)
386 float getTotalExplainedInformation(std::vector<int> & explained_, std::vector<float> & explained_by_RM_distance_weighted,
int *
duplicity_)
391 for (std::size_t i = 0; i < explained_.size (); i++)
393 if (explained_[i] > 0)
396 if (explained_[i] > 1)
405 float getTotalBadInformation(std::vector<RecognitionModelPtr> &
recog_models)
414 float getUnexplainedInformationInNeighborhood(std::vector<float> &
unexplained, std::vector<int> &
explained)
431 evaluateSolution(
const std::vector<bool> &
active,
int changed);
437 computeClutterCue(RecognitionModelPtr &
recog_model);
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;