330 lines
10 KiB
C
330 lines
10 KiB
C
|
/*
|
||
|
|
* Software License Agreement (BSD License)
|
||
|
|
*
|
||
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
||
|
|
* Copyright (c) 2010-2011, Willow Garage, Inc.
|
||
|
|
*
|
||
|
|
* All rights reserved.
|
||
|
|
*
|
||
|
|
* Redistribution and use in source and binary forms, with or without
|
||
|
|
* modification, are permitted provided that the following conditions
|
||
|
|
* are met:
|
||
|
|
*
|
||
|
|
* * Redistributions of source code must retain the above copyright
|
||
|
|
* notice, this list of conditions and the following disclaimer.
|
||
|
|
* * Redistributions in binary form must reproduce the above
|
||
|
|
* copyright notice, this list of conditions and the following
|
||
|
|
* disclaimer in the documentation and/or other materials provided
|
||
|
|
* with the distribution.
|
||
|
|
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||
|
|
* contributors may be used to endorse or promote products derived
|
||
|
|
* from this software without specific prior written permission.
|
||
|
|
*
|
||
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <pcl/pcl_macros.h>
|
||
|
|
#include "pcl/recognition/hv/occlusion_reasoning.h"
|
||
|
|
#include "pcl/recognition/impl/hv/occlusion_reasoning.hpp"
|
||
|
|
#include <pcl/search/kdtree.h>
|
||
|
|
#include <pcl/filters/voxel_grid.h>
|
||
|
|
|
||
|
|
namespace pcl
|
||
|
|
{
|
||
|
|
|
||
|
|
/**
|
||
|
|
* \brief Abstract class for hypotheses verification methods
|
||
|
|
* \author Aitor Aldoma, Federico Tombari
|
||
|
|
*/
|
||
|
|
|
||
|
|
template<typename ModelT, typename SceneT>
|
||
|
|
class PCL_EXPORTS HypothesisVerification
|
||
|
|
{
|
||
|
|
|
||
|
|
protected:
|
||
|
|
/*
|
||
|
|
* \brief Boolean vector indicating if a hypothesis is accepted/rejected (output of HV stage)
|
||
|
|
*/
|
||
|
|
std::vector<bool> mask_;
|
||
|
|
/*
|
||
|
|
* \brief Scene point cloud
|
||
|
|
*/
|
||
|
|
typename pcl::PointCloud<SceneT>::ConstPtr scene_cloud_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Scene point cloud
|
||
|
|
*/
|
||
|
|
typename pcl::PointCloud<SceneT>::ConstPtr occlusion_cloud_;
|
||
|
|
|
||
|
|
bool occlusion_cloud_set_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Downsampled scene point cloud
|
||
|
|
*/
|
||
|
|
typename pcl::PointCloud<SceneT>::Ptr scene_cloud_downsampled_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Scene tree of the downsampled cloud
|
||
|
|
*/
|
||
|
|
typename pcl::search::KdTree<SceneT>::Ptr scene_downsampled_tree_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Vector of point clouds representing the 3D models after occlusion reasoning
|
||
|
|
* the 3D models are pruned of occluded points, and only visible points are left.
|
||
|
|
* the coordinate system is that of the scene cloud
|
||
|
|
*/
|
||
|
|
typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> visible_models_;
|
||
|
|
|
||
|
|
std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> visible_normal_models_;
|
||
|
|
/*
|
||
|
|
* \brief Vector of point clouds representing the complete 3D model (in same coordinates as the scene cloud)
|
||
|
|
*/
|
||
|
|
typename std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> complete_models_;
|
||
|
|
|
||
|
|
std::vector<typename pcl::PointCloud<pcl::Normal>::ConstPtr> complete_normal_models_;
|
||
|
|
/*
|
||
|
|
* \brief Resolutions in pixel for the depth scene buffer
|
||
|
|
*/
|
||
|
|
int zbuffer_scene_resolution_;
|
||
|
|
/*
|
||
|
|
* \brief Resolutions in pixel for the depth model self-occlusion buffer
|
||
|
|
*/
|
||
|
|
int zbuffer_self_occlusion_resolution_;
|
||
|
|
/*
|
||
|
|
* \brief The resolution of models and scene used to verify hypotheses (in meters)
|
||
|
|
*/
|
||
|
|
float resolution_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Threshold for inliers
|
||
|
|
*/
|
||
|
|
float inliers_threshold_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Threshold to consider a point being occluded
|
||
|
|
*/
|
||
|
|
float occlusion_thres_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Whether the HV method requires normals or not, by default = false
|
||
|
|
*/
|
||
|
|
bool requires_normals_;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Whether the normals have been set
|
||
|
|
*/
|
||
|
|
bool normals_set_;
|
||
|
|
public:
|
||
|
|
|
||
|
|
HypothesisVerification ()
|
||
|
|
{
|
||
|
|
zbuffer_scene_resolution_ = 100;
|
||
|
|
zbuffer_self_occlusion_resolution_ = 150;
|
||
|
|
resolution_ = 0.005f;
|
||
|
|
inliers_threshold_ = static_cast<float>(resolution_);
|
||
|
|
occlusion_cloud_set_ = false;
|
||
|
|
occlusion_thres_ = 0.005f;
|
||
|
|
normals_set_ = false;
|
||
|
|
requires_normals_ = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual
|
||
|
|
~HypothesisVerification() = default;
|
||
|
|
|
||
|
|
bool getRequiresNormals() {
|
||
|
|
return requires_normals_;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Sets the resolution of scene cloud and models used to verify hypotheses
|
||
|
|
* mask r resolution
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
setResolution(float r) {
|
||
|
|
resolution_ = r;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Sets the occlusion threshold
|
||
|
|
* mask t threshold
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
setOcclusionThreshold(float t) {
|
||
|
|
occlusion_thres_ = t;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Sets the resolution of scene cloud and models used to verify hypotheses
|
||
|
|
* mask r resolution
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
setInlierThreshold(float r) {
|
||
|
|
inliers_threshold_ = r;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Returns a vector of booleans representing which hypotheses have been accepted/rejected (true/false)
|
||
|
|
* mask vector of booleans
|
||
|
|
*/
|
||
|
|
|
||
|
|
void
|
||
|
|
getMask (std::vector<bool> & mask)
|
||
|
|
{
|
||
|
|
mask = mask_;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Sets the 3D complete models. NOTE: If addModels is called with occlusion_reasoning=true, then
|
||
|
|
* there is no need to call this function.
|
||
|
|
* mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_)
|
||
|
|
*/
|
||
|
|
|
||
|
|
void
|
||
|
|
addCompleteModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & complete_models)
|
||
|
|
{
|
||
|
|
complete_models_ = complete_models;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Sets the normals of the 3D complete models and sets normals_set_ to true.
|
||
|
|
* Normals need to be added before calling the addModels method.
|
||
|
|
* complete_models The normals of the models.
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
addNormalsClouds (std::vector<pcl::PointCloud<pcl::Normal>::ConstPtr> & complete_models)
|
||
|
|
{
|
||
|
|
complete_normal_models_ = complete_models;
|
||
|
|
normals_set_ = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Sets the models (recognition hypotheses) - requires the scene_cloud_ to be set first if reasoning about occlusions
|
||
|
|
* mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_)
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
addModels (std::vector<typename pcl::PointCloud<ModelT>::ConstPtr> & models, bool occlusion_reasoning = false)
|
||
|
|
{
|
||
|
|
|
||
|
|
mask_.clear();
|
||
|
|
if(!occlusion_cloud_set_) {
|
||
|
|
PCL_WARN("Occlusion cloud not set, using scene_cloud instead...\n");
|
||
|
|
occlusion_cloud_ = scene_cloud_;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!occlusion_reasoning)
|
||
|
|
visible_models_ = models;
|
||
|
|
else
|
||
|
|
{
|
||
|
|
//we need to reason about occlusions before setting the model
|
||
|
|
if (scene_cloud_ == nullptr)
|
||
|
|
{
|
||
|
|
PCL_ERROR("setSceneCloud should be called before adding the model if reasoning about occlusions...\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!occlusion_cloud_->isOrganized ())
|
||
|
|
{
|
||
|
|
PCL_WARN("Scene not organized... filtering using computed depth buffer\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_scene (zbuffer_scene_resolution_, zbuffer_scene_resolution_, 1.f);
|
||
|
|
if (!occlusion_cloud_->isOrganized ())
|
||
|
|
{
|
||
|
|
zbuffer_scene.computeDepthMap (occlusion_cloud_, true);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < models.size (); i++)
|
||
|
|
{
|
||
|
|
|
||
|
|
//self-occlusions
|
||
|
|
typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
|
||
|
|
typename pcl::occlusion_reasoning::ZBuffering<ModelT, SceneT> zbuffer_self_occlusion (75, 75, 1.f);
|
||
|
|
zbuffer_self_occlusion.computeDepthMap (models[i], true);
|
||
|
|
pcl::Indices indices;
|
||
|
|
zbuffer_self_occlusion.filter (models[i], indices, 0.005f);
|
||
|
|
pcl::copyPointCloud (*models[i], indices, *filtered);
|
||
|
|
|
||
|
|
if(normals_set_ && requires_normals_) {
|
||
|
|
pcl::PointCloud<pcl::Normal>::Ptr filtered_normals (new pcl::PointCloud<pcl::Normal> ());
|
||
|
|
pcl::copyPointCloud(*complete_normal_models_[i], indices, *filtered_normals);
|
||
|
|
visible_normal_models_.push_back(filtered_normals);
|
||
|
|
}
|
||
|
|
|
||
|
|
typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*filtered));
|
||
|
|
//typename pcl::PointCloud<ModelT>::ConstPtr const_filtered(new pcl::PointCloud<ModelT> (*models[i]));
|
||
|
|
//scene-occlusions
|
||
|
|
if (occlusion_cloud_->isOrganized ())
|
||
|
|
{
|
||
|
|
filtered = pcl::occlusion_reasoning::filter<ModelT,SceneT> (occlusion_cloud_, const_filtered, 525.f, occlusion_thres_);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
zbuffer_scene.filter (const_filtered, filtered, occlusion_thres_);
|
||
|
|
}
|
||
|
|
|
||
|
|
visible_models_.push_back (filtered);
|
||
|
|
}
|
||
|
|
|
||
|
|
complete_models_ = models;
|
||
|
|
}
|
||
|
|
|
||
|
|
occlusion_cloud_set_ = false;
|
||
|
|
normals_set_ = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Sets the scene cloud
|
||
|
|
* scene_cloud Point cloud representing the scene
|
||
|
|
*/
|
||
|
|
|
||
|
|
void
|
||
|
|
setSceneCloud (const typename pcl::PointCloud<SceneT>::Ptr & scene_cloud)
|
||
|
|
{
|
||
|
|
|
||
|
|
complete_models_.clear();
|
||
|
|
visible_models_.clear();
|
||
|
|
visible_normal_models_.clear();
|
||
|
|
|
||
|
|
scene_cloud_ = scene_cloud;
|
||
|
|
scene_cloud_downsampled_.reset(new pcl::PointCloud<SceneT>());
|
||
|
|
|
||
|
|
pcl::VoxelGrid<SceneT> voxel_grid;
|
||
|
|
voxel_grid.setInputCloud (scene_cloud);
|
||
|
|
voxel_grid.setLeafSize (resolution_, resolution_, resolution_);
|
||
|
|
voxel_grid.setDownsampleAllData(true);
|
||
|
|
voxel_grid.filter (*scene_cloud_downsampled_);
|
||
|
|
|
||
|
|
//initialize kdtree for search
|
||
|
|
scene_downsampled_tree_.reset (new pcl::search::KdTree<SceneT>);
|
||
|
|
scene_downsampled_tree_->setInputCloud(scene_cloud_downsampled_);
|
||
|
|
}
|
||
|
|
|
||
|
|
void setOcclusionCloud (const typename pcl::PointCloud<SceneT>::Ptr & occ_cloud)
|
||
|
|
{
|
||
|
|
occlusion_cloud_ = occ_cloud;
|
||
|
|
occlusion_cloud_set_ = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* \brief Function that performs the hypotheses verification, needs to be implemented in the subclasses
|
||
|
|
* This function modifies the values of mask_ and needs to be called after both scene and model have been added
|
||
|
|
*/
|
||
|
|
|
||
|
|
virtual void
|
||
|
|
verify ()=0;
|
||
|
|
};
|
||
|
|
|
||
|
|
}
|