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 #pragma once 00038 #include <pcl/recognition/hv/hv_papazov.h> 00039 00040 /////////////////////////////////////////////////////////////////////////////////////////////////// 00041 template<typename ModelT, typename SceneT> 00042 void 00043 pcl::PapazovHV<ModelT, SceneT>::initialize () 00044 { 00045 00046 //clear stuff 00047 recognition_models_.clear (); 00048 graph_id_model_map_.clear (); 00049 conflict_graph_.clear (); 00050 explained_by_RM_.clear (); 00051 points_explained_by_rm_.clear (); 00052 00053 // initialize mask... 00054 mask_.resize (complete_models_.size ()); 00055 for (size_t i = 0; i < complete_models_.size (); i++) 00056 mask_[i] = true; 00057 00058 // initialize explained_by_RM 00059 explained_by_RM_.resize (scene_cloud_downsampled_->points.size ()); 00060 points_explained_by_rm_.resize (scene_cloud_downsampled_->points.size ()); 00061 00062 // initalize model 00063 for (size_t m = 0; m < complete_models_.size (); m++) 00064 { 00065 boost::shared_ptr < RecognitionModel > recog_model (new RecognitionModel); 00066 // voxelize model cloud 00067 recog_model->cloud_.reset (new pcl::PointCloud<ModelT>); 00068 recog_model->complete_cloud_.reset (new pcl::PointCloud<ModelT>); 00069 recog_model->id_ = static_cast<int> (m); 00070 00071 pcl::VoxelGrid<ModelT> voxel_grid; 00072 voxel_grid.setInputCloud (visible_models_[m]); 00073 voxel_grid.setLeafSize (resolution_, resolution_, resolution_); 00074 voxel_grid.filter (*(recog_model->cloud_)); 00075 00076 pcl::VoxelGrid<ModelT> voxel_grid_complete; 00077 voxel_grid_complete.setInputCloud (complete_models_[m]); 00078 voxel_grid_complete.setLeafSize (resolution_, resolution_, resolution_); 00079 voxel_grid_complete.filter (*(recog_model->complete_cloud_)); 00080 00081 std::vector<int> explained_indices; 00082 std::vector<int> outliers; 00083 std::vector<int> nn_indices; 00084 std::vector<float> nn_distances; 00085 00086 for (size_t i = 0; i < recog_model->cloud_->points.size (); i++) 00087 { 00088 if (!scene_downsampled_tree_->radiusSearch (recog_model->cloud_->points[i], inliers_threshold_, nn_indices, nn_distances, 00089 std::numeric_limits<int>::max ())) 00090 { 00091 outliers.push_back (static_cast<int> (i)); 00092 } 00093 else 00094 { 00095 for (size_t k = 0; k < nn_distances.size (); k++) 00096 { 00097 explained_indices.push_back (nn_indices[k]); //nn_indices[k] points to the scene 00098 } 00099 } 00100 } 00101 00102 std::sort (explained_indices.begin (), explained_indices.end ()); 00103 explained_indices.erase (std::unique (explained_indices.begin (), explained_indices.end ()), explained_indices.end ()); 00104 00105 recog_model->bad_information_ = static_cast<int> (outliers.size ()); 00106 00107 if ((static_cast<float> (recog_model->bad_information_) / static_cast<float> (recog_model->complete_cloud_->points.size ())) 00108 <= penalty_threshold_ && (static_cast<float> (explained_indices.size ()) 00109 / static_cast<float> (recog_model->complete_cloud_->points.size ())) >= support_threshold_) 00110 { 00111 recog_model->explained_ = explained_indices; 00112 recognition_models_.push_back (recog_model); 00113 00114 // update explained_by_RM_, add 1 00115 for (size_t i = 0; i < explained_indices.size (); i++) 00116 { 00117 explained_by_RM_[explained_indices[i]]++; 00118 points_explained_by_rm_[explained_indices[i]].push_back (recog_model); 00119 } 00120 } 00121 else 00122 { 00123 mask_[m] = false; // the model didnt survive the sequential check... 00124 } 00125 } 00126 } 00127 00128 /////////////////////////////////////////////////////////////////////////////////////////////////// 00129 template<typename ModelT, typename SceneT> 00130 void 00131 pcl::PapazovHV<ModelT, SceneT>::nonMaximaSuppresion () 00132 { 00133 // iterate over all vertices of the graph and check if they have a better neighbour, then remove that vertex 00134 typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIterator; 00135 VertexIterator vi, vi_end, next; 00136 boost::tie (vi, vi_end) = boost::vertices (conflict_graph_); 00137 00138 for (next = vi; next != vi_end; next++) 00139 { 00140 const typename Graph::vertex_descriptor v = boost::vertex (*next, conflict_graph_); 00141 typename boost::graph_traits<Graph>::adjacency_iterator ai; 00142 typename boost::graph_traits<Graph>::adjacency_iterator ai_end; 00143 00144 boost::shared_ptr < RecognitionModel > current = static_cast<boost::shared_ptr<RecognitionModel> > (graph_id_model_map_[int (v)]); 00145 00146 bool a_better_one = false; 00147 for (boost::tie (ai, ai_end) = boost::adjacent_vertices (v, conflict_graph_); (ai != ai_end) && !a_better_one; ++ai) 00148 { 00149 boost::shared_ptr < RecognitionModel > neighbour = static_cast<boost::shared_ptr<RecognitionModel> > (graph_id_model_map_[int (*ai)]); 00150 if ((neighbour->explained_.size () >= current->explained_.size ()) && mask_[neighbour->id_]) 00151 { 00152 a_better_one = true; 00153 } 00154 } 00155 00156 if (a_better_one) 00157 { 00158 mask_[current->id_] = false; 00159 } 00160 } 00161 } 00162 00163 /////////////////////////////////////////////////////////////////////////////////////////////////// 00164 template<typename ModelT, typename SceneT> 00165 void 00166 pcl::PapazovHV<ModelT, SceneT>::buildConflictGraph () 00167 { 00168 // create vertices for the graph 00169 for (size_t i = 0; i < (recognition_models_.size ()); i++) 00170 { 00171 const typename Graph::vertex_descriptor v = boost::add_vertex (recognition_models_[i], conflict_graph_); 00172 graph_id_model_map_[int (v)] = static_cast<boost::shared_ptr<RecognitionModel> > (recognition_models_[i]); 00173 } 00174 00175 // iterate over the remaining models and check for each one if there is a conflict with another one 00176 for (size_t i = 0; i < recognition_models_.size (); i++) 00177 { 00178 for (size_t j = i; j < recognition_models_.size (); j++) 00179 { 00180 if (i != j) 00181 { 00182 float n_conflicts = 0.f; 00183 // count scene points explained by both models 00184 for (size_t k = 0; k < explained_by_RM_.size (); k++) 00185 { 00186 if (explained_by_RM_[k] > 1) 00187 { 00188 // this point could be a conflict 00189 bool i_found = false; 00190 bool j_found = false; 00191 bool both_found = false; 00192 for (size_t kk = 0; (kk < points_explained_by_rm_[k].size ()) && !both_found; kk++) 00193 { 00194 if (points_explained_by_rm_[k][kk]->id_ == recognition_models_[i]->id_) 00195 i_found = true; 00196 00197 if (points_explained_by_rm_[k][kk]->id_ == recognition_models_[j]->id_) 00198 j_found = true; 00199 00200 if (i_found && j_found) 00201 both_found = true; 00202 } 00203 00204 if (both_found) 00205 n_conflicts += 1.f; 00206 } 00207 } 00208 00209 // check if number of points is big enough to create a conflict 00210 bool add_conflict = false; 00211 add_conflict = ((n_conflicts / static_cast<float> (recognition_models_[i]->complete_cloud_->points.size ())) > conflict_threshold_size_) 00212 || ((n_conflicts / static_cast<float> (recognition_models_[j]->complete_cloud_->points.size ())) > conflict_threshold_size_); 00213 00214 if (add_conflict) 00215 { 00216 boost::add_edge (i, j, conflict_graph_); 00217 } 00218 } 00219 } 00220 } 00221 } 00222 00223 /////////////////////////////////////////////////////////////////////////////////////////////////// 00224 template<typename ModelT, typename SceneT> 00225 void 00226 pcl::PapazovHV<ModelT, SceneT>::verify () 00227 { 00228 initialize (); 00229 buildConflictGraph (); 00230 nonMaximaSuppresion (); 00231 } 00232 00233 #define PCL_INSTANTIATE_PapazovHV(T1,T2) template class PCL_EXPORTS pcl::PapazovHV<T1,T2>;