occlusion_reasoning.h
/*
* 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/common/io.h>
namespace pcl
{
namespace occlusion_reasoning
{
/**
* \brief Class to reason about occlusions
* \author Aitor Aldoma
*/
template<typename ModelT, typename SceneT>
class ZBuffering
{
private:
float f_;
int cx_, cy_;
float * depth_;
public:
ZBuffering ();
ZBuffering (int resx, int resy, float f);
~ZBuffering ();
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);
void filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, pcl::Indices & indices, float thres = 0.01);
};
template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr
filter (typename pcl::PointCloud<SceneT>::ConstPtr & organized_cloud, typename pcl::PointCloud<ModelT>::ConstPtr & to_be_filtered, float f,
float threshold)
{
float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f);
float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f);
typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
pcl::Indices indices_to_keep;
indices_to_keep.resize (to_be_filtered->size ());
int keep = 0;
for (std::size_t i = 0; i < to_be_filtered->size (); i++)
{
float x = (*to_be_filtered)[i].x;
float y = (*to_be_filtered)[i].y;
float z = (*to_be_filtered)[i].z;
int u = static_cast<int> (f * x / z + cx);
int v = static_cast<int> (f * y / z + cy);
//Not out of bounds
if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0))
continue;
//Check for invalid depth
if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y)
|| !std::isfinite (organized_cloud->at (u, v).z))
continue;
float z_oc = organized_cloud->at (u, v).z;
//Check if point depth (distance to camera) is greater than the (u,v)
if ((z - z_oc) > threshold)
continue;
indices_to_keep[keep] = static_cast<int> (i);
keep++;
}
indices_to_keep.resize (keep);
pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered);
return filtered;
}
template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr
filter (typename pcl::PointCloud<SceneT>::Ptr & organized_cloud, typename pcl::PointCloud<ModelT>::Ptr & to_be_filtered, float f,
float threshold, bool check_invalid_depth = true)
{
float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f);
float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f);
typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
std::vector<int> indices_to_keep;
indices_to_keep.resize (to_be_filtered->size ());
int keep = 0;
for (std::size_t i = 0; i < to_be_filtered->size (); i++)
{
float x = (*to_be_filtered)[i].x;
float y = (*to_be_filtered)[i].y;
float z = (*to_be_filtered)[i].z;
int u = static_cast<int> (f * x / z + cx);
int v = static_cast<int> (f * y / z + cy);
//Not out of bounds
if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0))
continue;
//Check for invalid depth
if (check_invalid_depth)
{
if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y)
|| !std::isfinite (organized_cloud->at (u, v).z))
continue;
}
float z_oc = organized_cloud->at (u, v).z;
//Check if point depth (distance to camera) is greater than the (u,v)
if ((z - z_oc) > threshold)
continue;
indices_to_keep[keep] = static_cast<int> (i);
keep++;
}
indices_to_keep.resize (keep);
pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered);
return filtered;
}
template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr
getOccludedCloud (typename pcl::PointCloud<SceneT>::Ptr & organized_cloud, typename pcl::PointCloud<ModelT>::Ptr & to_be_filtered, float f,
float threshold, bool check_invalid_depth = true)
{
float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f);
float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f);
typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
std::vector<int> indices_to_keep;
indices_to_keep.resize (to_be_filtered->size ());
int keep = 0;
for (std::size_t i = 0; i < to_be_filtered->size (); i++)
{
float x = (*to_be_filtered)[i].x;
float y = (*to_be_filtered)[i].y;
float z = (*to_be_filtered)[i].z;
int u = static_cast<int> (f * x / z + cx);
int v = static_cast<int> (f * y / z + cy);
//Out of bounds
if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0))
continue;
//Check for invalid depth
if (check_invalid_depth)
{
if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y)
|| !std::isfinite (organized_cloud->at (u, v).z))
continue;
}
float z_oc = organized_cloud->at (u, v).z;
//Check if point depth (distance to camera) is greater than the (u,v)
if ((z - z_oc) > threshold)
{
indices_to_keep[keep] = static_cast<int> (i);
keep++;
}
}
indices_to_keep.resize (keep);
pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered);
return filtered;
}
}
}
#ifdef PCL_NO_PRECOMPILE
#include <pcl/recognition/impl/hv/occlusion_reasoning.hpp>
#endif
pcl::PointCloud< ModelT >
pcl::PointCloud::at
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:262
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
pcl::occlusion_reasoning::ZBuffering
Class to reason about occlusions.
Definition: occlusion_reasoning.h:53
pcl::occlusion_reasoning::ZBuffering::~ZBuffering
~ZBuffering()
Definition: occlusion_reasoning.hpp:60
pcl::occlusion_reasoning::ZBuffering::computeDepthMap
void computeDepthMap(typename pcl::PointCloud< SceneT >::ConstPtr &scene, bool compute_focal=false, bool smooth=false, int wsize=3)
Definition: occlusion_reasoning.hpp:111
pcl::occlusion_reasoning::ZBuffering::filter
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
Definition: occlusion_reasoning.hpp:67
pcl::occlusion_reasoning::ZBuffering::ZBuffering
ZBuffering()
Definition: occlusion_reasoning.hpp:53
pcl::copyPointCloud
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:144
pcl::occlusion_reasoning::filter
pcl::PointCloud< ModelT >::Ptr filter(typename pcl::PointCloud< SceneT >::ConstPtr &organized_cloud, typename pcl::PointCloud< ModelT >::ConstPtr &to_be_filtered, float f, float threshold)
Definition: occlusion_reasoning.h:72
pcl::occlusion_reasoning::getOccludedCloud
pcl::PointCloud< ModelT >::Ptr getOccludedCloud(typename pcl::PointCloud< SceneT >::Ptr &organized_cloud, typename pcl::PointCloud< ModelT >::Ptr &to_be_filtered, float f, float threshold, bool check_invalid_depth=true)
Definition: occlusion_reasoning.h:163
pcl
Definition: convolution.h:46
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133