214 lines
7.9 KiB
C
214 lines
7.9 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/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
|