Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * go.h 00003 * 00004 * Created on: Jun 4, 2012 00005 * Author: aitor 00006 */ 00007 00008 #ifndef GO_H_ 00009 #define GO_H_ 00010 00011 #include <pcl/pcl_macros.h> 00012 #include <pcl/recognition/hv/hypotheses_verification.h> 00013 #include <pcl/common/common.h> 00014 #include "pcl/recognition/3rdparty/metslib/mets.hh" 00015 #include <pcl/features/normal_3d.h> 00016 #include <boost/graph/graph_traits.hpp> 00017 #include <boost/graph/adjacency_list.hpp> 00018 00019 namespace pcl 00020 { 00021 00022 /** \brief A hypothesis verification method proposed in 00023 * "A Global Hypotheses Verification Method for 3D Object Recognition", A. Aldoma and F. Tombari and L. Di Stefano and Markus Vincze, ECCV 2012 00024 * \author Aitor Aldoma 00025 */ 00026 template<typename ModelT, typename SceneT> 00027 class PCL_EXPORTS GlobalHypothesesVerification: public HypothesisVerification<ModelT, SceneT> 00028 { 00029 private: 00030 00031 //Helper classes 00032 struct RecognitionModel 00033 { 00034 public: 00035 std::vector<int> explained_; //indices vector referencing explained_by_RM_ 00036 std::vector<float> explained_distances_; //closest distances to the scene for point i 00037 std::vector<int> unexplained_in_neighborhood; //indices vector referencing unexplained_by_RM_neighboorhods 00038 std::vector<float> unexplained_in_neighborhood_weights; //weights for the points not being explained in the neighborhood of a hypothesis 00039 std::vector<int> outlier_indices_; //outlier indices of this model 00040 std::vector<int> complete_cloud_occupancy_indices_; 00041 typename pcl::PointCloud<ModelT>::Ptr cloud_; 00042 typename pcl::PointCloud<ModelT>::Ptr complete_cloud_; 00043 int bad_information_; 00044 float outliers_weight_; 00045 pcl::PointCloud<pcl::Normal>::Ptr normals_; 00046 int id_; 00047 }; 00048 00049 typedef GlobalHypothesesVerification<ModelT, SceneT> SAOptimizerT; 00050 class SAModel: public mets::evaluable_solution 00051 { 00052 public: 00053 std::vector<bool> solution_; 00054 SAOptimizerT * opt_; 00055 mets::gol_type cost_; 00056 00057 //Evaluates the current solution 00058 mets::gol_type cost_function() const 00059 { 00060 return cost_; 00061 } 00062 00063 void copy_from(const mets::copyable& o) 00064 { 00065 const SAModel& s = dynamic_cast<const SAModel&> (o); 00066 solution_ = s.solution_; 00067 opt_ = s.opt_; 00068 cost_ = s.cost_; 00069 } 00070 00071 mets::gol_type what_if(int /*index*/, bool /*val*/) const 00072 { 00073 /*std::vector<bool> tmp (solution_); 00074 tmp[index] = val; 00075 mets::gol_type sol = opt_->evaluateSolution (solution_, index); //evaluate without updating status 00076 return sol;*/ 00077 return static_cast<mets::gol_type>(0); 00078 } 00079 00080 mets::gol_type apply_and_evaluate(int index, bool val) 00081 { 00082 solution_[index] = val; 00083 mets::gol_type sol = opt_->evaluateSolution (solution_, index); //this will update the state of the solution 00084 cost_ = sol; 00085 return sol; 00086 } 00087 00088 void apply(int /*index*/, bool /*val*/) 00089 { 00090 00091 } 00092 00093 void unapply(int index, bool val) 00094 { 00095 solution_[index] = val; 00096 //update optimizer solution 00097 cost_ = opt_->evaluateSolution (solution_, index); //this will udpate the cost function in opt_ 00098 } 00099 void setSolution(std::vector<bool> & sol) 00100 { 00101 solution_ = sol; 00102 } 00103 00104 void setOptimizer(SAOptimizerT * opt) 00105 { 00106 opt_ = opt; 00107 } 00108 }; 00109 00110 /* 00111 * Represents a move, deactivate a hypothesis 00112 */ 00113 00114 class move: public mets::move 00115 { 00116 int index_; 00117 public: 00118 move(int i) : 00119 index_ (i) 00120 { 00121 } 00122 00123 mets::gol_type evaluate(const mets::feasible_solution& /*cs*/) const 00124 { 00125 return static_cast<mets::gol_type>(0); 00126 } 00127 00128 mets::gol_type apply_and_evaluate(mets::feasible_solution& cs) 00129 { 00130 SAModel& model = dynamic_cast<SAModel&> (cs); 00131 return model.apply_and_evaluate (index_, !model.solution_[index_]); 00132 } 00133 00134 void apply(mets::feasible_solution& /*s*/) const 00135 { 00136 } 00137 00138 void unapply(mets::feasible_solution& s) const 00139 { 00140 SAModel& model = dynamic_cast<SAModel&> (s); 00141 model.unapply (index_, !model.solution_[index_]); 00142 } 00143 }; 00144 00145 class move_manager 00146 { 00147 public: 00148 std::vector<move*> moves_m; 00149 typedef typename std::vector<move*>::iterator iterator; 00150 iterator begin() 00151 { 00152 return moves_m.begin (); 00153 } 00154 iterator end() 00155 { 00156 return moves_m.end (); 00157 } 00158 00159 move_manager(int problem_size) 00160 { 00161 for (int ii = 0; ii != problem_size; ++ii) 00162 moves_m.push_back (new move (ii)); 00163 } 00164 00165 ~move_manager() 00166 { 00167 // delete all moves 00168 for (iterator ii = begin (); ii != end (); ++ii) 00169 delete (*ii); 00170 } 00171 00172 void refresh(mets::feasible_solution& /*s*/) 00173 { 00174 std::random_shuffle (moves_m.begin (), moves_m.end ()); 00175 } 00176 00177 }; 00178 00179 //inherited class attributes 00180 using HypothesisVerification<ModelT, SceneT>::mask_; 00181 using HypothesisVerification<ModelT, SceneT>::scene_cloud_downsampled_; 00182 using HypothesisVerification<ModelT, SceneT>::scene_downsampled_tree_; 00183 using HypothesisVerification<ModelT, SceneT>::visible_models_; 00184 using HypothesisVerification<ModelT, SceneT>::complete_models_; 00185 using HypothesisVerification<ModelT, SceneT>::resolution_; 00186 using HypothesisVerification<ModelT, SceneT>::inliers_threshold_; 00187 00188 //class attributes 00189 typedef typename pcl::NormalEstimation<SceneT, pcl::Normal> NormalEstimator_; 00190 pcl::PointCloud<pcl::Normal>::Ptr scene_normals_; 00191 pcl::PointCloud<pcl::PointXYZI>::Ptr clusters_cloud_; 00192 00193 std::vector<int> complete_cloud_occupancy_by_RM_; 00194 float res_occupancy_grid_; 00195 float w_occupied_multiple_cm_; 00196 00197 std::vector<int> explained_by_RM_; //represents the points of scene_cloud_ that are explained by the recognition models 00198 std::vector<float> explained_by_RM_distance_weighted; //represents the points of scene_cloud_ that are explained by the recognition models 00199 std::vector<float> unexplained_by_RM_neighboorhods; //represents the points of scene_cloud_ that are not explained by the active hypotheses in the neighboorhod of the recognition models 00200 std::vector<boost::shared_ptr<RecognitionModel> > recognition_models_; 00201 std::vector<size_t> indices_; 00202 00203 float regularizer_; 00204 float clutter_regularizer_; 00205 bool detect_clutter_; 00206 float radius_neighborhood_GO_; 00207 float radius_normals_; 00208 00209 float previous_explained_value; 00210 int previous_duplicity_; 00211 int previous_duplicity_complete_models_; 00212 float previous_bad_info_; 00213 float previous_unexplained_; 00214 00215 int max_iterations_; //max iterations without improvement 00216 SAModel best_seen_; 00217 float initial_temp_; 00218 00219 int n_cc_; 00220 std::vector<std::vector<int> > cc_; 00221 00222 void setPreviousBadInfo(float f) 00223 { 00224 previous_bad_info_ = f; 00225 } 00226 00227 float getPreviousBadInfo() 00228 { 00229 return previous_bad_info_; 00230 } 00231 00232 void setPreviousExplainedValue(float v) 00233 { 00234 previous_explained_value = v; 00235 } 00236 00237 void setPreviousDuplicity(int v) 00238 { 00239 previous_duplicity_ = v; 00240 } 00241 00242 void setPreviousDuplicityCM(int v) 00243 { 00244 previous_duplicity_complete_models_ = v; 00245 } 00246 00247 void setPreviousUnexplainedValue(float v) 00248 { 00249 previous_unexplained_ = v; 00250 } 00251 00252 float getPreviousUnexplainedValue() 00253 { 00254 return previous_unexplained_; 00255 } 00256 00257 float getExplainedValue() 00258 { 00259 return previous_explained_value; 00260 } 00261 00262 int getDuplicity() 00263 { 00264 return previous_duplicity_; 00265 } 00266 00267 int getDuplicityCM() 00268 { 00269 return previous_duplicity_complete_models_; 00270 } 00271 00272 void updateUnexplainedVector(std::vector<int> & unexplained_, std::vector<float> & unexplained_distances, std::vector<float> & unexplained_by_RM, 00273 std::vector<int> & explained, std::vector<int> & explained_by_RM, float val) 00274 { 00275 { 00276 00277 float add_to_unexplained = 0.f; 00278 00279 for (size_t i = 0; i < unexplained_.size (); i++) 00280 { 00281 00282 bool prev_unexplained = (unexplained_by_RM[unexplained_[i]] > 0) && (explained_by_RM[unexplained_[i]] == 0); 00283 unexplained_by_RM[unexplained_[i]] += val * unexplained_distances[i]; 00284 00285 if (val < 0) //the hypothesis is being removed 00286 { 00287 if (prev_unexplained) 00288 { 00289 //decrease by 1 00290 add_to_unexplained -= unexplained_distances[i]; 00291 } 00292 } else //the hypothesis is being added and unexplains unexplained_[i], so increase by 1 unless its explained by another hypothesis 00293 { 00294 if (explained_by_RM[unexplained_[i]] == 0) 00295 add_to_unexplained += unexplained_distances[i]; 00296 } 00297 } 00298 00299 for (size_t i = 0; i < explained.size (); i++) 00300 { 00301 if (val < 0) 00302 { 00303 //the hypothesis is being removed, check that there are no points that become unexplained and have clutter unexplained hypotheses 00304 if ((explained_by_RM[explained[i]] == 0) && (unexplained_by_RM[explained[i]] > 0)) 00305 { 00306 add_to_unexplained += unexplained_by_RM[explained[i]]; //the points become unexplained 00307 } 00308 } else 00309 { 00310 //std::cout << "being added..." << add_to_unexplained << " " << unexplained_by_RM[explained[i]] << std::endl; 00311 if ((explained_by_RM[explained[i]] == 1) && (unexplained_by_RM[explained[i]] > 0)) 00312 { //the only hypothesis explaining that point 00313 add_to_unexplained -= unexplained_by_RM[explained[i]]; //the points are not unexplained any longer because this hypothesis explains them 00314 } 00315 } 00316 } 00317 00318 //std::cout << add_to_unexplained << std::endl; 00319 previous_unexplained_ += add_to_unexplained; 00320 } 00321 } 00322 00323 void updateExplainedVector(std::vector<int> & vec, std::vector<float> & vec_float, std::vector<int> & explained_, 00324 std::vector<float> & explained_by_RM_distance_weighted, float sign) 00325 { 00326 float add_to_explained = 0.f; 00327 int add_to_duplicity_ = 0; 00328 00329 for (size_t i = 0; i < vec.size (); i++) 00330 { 00331 bool prev_dup = explained_[vec[i]] > 1; 00332 00333 explained_[vec[i]] += static_cast<int> (sign); 00334 explained_by_RM_distance_weighted[vec[i]] += vec_float[i] * sign; 00335 00336 add_to_explained += vec_float[i] * sign; 00337 00338 if ((explained_[vec[i]] > 1) && prev_dup) 00339 { //its still a duplicate, we are adding 00340 add_to_duplicity_ += static_cast<int> (sign); //so, just add or remove one 00341 } else if ((explained_[vec[i]] == 1) && prev_dup) 00342 { //if was duplicate before, now its not, remove 2, we are removing the hypothesis 00343 add_to_duplicity_ -= 2; 00344 } else if ((explained_[vec[i]] > 1) && !prev_dup) 00345 { //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point 00346 add_to_duplicity_ += 2; 00347 } 00348 } 00349 00350 //update explained and duplicity values... 00351 previous_explained_value += add_to_explained; 00352 previous_duplicity_ += add_to_duplicity_; 00353 } 00354 00355 void updateCMDuplicity(std::vector<int> & vec, std::vector<int> & occupancy_vec, float sign) { 00356 int add_to_duplicity_ = 0; 00357 for (size_t i = 0; i < vec.size (); i++) 00358 { 00359 bool prev_dup = occupancy_vec[vec[i]] > 1; 00360 occupancy_vec[vec[i]] += static_cast<int> (sign); 00361 if ((occupancy_vec[vec[i]] > 1) && prev_dup) 00362 { //its still a duplicate, we are adding 00363 add_to_duplicity_ += static_cast<int> (sign); //so, just add or remove one 00364 } else if ((occupancy_vec[vec[i]] == 1) && prev_dup) 00365 { //if was duplicate before, now its not, remove 2, we are removing the hypothesis 00366 add_to_duplicity_ -= 2; 00367 } else if ((occupancy_vec[vec[i]] > 1) && !prev_dup) 00368 { //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point 00369 add_to_duplicity_ += 2; 00370 } 00371 } 00372 00373 previous_duplicity_complete_models_ += add_to_duplicity_; 00374 } 00375 00376 float getTotalExplainedInformation(std::vector<int> & explained_, std::vector<float> & explained_by_RM_distance_weighted, int * duplicity_) 00377 { 00378 float explained_info = 0; 00379 int duplicity = 0; 00380 00381 for (size_t i = 0; i < explained_.size (); i++) 00382 { 00383 if (explained_[i] > 0) 00384 explained_info += explained_by_RM_distance_weighted[i]; 00385 00386 if (explained_[i] > 1) 00387 duplicity += explained_[i]; 00388 } 00389 00390 *duplicity_ = duplicity; 00391 00392 return explained_info; 00393 } 00394 00395 float getTotalBadInformation(std::vector<boost::shared_ptr<RecognitionModel> > & recog_models) 00396 { 00397 float bad_info = 0; 00398 for (size_t i = 0; i < recog_models.size (); i++) 00399 bad_info += recog_models[i]->outliers_weight_ * static_cast<float> (recog_models[i]->bad_information_); 00400 00401 return bad_info; 00402 } 00403 00404 float getUnexplainedInformationInNeighborhood(std::vector<float> & unexplained, std::vector<int> & explained) 00405 { 00406 float unexplained_sum = 0.f; 00407 for (size_t i = 0; i < unexplained.size (); i++) 00408 { 00409 if (unexplained[i] > 0 && explained[i] == 0) 00410 unexplained_sum += unexplained[i]; 00411 } 00412 00413 return unexplained_sum; 00414 } 00415 00416 //Performs smooth segmentation of the scene cloud and compute the model cues 00417 void 00418 initialize(); 00419 00420 mets::gol_type 00421 evaluateSolution(const std::vector<bool> & active, int changed); 00422 00423 bool 00424 addModel(typename pcl::PointCloud<ModelT>::ConstPtr & model, typename pcl::PointCloud<ModelT>::ConstPtr & complete_model, 00425 boost::shared_ptr<RecognitionModel> & recog_model); 00426 00427 void 00428 computeClutterCue(boost::shared_ptr<RecognitionModel> & recog_model); 00429 00430 void 00431 SAOptimize(std::vector<int> & cc_indices, std::vector<bool> & sub_solution); 00432 00433 public: 00434 GlobalHypothesesVerification() : HypothesisVerification<ModelT, SceneT>() 00435 { 00436 resolution_ = 0.005f; 00437 max_iterations_ = 5000; 00438 regularizer_ = 1.f; 00439 radius_normals_ = 0.01f; 00440 initial_temp_ = 1000; 00441 detect_clutter_ = true; 00442 radius_neighborhood_GO_ = 0.03f; 00443 clutter_regularizer_ = 5.f; 00444 res_occupancy_grid_ = 0.01f; 00445 w_occupied_multiple_cm_ = 4.f; 00446 } 00447 00448 void 00449 verify(); 00450 00451 void setRadiusNormals(float r) 00452 { 00453 radius_normals_ = r; 00454 } 00455 00456 void setMaxIterations(int i) 00457 { 00458 max_iterations_ = i; 00459 } 00460 00461 void setInitialTemp(float t) 00462 { 00463 initial_temp_ = t; 00464 } 00465 00466 void setRegularizer(float r) 00467 { 00468 regularizer_ = r; 00469 } 00470 00471 void setRadiusClutter(float r) 00472 { 00473 radius_neighborhood_GO_ = r; 00474 } 00475 00476 void setClutterRegularizer(float cr) 00477 { 00478 clutter_regularizer_ = cr; 00479 } 00480 00481 void setDetectClutter(bool d) 00482 { 00483 detect_clutter_ = d; 00484 } 00485 }; 00486 } 00487 00488 #ifdef PCL_NO_PRECOMPILE 00489 #include <pcl/recognition/impl/hv/hv_go.hpp> 00490 #endif 00491 00492 #endif /* GO_H_ */