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_HYPOTHESIS_VERIFICATION_H_ 00038 #define PCL_RECOGNITION_HYPOTHESIS_VERIFICATION_H_ 00039 00040 #include <pcl/pcl_macros.h> 00041 #include "pcl/recognition/hv/occlusion_reasoning.h" 00042 #include "pcl/recognition/impl/hv/occlusion_reasoning.hpp" 00043 #include <pcl/common/common.h> 00044 #include <pcl/search/kdtree.h> 00045 #include <pcl/filters/voxel_grid.h> 00046 00047 namespace pcl 00048 { 00049 00050 /** 00051 * \brief Abstract class for hypotheses verification methods 00052 * \author Aitor Aldoma, Federico Tombari 00053 */ 00054 00055 template<typename ModelT, typename SceneT> 00056 class PCL_EXPORTS HypothesisVerification 00057 { 00058 00059 protected: 00060 /* 00061 * \brief Boolean vector indicating if a hypothesis is accepted/rejected (output of HV stage) 00062 */ 00063 std::vector<bool> mask_; 00064 /* 00065 * \brief Scene point cloud 00066 */ 00067 typename pcl::PointCloud<SceneT>::ConstPtr scene_cloud_; 00068 00069 /* 00070 * \brief Scene point cloud 00071 */ 00072 typename pcl::PointCloud<SceneT>::ConstPtr occlusion_cloud_; 00073 00074 bool occlusion_cloud_set_; 00075 00076 /* 00077 * \brief Downsampled scene point cloud 00078 */ 00079 typename pcl::PointCloud<SceneT>::Ptr scene_cloud_downsampled_; 00080 00081 /* 00082 * \brief Scene tree of the downsampled cloud 00083 */ 00084 typename pcl::search::KdTree<SceneT>::Ptr scene_downsampled_tree_; 00085 00086 /* 00087 * \brief Vector of point clouds representing the 3D models after occlusion reasoning 00088 * the 3D models are pruned of occluded points, and only visible points are left. 00089 * the coordinate system is that of the scene cloud 00090 */ 00091 typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> visible_models_; 00092 00093 std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> visible_normal_models_; 00094 /* 00095 * \brief Vector of point clouds representing the complete 3D model (in same coordinates as the scene cloud) 00096 */ 00097 typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> complete_models_; 00098 00099 std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> complete_normal_models_; 00100 /* 00101 * \brief Resolutions in pixel for the depth scene buffer 00102 */ 00103 int zbuffer_scene_resolution_; 00104 /* 00105 * \brief Resolutions in pixel for the depth model self-occlusion buffer 00106 */ 00107 int zbuffer_self_occlusion_resolution_; 00108 /* 00109 * \brief The resolution of models and scene used to verify hypotheses (in meters) 00110 */ 00111 float resolution_; 00112 00113 /* 00114 * \brief Threshold for inliers 00115 */ 00116 float inliers_threshold_; 00117 00118 /* 00119 * \brief Threshold to consider a point being occluded 00120 */ 00121 float occlusion_thres_; 00122 00123 /* 00124 * \brief Whether the HV method requires normals or not, by default = false 00125 */ 00126 bool requires_normals_; 00127 00128 /* 00129 * \brief Whether the normals have been set 00130 */ 00131 bool normals_set_; 00132 public: 00133 00134 HypothesisVerification () 00135 { 00136 zbuffer_scene_resolution_ = 100; 00137 zbuffer_self_occlusion_resolution_ = 150; 00138 resolution_ = 0.005f; 00139 inliers_threshold_ = static_cast<float>(resolution_); 00140 occlusion_cloud_set_ = false; 00141 occlusion_thres_ = 0.005f; 00142 normals_set_ = false; 00143 requires_normals_ = false; 00144 } 00145 00146 bool getRequiresNormals() { 00147 return requires_normals_; 00148 } 00149 00150 /* 00151 * \brief Sets the resolution of scene cloud and models used to verify hypotheses 00152 * mask r resolution 00153 */ 00154 void 00155 setResolution(float r) { 00156 resolution_ = r; 00157 } 00158 00159 /* 00160 * \brief Sets the occlusion threshold 00161 * mask t threshold 00162 */ 00163 void 00164 setOcclusionThreshold(float t) { 00165 occlusion_thres_ = t; 00166 } 00167 00168 /* 00169 * \brief Sets the resolution of scene cloud and models used to verify hypotheses 00170 * mask r resolution 00171 */ 00172 void 00173 setInlierThreshold(float r) { 00174 inliers_threshold_ = r; 00175 } 00176 00177 /* 00178 * \brief Returns a vector of booleans representing which hypotheses have been accepted/rejected (true/false) 00179 * mask vector of booleans 00180 */ 00181 00182 void 00183 getMask (std::vector<bool> & mask) 00184 { 00185 mask = mask_; 00186 } 00187 00188 /* 00189 * \brief Sets the 3D complete models. NOTE: If addModels is called with occlusion_reasoning=true, then 00190 * there is no need to call this function. 00191 * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_) 00192 */ 00193 00194 void 00195 addCompleteModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & complete_models) 00196 { 00197 complete_models_ = complete_models; 00198 } 00199 00200 /* 00201 * \brief Sets the normals of the 3D complete models and sets normals_set_ to true. 00202 * Normals need to be added before calling the addModels method. 00203 * complete_models The normals of the models. 00204 */ 00205 void 00206 addNormalsClouds (std::vector<pcl::PointCloud<pcl::Normal>::ConstPtr> & complete_models) 00207 { 00208 complete_normal_models_ = complete_models; 00209 normals_set_ = true; 00210 } 00211 00212 /* 00213 * \brief Sets the models (recognition hypotheses) - requires the scene_cloud_ to be set first if reasoning about occlusions 00214 * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_) 00215 */ 00216 void 00217 addModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & models, bool occlusion_reasoning = false) 00218 { 00219 00220 mask_.clear(); 00221 if(!occlusion_cloud_set_) { 00222 PCL_WARN("Occlusion cloud not set, using scene_cloud instead...\n"); 00223 occlusion_cloud_ = scene_cloud_; 00224 } 00225 00226 if (!occlusion_reasoning) 00227 visible_models_ = models; 00228 else 00229 { 00230 //we need to reason about occlusions before setting the model 00231 if (scene_cloud_ == 0) 00232 { 00233 PCL_ERROR("setSceneCloud should be called before adding the model if reasoning about occlusions..."); 00234 } 00235 00236 if (!occlusion_cloud_->isOrganized ()) 00237 { 00238 PCL_WARN("Scene not organized... filtering using computed depth buffer\n"); 00239 } 00240 00241 pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_scene (zbuffer_scene_resolution_, zbuffer_scene_resolution_, 1.f); 00242 if (!occlusion_cloud_->isOrganized ()) 00243 { 00244 zbuffer_scene.computeDepthMap (occlusion_cloud_, true); 00245 } 00246 00247 for (size_t i = 0; i < models.size (); i++) 00248 { 00249 00250 //self-occlusions 00251 typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ()); 00252 typename pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_self_occlusion (75, 75, 1.f); 00253 zbuffer_self_occlusion.computeDepthMap (models[i], true); 00254 std::vector<int> indices; 00255 zbuffer_self_occlusion.filter (models[i], indices, 0.005f); 00256 pcl::copyPointCloud (*models[i], indices, *filtered); 00257 00258 if(normals_set_ && requires_normals_) { 00259 pcl::PointCloud<pcl::Normal>::Ptr filtered_normals (new pcl::PointCloud<pcl::Normal> ()); 00260 pcl::copyPointCloud(*complete_normal_models_[i], indices, *filtered_normals); 00261 visible_normal_models_.push_back(filtered_normals); 00262 } 00263 00264 typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*filtered)); 00265 //typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*models[i])); 00266 //scene-occlusions 00267 if (occlusion_cloud_->isOrganized ()) 00268 { 00269 filtered = pcl::occlusion_reasoning::filter<ModelT,SceneT> (occlusion_cloud_, const_filtered, 525.f, occlusion_thres_); 00270 } 00271 else 00272 { 00273 zbuffer_scene.filter (const_filtered, filtered, occlusion_thres_); 00274 } 00275 00276 visible_models_.push_back (filtered); 00277 } 00278 00279 complete_models_ = models; 00280 } 00281 00282 occlusion_cloud_set_ = false; 00283 normals_set_ = false; 00284 } 00285 00286 /* 00287 * \brief Sets the scene cloud 00288 * scene_cloud Point cloud representing the scene 00289 */ 00290 00291 void 00292 setSceneCloud (const typename pcl::PointCloud<SceneT>::Ptr & scene_cloud) 00293 { 00294 00295 complete_models_.clear(); 00296 visible_models_.clear(); 00297 visible_normal_models_.clear(); 00298 00299 scene_cloud_ = scene_cloud; 00300 scene_cloud_downsampled_.reset(new pcl::PointCloud<SceneT>()); 00301 00302 pcl::VoxelGrid<SceneT> voxel_grid; 00303 voxel_grid.setInputCloud (scene_cloud); 00304 voxel_grid.setLeafSize (resolution_, resolution_, resolution_); 00305 voxel_grid.setDownsampleAllData(true); 00306 voxel_grid.filter (*scene_cloud_downsampled_); 00307 00308 //initialize kdtree for search 00309 scene_downsampled_tree_.reset (new pcl::search::KdTree<SceneT>); 00310 scene_downsampled_tree_->setInputCloud(scene_cloud_downsampled_); 00311 } 00312 00313 void setOcclusionCloud (const typename pcl::PointCloud<SceneT>::Ptr & occ_cloud) 00314 { 00315 occlusion_cloud_ = occ_cloud; 00316 occlusion_cloud_set_ = true; 00317 } 00318 00319 /* 00320 * \brief Function that performs the hypotheses verification, needs to be implemented in the subclasses 00321 * This function modifies the values of mask_ and needs to be called after both scene and model have been added 00322 */ 00323 00324 virtual void 00325 verify ()=0; 00326 }; 00327 00328 } 00329 00330 #endif /* PCL_RECOGNITION_HYPOTHESIS_VERIFICATION_H_ */