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>
54 template<
typename ModelT,
typename SceneT>
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;
149 return requires_normals_;
167 occlusion_thres_ = t;
176 inliers_threshold_ = r;
199 complete_models_ = complete_models;
210 complete_normal_models_ = complete_models;
223 if(!occlusion_cloud_set_) {
224 PCL_WARN(
"Occlusion cloud not set, using scene_cloud instead...\n");
225 occlusion_cloud_ = scene_cloud_;
228 if (!occlusion_reasoning)
229 visible_models_ = models;
233 if (scene_cloud_ ==
nullptr)
235 PCL_ERROR(
"setSceneCloud should be called before adding the model if reasoning about occlusions...");
240 PCL_WARN(
"Scene not organized... filtering using computed depth buffer\n");
249 for (std::size_t i = 0; i < models.size (); i++)
256 std::vector<int> indices;
257 zbuffer_self_occlusion.
filter (models[i], indices, 0.005f);
260 if(normals_set_ && requires_normals_) {
263 visible_normal_models_.push_back(filtered_normals);
271 filtered = pcl::occlusion_reasoning::filter<ModelT,SceneT> (occlusion_cloud_, const_filtered, 525.f, occlusion_thres_);
275 zbuffer_scene.
filter (const_filtered, filtered, occlusion_thres_);
278 visible_models_.push_back (filtered);
281 complete_models_ = models;
284 occlusion_cloud_set_ =
false;
285 normals_set_ =
false;
297 complete_models_.clear();
298 visible_models_.clear();
299 visible_normal_models_.clear();
301 scene_cloud_ = scene_cloud;
306 voxel_grid.
setLeafSize (resolution_, resolution_, resolution_);
308 voxel_grid.
filter (*scene_cloud_downsampled_);
312 scene_downsampled_tree_->
setInputCloud(scene_cloud_downsampled_);
317 occlusion_cloud_ = occ_cloud;
318 occlusion_cloud_set_ =
true;
shared_ptr< KdTree< PointT, Tree > > Ptr
pcl::PointCloud< SceneT >::Ptr scene_cloud_downsampled_
shared_ptr< PointCloud< PointT > > Ptr
int zbuffer_scene_resolution_
int zbuffer_self_occlusion_resolution_
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< 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.
void setOcclusionThreshold(float t)
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
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 setResolution(float r)
void setInlierThreshold(float r)
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
std::vector< bool > mask_
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.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
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_
bool getRequiresNormals()
bool occlusion_cloud_set_
void getMask(std::vector< bool > &mask)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
shared_ptr< const PointCloud< PointT > > ConstPtr
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
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.