Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 */ 00036 00037 #ifndef PCL_RECOGNITION_HV_GREEDY_H_ 00038 #define PCL_RECOGNITION_HV_GREEDY_H_ 00039 00040 #include <pcl/pcl_macros.h> 00041 #include <pcl/recognition/hv/hypotheses_verification.h> 00042 #include <pcl/common/common.h> 00043 00044 namespace pcl 00045 { 00046 00047 /** 00048 * \brief A greedy hypothesis verification method 00049 * \author Aitor Aldoma 00050 */ 00051 00052 template<typename ModelT, typename SceneT> 00053 class PCL_EXPORTS GreedyVerification : public HypothesisVerification<ModelT, SceneT> 00054 { 00055 using HypothesisVerification<ModelT, SceneT>::mask_; 00056 using HypothesisVerification<ModelT, SceneT>::scene_cloud_downsampled_; 00057 using HypothesisVerification<ModelT, SceneT>::scene_downsampled_tree_; 00058 using HypothesisVerification<ModelT, SceneT>::visible_models_; 00059 using HypothesisVerification<ModelT, SceneT>::resolution_; 00060 using HypothesisVerification<ModelT, SceneT>::inliers_threshold_; 00061 00062 /* 00063 * \brief Recognition model using during the verification 00064 */ 00065 class RecognitionModel 00066 { 00067 public: 00068 std::vector<int> explained_; 00069 typename pcl::PointCloud<ModelT>::Ptr cloud_; 00070 int bad_information_; 00071 int good_information_; 00072 int id_; 00073 float regularizer_; 00074 }; 00075 00076 /* 00077 * \brief Sorts recognition models based on the number of explained scene points and visible outliers 00078 */ 00079 struct sortModelsClass 00080 { 00081 bool 00082 operator() (const boost::shared_ptr<RecognitionModel> & n1, const boost::shared_ptr<RecognitionModel> & n2) 00083 { 00084 float val1 = static_cast<float>(n1->good_information_) - static_cast<float>(n1->bad_information_) * n1->regularizer_; 00085 float val2 = static_cast<float>(n2->good_information_) - static_cast<float>(n2->bad_information_) * n2->regularizer_; 00086 return val1 > val2; 00087 } 00088 } sortModelsOp; 00089 00090 00091 /* 00092 * \brief Recognition model indices to keep track of the sorted recognition hypotheses 00093 */ 00094 struct modelIndices 00095 { 00096 int index_; 00097 boost::shared_ptr<RecognitionModel> model_; 00098 }; 00099 00100 /* 00101 * \brief Sorts model indices similar to sortModelsClass 00102 */ 00103 struct sortModelIndicesClass 00104 { 00105 bool 00106 operator() (const modelIndices & n1, const modelIndices & n2) 00107 { 00108 float val1 = static_cast<float>(n1.model_->good_information_) - static_cast<float>(n1.model_->bad_information_) * n1.model_->regularizer_; 00109 float val2 = static_cast<float>(n2.model_->good_information_) - static_cast<float>(n2.model_->bad_information_) * n2.model_->regularizer_; 00110 return val1 > val2; 00111 } 00112 } sortModelsIndicesOp; 00113 00114 /** \brief Recognition model and indices */ 00115 std::vector<modelIndices> indices_models_; 00116 00117 /** \brief Recognition models (hypotheses to be verified) */ 00118 std::vector<boost::shared_ptr<RecognitionModel> > recognition_models_; 00119 00120 /** \brief Recognition models that explain a scene points. */ 00121 std::vector<std::vector<boost::shared_ptr<RecognitionModel> > > points_explained_by_rm_; 00122 00123 /** \brief Weighting for outliers */ 00124 float regularizer_; 00125 00126 /** \brief Initialize the data structures */ 00127 void 00128 initialize (); 00129 00130 /** \brief Sorts the hypotheses for the greedy approach */ 00131 void 00132 sortModels () 00133 { 00134 indices_models_.clear (); 00135 for (size_t i = 0; i < recognition_models_.size (); i++) 00136 { 00137 modelIndices mi; 00138 mi.index_ = static_cast<int> (i); 00139 mi.model_ = recognition_models_[i]; 00140 indices_models_.push_back (mi); 00141 } 00142 00143 std::sort (indices_models_.begin (), indices_models_.end (), sortModelsIndicesOp); 00144 //sort also recognition models 00145 std::sort (recognition_models_.begin (), recognition_models_.end (), sortModelsOp); 00146 } 00147 00148 /** \brief Updates conflicting recognition hypotheses when a hypothesis is accepted */ 00149 void 00150 updateGoodInformation (int i) 00151 { 00152 for (size_t k = 0; k < recognition_models_[i]->explained_.size (); k++) 00153 { 00154 //update good_information_ for all hypotheses that were explaining the same points as hypothesis i 00155 for (size_t kk = 0; kk < points_explained_by_rm_[recognition_models_[i]->explained_[k]].size (); kk++) 00156 { 00157 (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->good_information_--; 00158 (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->bad_information_++; 00159 } 00160 } 00161 } 00162 00163 public: 00164 00165 /** \brief Constructor 00166 * \param[in] Regularizer value 00167 **/ 00168 GreedyVerification (float reg = 1.5f) : 00169 HypothesisVerification<ModelT, SceneT> () 00170 { 00171 regularizer_ = reg; 00172 } 00173 00174 /** \brief Starts verification */ 00175 void 00176 verify (); 00177 }; 00178 } 00179 00180 #ifdef PCL_NO_PRECOMPILE 00181 #include <pcl/recognition/impl/hv/greedy_verification.hpp> 00182 #endif 00183 00184 #endif /* PCL_RECOGNITION_HV_GREEDY_H_ */