Point Cloud Library (PCL)  1.9.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 #pragma once
38 
39 #include <pcl/pcl_macros.h>
40 #include "pcl/recognition/hv/occlusion_reasoning.h"
41 #include "pcl/recognition/impl/hv/occlusion_reasoning.hpp"
42 #include <pcl/common/common.h>
43 #include <pcl/search/kdtree.h>
44 #include <pcl/filters/voxel_grid.h>
45 
46 namespace pcl
47 {
48 
49  /**
50  * \brief Abstract class for hypotheses verification methods
51  * \author Aitor Aldoma, Federico Tombari
52  */
53 
54  template<typename ModelT, typename SceneT>
55  class PCL_EXPORTS HypothesisVerification
56  {
57 
58  protected:
59  /*
60  * \brief Boolean vector indicating if a hypothesis is accepted/rejected (output of HV stage)
61  */
62  std::vector<bool> mask_;
63  /*
64  * \brief Scene point cloud
65  */
67 
68  /*
69  * \brief Scene point cloud
70  */
72 
74 
75  /*
76  * \brief Downsampled scene point cloud
77  */
79 
80  /*
81  * \brief Scene tree of the downsampled cloud
82  */
84 
85  /*
86  * \brief Vector of point clouds representing the 3D models after occlusion reasoning
87  * the 3D models are pruned of occluded points, and only visible points are left.
88  * the coordinate system is that of the scene cloud
89  */
90  typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> visible_models_;
91 
92  std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> visible_normal_models_;
93  /*
94  * \brief Vector of point clouds representing the complete 3D model (in same coordinates as the scene cloud)
95  */
96  typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> complete_models_;
97 
98  std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> complete_normal_models_;
99  /*
100  * \brief Resolutions in pixel for the depth scene buffer
101  */
103  /*
104  * \brief Resolutions in pixel for the depth model self-occlusion buffer
105  */
107  /*
108  * \brief The resolution of models and scene used to verify hypotheses (in meters)
109  */
110  float resolution_;
111 
112  /*
113  * \brief Threshold for inliers
114  */
116 
117  /*
118  * \brief Threshold to consider a point being occluded
119  */
121 
122  /*
123  * \brief Whether the HV method requires normals or not, by default = false
124  */
126 
127  /*
128  * \brief Whether the normals have been set
129  */
131  public:
132 
134  {
135  zbuffer_scene_resolution_ = 100;
136  zbuffer_self_occlusion_resolution_ = 150;
137  resolution_ = 0.005f;
138  inliers_threshold_ = static_cast<float>(resolution_);
139  occlusion_cloud_set_ = false;
140  occlusion_thres_ = 0.005f;
141  normals_set_ = false;
142  requires_normals_ = false;
143  }
144 
145  virtual
146  ~HypothesisVerification() = default;
147 
149  return requires_normals_;
150  }
151 
152  /*
153  * \brief Sets the resolution of scene cloud and models used to verify hypotheses
154  * mask r resolution
155  */
156  void
157  setResolution(float r) {
158  resolution_ = r;
159  }
160 
161  /*
162  * \brief Sets the occlusion threshold
163  * mask t threshold
164  */
165  void
167  occlusion_thres_ = t;
168  }
169 
170  /*
171  * \brief Sets the resolution of scene cloud and models used to verify hypotheses
172  * mask r resolution
173  */
174  void
176  inliers_threshold_ = r;
177  }
178 
179  /*
180  * \brief Returns a vector of booleans representing which hypotheses have been accepted/rejected (true/false)
181  * mask vector of booleans
182  */
183 
184  void
185  getMask (std::vector<bool> & mask)
186  {
187  mask = mask_;
188  }
189 
190  /*
191  * \brief Sets the 3D complete models. NOTE: If addModels is called with occlusion_reasoning=true, then
192  * there is no need to call this function.
193  * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_)
194  */
195 
196  void
197  addCompleteModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & complete_models)
198  {
199  complete_models_ = complete_models;
200  }
201 
202  /*
203  * \brief Sets the normals of the 3D complete models and sets normals_set_ to true.
204  * Normals need to be added before calling the addModels method.
205  * complete_models The normals of the models.
206  */
207  void
209  {
210  complete_normal_models_ = complete_models;
211  normals_set_ = true;
212  }
213 
214  /*
215  * \brief Sets the models (recognition hypotheses) - requires the scene_cloud_ to be set first if reasoning about occlusions
216  * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_)
217  */
218  void
219  addModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & models, bool occlusion_reasoning = false)
220  {
221 
222  mask_.clear();
223  if(!occlusion_cloud_set_) {
224  PCL_WARN("Occlusion cloud not set, using scene_cloud instead...\n");
225  occlusion_cloud_ = scene_cloud_;
226  }
227 
228  if (!occlusion_reasoning)
229  visible_models_ = models;
230  else
231  {
232  //we need to reason about occlusions before setting the model
233  if (scene_cloud_ == 0)
234  {
235  PCL_ERROR("setSceneCloud should be called before adding the model if reasoning about occlusions...");
236  }
237 
238  if (!occlusion_cloud_->isOrganized ())
239  {
240  PCL_WARN("Scene not organized... filtering using computed depth buffer\n");
241  }
242 
243  pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_scene (zbuffer_scene_resolution_, zbuffer_scene_resolution_, 1.f);
244  if (!occlusion_cloud_->isOrganized ())
245  {
246  zbuffer_scene.computeDepthMap (occlusion_cloud_, true);
247  }
248 
249  for (size_t i = 0; i < models.size (); i++)
250  {
251 
252  //self-occlusions
253  typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
254  typename pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_self_occlusion (75, 75, 1.f);
255  zbuffer_self_occlusion.computeDepthMap (models[i], true);
256  std::vector<int> indices;
257  zbuffer_self_occlusion.filter (models[i], indices, 0.005f);
258  pcl::copyPointCloud (*models[i], indices, *filtered);
259 
260  if(normals_set_ && requires_normals_) {
262  pcl::copyPointCloud(*complete_normal_models_[i], indices, *filtered_normals);
263  visible_normal_models_.push_back(filtered_normals);
264  }
265 
266  typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*filtered));
267  //typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*models[i]));
268  //scene-occlusions
269  if (occlusion_cloud_->isOrganized ())
270  {
271  filtered = pcl::occlusion_reasoning::filter<ModelT,SceneT> (occlusion_cloud_, const_filtered, 525.f, occlusion_thres_);
272  }
273  else
274  {
275  zbuffer_scene.filter (const_filtered, filtered, occlusion_thres_);
276  }
277 
278  visible_models_.push_back (filtered);
279  }
280 
281  complete_models_ = models;
282  }
283 
284  occlusion_cloud_set_ = false;
285  normals_set_ = false;
286  }
287 
288  /*
289  * \brief Sets the scene cloud
290  * scene_cloud Point cloud representing the scene
291  */
292 
293  void
294  setSceneCloud (const typename pcl::PointCloud<SceneT>::Ptr & scene_cloud)
295  {
296 
297  complete_models_.clear();
298  visible_models_.clear();
299  visible_normal_models_.clear();
300 
301  scene_cloud_ = scene_cloud;
302  scene_cloud_downsampled_.reset(new pcl::PointCloud<SceneT>());
303 
304  pcl::VoxelGrid<SceneT> voxel_grid;
305  voxel_grid.setInputCloud (scene_cloud);
306  voxel_grid.setLeafSize (resolution_, resolution_, resolution_);
307  voxel_grid.setDownsampleAllData(true);
308  voxel_grid.filter (*scene_cloud_downsampled_);
309 
310  //initialize kdtree for search
311  scene_downsampled_tree_.reset (new pcl::search::KdTree<SceneT>);
312  scene_downsampled_tree_->setInputCloud(scene_cloud_downsampled_);
313  }
314 
315  void setOcclusionCloud (const typename pcl::PointCloud<SceneT>::Ptr & occ_cloud)
316  {
317  occlusion_cloud_ = occ_cloud;
318  occlusion_cloud_set_ = true;
319  }
320 
321  /*
322  * \brief Function that performs the hypotheses verification, needs to be implemented in the subclasses
323  * This function modifies the values of mask_ and needs to be called after both scene and model have been added
324  */
325 
326  virtual void
327  verify ()=0;
328  };
329 
330 }
pcl::PointCloud< SceneT >::Ptr scene_cloud_downsampled_
std::vector< typename pcl::PointCloud< pcl::Normal >::ConstPtr > complete_normal_models_
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
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:131
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:177
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< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
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.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:428
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
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:330
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