Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
45namespace 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>
54 class PCL_EXPORTS HypothesisVerification
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 */
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
PointCloud represents the base class in PCL for storing collections of 3D points.
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition voxel_grid.h:177
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition voxel_grid.h:248
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:214
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)
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
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.