hv_go.h
Go to the documentation of this file.
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 
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_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:48