Point Cloud Library (PCL)  1.8.1-dev
hypotheses_verification.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
37 #ifndef PCL_RECOGNITION_HYPOTHESIS_VERIFICATION_H_
38 #define PCL_RECOGNITION_HYPOTHESIS_VERIFICATION_H_
39 
40 #include <pcl/pcl_macros.h>
41 #include "pcl/recognition/hv/occlusion_reasoning.h"
42 #include "pcl/recognition/impl/hv/occlusion_reasoning.hpp"
43 #include <pcl/common/common.h>
44 #include <pcl/search/kdtree.h>
45 #include <pcl/filters/voxel_grid.h>
46 
47 namespace pcl
48 {
49 
50  /**
51  * \brief Abstract class for hypotheses verification methods
52  * \author Aitor Aldoma, Federico Tombari
53  */
54 
55  template<typename ModelT, typename SceneT>
56  class PCL_EXPORTS HypothesisVerification
57  {
58 
59  protected:
60  /*
61  * \brief Boolean vector indicating if a hypothesis is accepted/rejected (output of HV stage)
62  */
63  std::vector<bool> mask_;
64  /*
65  * \brief Scene point cloud
66  */
68 
69  /*
70  * \brief Scene point cloud
71  */
73 
75 
76  /*
77  * \brief Downsampled scene point cloud
78  */
80 
81  /*
82  * \brief Scene tree of the downsampled cloud
83  */
85 
86  /*
87  * \brief Vector of point clouds representing the 3D models after occlusion reasoning
88  * the 3D models are pruned of occluded points, and only visible points are left.
89  * the coordinate system is that of the scene cloud
90  */
91  typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> visible_models_;
92 
93  std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> visible_normal_models_;
94  /*
95  * \brief Vector of point clouds representing the complete 3D model (in same coordinates as the scene cloud)
96  */
97  typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> complete_models_;
98 
99  std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> complete_normal_models_;
100  /*
101  * \brief Resolutions in pixel for the depth scene buffer
102  */
104  /*
105  * \brief Resolutions in pixel for the depth model self-occlusion buffer
106  */
108  /*
109  * \brief The resolution of models and scene used to verify hypotheses (in meters)
110  */
111  float resolution_;
112 
113  /*
114  * \brief Threshold for inliers
115  */
117 
118  /*
119  * \brief Threshold to consider a point being occluded
120  */
122 
123  /*
124  * \brief Whether the HV method requires normals or not, by default = false
125  */
127 
128  /*
129  * \brief Whether the normals have been set
130  */
132  public:
133 
135  {
136  zbuffer_scene_resolution_ = 100;
137  zbuffer_self_occlusion_resolution_ = 150;
138  resolution_ = 0.005f;
139  inliers_threshold_ = static_cast<float>(resolution_);
140  occlusion_cloud_set_ = false;
141  occlusion_thres_ = 0.005f;
142  normals_set_ = false;
143  requires_normals_ = false;
144  }
145 
147  return requires_normals_;
148  }
149 
150  /*
151  * \brief Sets the resolution of scene cloud and models used to verify hypotheses
152  * mask r resolution
153  */
154  void
155  setResolution(float r) {
156  resolution_ = r;
157  }
158 
159  /*
160  * \brief Sets the occlusion threshold
161  * mask t threshold
162  */
163  void
165  occlusion_thres_ = t;
166  }
167 
168  /*
169  * \brief Sets the resolution of scene cloud and models used to verify hypotheses
170  * mask r resolution
171  */
172  void
174  inliers_threshold_ = r;
175  }
176 
177  /*
178  * \brief Returns a vector of booleans representing which hypotheses have been accepted/rejected (true/false)
179  * mask vector of booleans
180  */
181 
182  void
183  getMask (std::vector<bool> & mask)
184  {
185  mask = mask_;
186  }
187 
188  /*
189  * \brief Sets the 3D complete models. NOTE: If addModels is called with occlusion_reasoning=true, then
190  * there is no need to call this function.
191  * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_)
192  */
193 
194  void
195  addCompleteModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & complete_models)
196  {
197  complete_models_ = complete_models;
198  }
199 
200  /*
201  * \brief Sets the normals of the 3D complete models and sets normals_set_ to true.
202  * Normals need to be added before calling the addModels method.
203  * complete_models The normals of the models.
204  */
205  void
207  {
208  complete_normal_models_ = complete_models;
209  normals_set_ = true;
210  }
211 
212  /*
213  * \brief Sets the models (recognition hypotheses) - requires the scene_cloud_ to be set first if reasoning about occlusions
214  * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_)
215  */
216  void
217  addModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & models, bool occlusion_reasoning = false)
218  {
219 
220  mask_.clear();
221  if(!occlusion_cloud_set_) {
222  PCL_WARN("Occlusion cloud not set, using scene_cloud instead...\n");
223  occlusion_cloud_ = scene_cloud_;
224  }
225 
226  if (!occlusion_reasoning)
227  visible_models_ = models;
228  else
229  {
230  //we need to reason about occlusions before setting the model
231  if (scene_cloud_ == 0)
232  {
233  PCL_ERROR("setSceneCloud should be called before adding the model if reasoning about occlusions...");
234  }
235 
236  if (!occlusion_cloud_->isOrganized ())
237  {
238  PCL_WARN("Scene not organized... filtering using computed depth buffer\n");
239  }
240 
241  pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_scene (zbuffer_scene_resolution_, zbuffer_scene_resolution_, 1.f);
242  if (!occlusion_cloud_->isOrganized ())
243  {
244  zbuffer_scene.computeDepthMap (occlusion_cloud_, true);
245  }
246 
247  for (size_t i = 0; i < models.size (); i++)
248  {
249 
250  //self-occlusions
251  typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
252  typename pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_self_occlusion (75, 75, 1.f);
253  zbuffer_self_occlusion.computeDepthMap (models[i], true);
254  std::vector<int> indices;
255  zbuffer_self_occlusion.filter (models[i], indices, 0.005f);
256  pcl::copyPointCloud (*models[i], indices, *filtered);
257 
258  if(normals_set_ && requires_normals_) {
260  pcl::copyPointCloud(*complete_normal_models_[i], indices, *filtered_normals);
261  visible_normal_models_.push_back(filtered_normals);
262  }
263 
264  typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*filtered));
265  //typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*models[i]));
266  //scene-occlusions
267  if (occlusion_cloud_->isOrganized ())
268  {
269  filtered = pcl::occlusion_reasoning::filter<ModelT,SceneT> (occlusion_cloud_, const_filtered, 525.f, occlusion_thres_);
270  }
271  else
272  {
273  zbuffer_scene.filter (const_filtered, filtered, occlusion_thres_);
274  }
275 
276  visible_models_.push_back (filtered);
277  }
278 
279  complete_models_ = models;
280  }
281 
282  occlusion_cloud_set_ = false;
283  normals_set_ = false;
284  }
285 
286  /*
287  * \brief Sets the scene cloud
288  * scene_cloud Point cloud representing the scene
289  */
290 
291  void
292  setSceneCloud (const typename pcl::PointCloud<SceneT>::Ptr & scene_cloud)
293  {
294 
295  complete_models_.clear();
296  visible_models_.clear();
297  visible_normal_models_.clear();
298 
299  scene_cloud_ = scene_cloud;
300  scene_cloud_downsampled_.reset(new pcl::PointCloud<SceneT>());
301 
302  pcl::VoxelGrid<SceneT> voxel_grid;
303  voxel_grid.setInputCloud (scene_cloud);
304  voxel_grid.setLeafSize (resolution_, resolution_, resolution_);
305  voxel_grid.setDownsampleAllData(true);
306  voxel_grid.filter (*scene_cloud_downsampled_);
307 
308  //initialize kdtree for search
309  scene_downsampled_tree_.reset (new pcl::search::KdTree<SceneT>);
310  scene_downsampled_tree_->setInputCloud(scene_cloud_downsampled_);
311  }
312 
313  void setOcclusionCloud (const typename pcl::PointCloud<SceneT>::Ptr & occ_cloud)
314  {
315  occlusion_cloud_ = occ_cloud;
316  occlusion_cloud_set_ = true;
317  }
318 
319  /*
320  * \brief Function that performs the hypotheses verification, needs to be implemented in the subclasses
321  * This function modifies the values of mask_ and needs to be called after both scene and model have been added
322  */
323 
324  virtual void
325  verify ()=0;
326  };
327 
328 }
329 
330 #endif /* PCL_RECOGNITION_HYPOTHESIS_VERIFICATION_H_ */
pcl::PointCloud< SceneT >::Ptr scene_cloud_downsampled_
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
std::vector< typename pcl::PointCloud< pcl::Normal >::ConstPtr > complete_normal_models_
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void addNormalsClouds(std::vector< pcl::PointCloud< pcl::Normal >::ConstPtr > &complete_models)
std::vector< typename pcl::PointCloud< ModelT >::ConstPtr > visible_models_
Class to reason about occlusions.
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition: voxel_grid.h:257
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:132
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
std::vector< typename pcl::PointCloud< pcl::Normal >::ConstPtr > visible_normal_models_
void computeDepthMap(typename pcl::PointCloud< SceneT >::ConstPtr &scene, bool compute_focal=false, bool smooth=false, int wsize=3)
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:79
std::vector< typename pcl::PointCloud< ModelT >::ConstPtr > complete_models_
void setSceneCloud(const typename pcl::PointCloud< SceneT >::Ptr &scene_cloud)
Abstract class for hypotheses verification methods.
pcl::PointCloud< SceneT >::ConstPtr occlusion_cloud_
pcl::PointCloud< SceneT >::ConstPtr scene_cloud_
void getMask(std::vector< bool > &mask)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
void addCompleteModels(std::vector< typename pcl::PointCloud< ModelT >::ConstPtr > &complete_models)
void addModels(std::vector< typename pcl::PointCloud< ModelT >::ConstPtr > &models, bool occlusion_reasoning=false)
void setOcclusionCloud(const typename pcl::PointCloud< SceneT >::Ptr &occ_cloud)
pcl::search::KdTree< SceneT >::Ptr scene_downsampled_tree_
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:223