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 <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>
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...
void setClutterRegularizer(float cr)
void setMaxIterations(int i)
GlobalHypothesesVerification()
void setRegularizer(float r)
void setRadiusNormals(float r)
void setInitialTemp(float t)
void setDetectClutter(bool d)
void setRadiusClutter(float r)
void setResolutionOccupancyGrid(float r)
Abstract class for hypotheses verification methods.
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Defines all the PCL and non-PCL macros used.