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