11 #include <pcl/pcl_macros.h>
12 #include <pcl/recognition/hv/hypotheses_verification.h>
13 #include <pcl/common/common.h>
14 #include "metslib/mets.hh"
15 #include <pcl/features/normal_3d.h>
16 #include <boost/graph/graph_traits.hpp>
17 #include <boost/graph/adjacency_list.hpp>
26 template<
typename ModelT,
typename SceneT>
32 struct RecognitionModel
35 std::vector<int> explained_;
36 std::vector<float> explained_distances_;
37 std::vector<int> unexplained_in_neighborhood;
38 std::vector<float> unexplained_in_neighborhood_weights;
39 std::vector<int> outlier_indices_;
40 std::vector<int> complete_cloud_occupancy_indices_;
44 float outliers_weight_;
50 class SAModel:
public mets::evaluable_solution
53 std::vector<bool> solution_;
58 mets::gol_type cost_function()
const
63 void copy_from(
const mets::copyable& o)
65 const SAModel& s =
dynamic_cast<const SAModel&
> (o);
66 solution_ = s.solution_;
71 mets::gol_type what_if(
int ,
bool )
const
77 return static_cast<mets::gol_type
>(0);
80 mets::gol_type apply_and_evaluate(
int index,
bool val)
82 solution_[index] = val;
83 mets::gol_type sol = opt_->evaluateSolution (solution_, index);
88 void apply(
int ,
bool )
93 void unapply(
int index,
bool val)
95 solution_[index] = val;
97 cost_ = opt_->evaluateSolution (solution_, index);
99 void setSolution(std::vector<bool> & sol)
114 class move:
public mets::move
123 mets::gol_type evaluate(
const mets::feasible_solution& )
const
125 return static_cast<mets::gol_type
>(0);
128 mets::gol_type apply_and_evaluate(mets::feasible_solution& cs)
130 SAModel& model =
dynamic_cast<SAModel&
> (cs);
131 return model.apply_and_evaluate (index_, !model.solution_[index_]);
134 void apply(mets::feasible_solution& )
const
138 void unapply(mets::feasible_solution& s)
const
140 SAModel& model =
dynamic_cast<SAModel&
> (s);
141 model.unapply (index_, !model.solution_[index_]);
148 std::vector<move*> moves_m;
149 typedef typename std::vector<move*>::iterator iterator;
152 return moves_m.begin ();
156 return moves_m.end ();
159 move_manager(
int problem_size)
161 for (
int ii = 0; ii != problem_size; ++ii)
162 moves_m.push_back (
new move (ii));
168 for (iterator ii = begin (); ii != end (); ++ii)
172 void refresh(mets::feasible_solution& )
174 std::random_shuffle (moves_m.begin (), moves_m.end ());
193 std::vector<int> complete_cloud_occupancy_by_RM_;
194 float res_occupancy_grid_;
195 float w_occupied_multiple_cm_;
197 std::vector<int> explained_by_RM_;
198 std::vector<float> explained_by_RM_distance_weighted;
199 std::vector<float> unexplained_by_RM_neighboorhods;
200 std::vector<boost::shared_ptr<RecognitionModel> > recognition_models_;
201 std::vector<size_t> indices_;
204 float clutter_regularizer_;
205 bool detect_clutter_;
206 float radius_neighborhood_GO_;
207 float radius_normals_;
209 float previous_explained_value;
210 int previous_duplicity_;
211 int previous_duplicity_complete_models_;
212 float previous_bad_info_;
213 float previous_unexplained_;
220 std::vector<std::vector<int> > cc_;
222 void setPreviousBadInfo(
float f)
224 previous_bad_info_ = f;
227 float getPreviousBadInfo()
229 return previous_bad_info_;
232 void setPreviousExplainedValue(
float v)
234 previous_explained_value = v;
237 void setPreviousDuplicity(
int v)
239 previous_duplicity_ = v;
242 void setPreviousDuplicityCM(
int v)
244 previous_duplicity_complete_models_ = v;
247 void setPreviousUnexplainedValue(
float v)
249 previous_unexplained_ = v;
252 float getPreviousUnexplainedValue()
254 return previous_unexplained_;
257 float getExplainedValue()
259 return previous_explained_value;
264 return previous_duplicity_;
269 return previous_duplicity_complete_models_;
272 void updateUnexplainedVector(std::vector<int> & unexplained_, std::vector<float> & unexplained_distances, std::vector<float> & unexplained_by_RM,
273 std::vector<int> & explained, std::vector<int> & explained_by_RM,
float val)
277 float add_to_unexplained = 0.f;
279 for (
size_t i = 0; i < unexplained_.size (); i++)
282 bool prev_unexplained = (unexplained_by_RM[unexplained_[i]] > 0) && (explained_by_RM[unexplained_[i]] == 0);
283 unexplained_by_RM[unexplained_[i]] += val * unexplained_distances[i];
287 if (prev_unexplained)
290 add_to_unexplained -= unexplained_distances[i];
294 if (explained_by_RM[unexplained_[i]] == 0)
295 add_to_unexplained += unexplained_distances[i];
299 for (
size_t i = 0; i < explained.size (); i++)
304 if ((explained_by_RM[explained[i]] == 0) && (unexplained_by_RM[explained[i]] > 0))
306 add_to_unexplained += unexplained_by_RM[explained[i]];
311 if ((explained_by_RM[explained[i]] == 1) && (unexplained_by_RM[explained[i]] > 0))
313 add_to_unexplained -= unexplained_by_RM[explained[i]];
319 previous_unexplained_ += add_to_unexplained;
323 void updateExplainedVector(std::vector<int> & vec, std::vector<float> & vec_float, std::vector<int> & explained_,
324 std::vector<float> & explained_by_RM_distance_weighted,
float sign)
326 float add_to_explained = 0.f;
327 int add_to_duplicity_ = 0;
329 for (
size_t i = 0; i < vec.size (); i++)
331 bool prev_dup = explained_[vec[i]] > 1;
333 explained_[vec[i]] +=
static_cast<int> (sign);
334 explained_by_RM_distance_weighted[vec[i]] += vec_float[i] * sign;
336 add_to_explained += vec_float[i] * sign;
338 if ((explained_[vec[i]] > 1) && prev_dup)
340 add_to_duplicity_ +=
static_cast<int> (sign);
341 }
else if ((explained_[vec[i]] == 1) && prev_dup)
343 add_to_duplicity_ -= 2;
344 }
else if ((explained_[vec[i]] > 1) && !prev_dup)
346 add_to_duplicity_ += 2;
351 previous_explained_value += add_to_explained;
352 previous_duplicity_ += add_to_duplicity_;
355 void updateCMDuplicity(std::vector<int> & vec, std::vector<int> & occupancy_vec,
float sign) {
356 int add_to_duplicity_ = 0;
357 for (
size_t i = 0; i < vec.size (); i++)
359 bool prev_dup = occupancy_vec[vec[i]] > 1;
360 occupancy_vec[vec[i]] +=
static_cast<int> (sign);
361 if ((occupancy_vec[vec[i]] > 1) && prev_dup)
363 add_to_duplicity_ +=
static_cast<int> (sign);
364 }
else if ((occupancy_vec[vec[i]] == 1) && prev_dup)
366 add_to_duplicity_ -= 2;
367 }
else if ((occupancy_vec[vec[i]] > 1) && !prev_dup)
369 add_to_duplicity_ += 2;
373 previous_duplicity_complete_models_ += add_to_duplicity_;
376 float getTotalExplainedInformation(std::vector<int> & explained_, std::vector<float> & explained_by_RM_distance_weighted,
int * duplicity_)
378 float explained_info = 0;
381 for (
size_t i = 0; i < explained_.size (); i++)
383 if (explained_[i] > 0)
384 explained_info += explained_by_RM_distance_weighted[i];
386 if (explained_[i] > 1)
387 duplicity += explained_[i];
390 *duplicity_ = duplicity;
392 return explained_info;
395 float getTotalBadInformation(std::vector<boost::shared_ptr<RecognitionModel> > & recog_models)
398 for (
size_t i = 0; i < recog_models.size (); i++)
399 bad_info += recog_models[i]->outliers_weight_ * static_cast<float> (recog_models[i]->bad_information_);
404 float getUnexplainedInformationInNeighborhood(std::vector<float> & unexplained, std::vector<int> & explained)
406 float unexplained_sum = 0.f;
407 for (
size_t i = 0; i < unexplained.size (); i++)
409 if (unexplained[i] > 0 && explained[i] == 0)
410 unexplained_sum += unexplained[i];
413 return unexplained_sum;
421 evaluateSolution(
const std::vector<bool> & active,
int changed);
425 boost::shared_ptr<RecognitionModel> & recog_model);
428 computeClutterCue(boost::shared_ptr<RecognitionModel> & recog_model);
431 SAOptimize(std::vector<int> & cc_indices, std::vector<bool> & sub_solution);
436 resolution_ = 0.005f;
437 max_iterations_ = 5000;
439 radius_normals_ = 0.01f;
440 initial_temp_ = 1000;
441 detect_clutter_ =
true;
442 radius_neighborhood_GO_ = 0.03f;
443 clutter_regularizer_ = 5.f;
444 res_occupancy_grid_ = 0.01f;
445 w_occupied_multiple_cm_ = 4.f;
473 radius_neighborhood_GO_ = r;
478 clutter_regularizer_ = cr;
488 #ifdef PCL_NO_PRECOMPILE
489 #include <pcl/recognition/impl/hv/hv_go.hpp>
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void setClutterRegularizer(float cr)
boost::shared_ptr< PointCloud< PointT > > Ptr
void setRadiusNormals(float r)
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void setDetectClutter(bool d)
void setRadiusClutter(float r)
void setInitialTemp(float t)
void setRegularizer(float r)
Abstract class for hypotheses verification methods.
GlobalHypothesesVerification()
void setMaxIterations(int i)
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...