11 #include <pcl/pcl_macros.h>
12 #include <pcl/recognition/hv/hypotheses_verification.h>
13 #include <pcl/common/common.h>
14 #include <pcl/recognition/3rdparty/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)
104 void setOptimizer(SAOptimizerT * opt)
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;
453 res_occupancy_grid_ = r;
478 radius_neighborhood_GO_ = r;
483 clutter_regularizer_ = cr;
493 #ifdef PCL_NO_PRECOMPILE
494 #include <pcl/recognition/impl/hv/hv_go.hpp>
boost::shared_ptr< PointCloud< PointT > > Ptr
void setClutterRegularizer(float cr)
This file defines compatibility wrappers for low level I/O functions.
void setResolutionOccupancyGrid(float r)
void setRadiusNormals(float r)
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
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...