Loading...
Searching...
No Matches
convolution_3d.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, 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 the copyright holder(s) 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.
*
* $Id$
*
*/
#ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
#define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
#include <pcl/common/point_tests.h> // for isFinite
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
#include <pcl/pcl_config.h>
#include <pcl/point_types.h>
#include <pcl/common/point_tests.h>
#include <cmath>
#include <cstdint>
#include <limits>
#include <vector>
///////////////////////////////////////////////////////////////////////////////////////////////////
namespace pcl
{
namespace filters
{
template <typename PointT>
class ConvolvingKernel<PointT, pcl::Normal>
{
void
makeInfinite (pcl::Normal& n)
{
n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN ();
}
};
template <typename PointT> class
ConvolvingKernel<PointT, pcl::PointXY>
{
void
makeInfinite (pcl::PointXY& p)
{
p.x = p.y = std::numeric_limits<float>::quiet_NaN ();
}
};
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointOutT> bool
pcl::filters::GaussianKernel<PointInT, PointOutT>::initCompute ()
{
if (sigma_ == 0)
{
PCL_ERROR ("Sigma is not set or equal to 0!\n", sigma_);
return (false);
}
sigma_sqr_ = sigma_ * sigma_;
if (sigma_coefficient_)
{
if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3)
{
PCL_ERROR ("Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_));
return (false);
}
else
threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_;
}
return (true);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointOutT> PointOutT
pcl::filters::GaussianKernel<PointInT, PointOutT>::operator() (const Indices& indices,
const std::vector<float>& distances)
{
using namespace pcl::common;
PointOutT result;
float total_weight = 0;
std::vector<float>::const_iterator dist_it = distances.begin ();
for (Indices::const_iterator idx_it = indices.begin ();
idx_it != indices.end ();
++idx_it, ++dist_it)
{
if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it]))
{
float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
result += weight * (*input_) [*idx_it];
total_weight += weight;
}
}
if (total_weight != 0)
result /= total_weight;
else
makeInfinite (result);
return (result);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointOutT> PointOutT
pcl::filters::GaussianKernelRGB<PointInT, PointOutT>::operator() (const Indices& indices, const std::vector<float>& distances)
{
using namespace pcl::common;
PointOutT result;
float total_weight = 0;
float r = 0, g = 0, b = 0;
std::vector<float>::const_iterator dist_it = distances.begin ();
for (Indices::const_iterator idx_it = indices.begin ();
idx_it != indices.end ();
++idx_it, ++dist_it)
{
if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it]))
{
float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
result.x += weight * (*input_) [*idx_it].x;
result.y += weight * (*input_) [*idx_it].y;
result.z += weight * (*input_) [*idx_it].z;
r += weight * static_cast<float> ((*input_) [*idx_it].r);
g += weight * static_cast<float> ((*input_) [*idx_it].g);
b += weight * static_cast<float> ((*input_) [*idx_it].b);
total_weight += weight;
}
}
if (total_weight != 0)
{
total_weight = 1.f/total_weight;
r*= total_weight; g*= total_weight; b*= total_weight;
result.x*= total_weight; result.y*= total_weight; result.z*= total_weight;
result.r = static_cast<std::uint8_t> (r);
result.g = static_cast<std::uint8_t> (g);
result.b = static_cast<std::uint8_t> (b);
}
else
makeInfinite (result);
return (result);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename KernelT>
pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::Convolution3D ()
: PCLBase <PointInT> ()
, surface_ ()
, tree_ ()
, search_radius_ (0)
{}
///////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename KernelT> bool
pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::initCompute ()
{
if (!PCLBase<PointInT>::initCompute ())
{
PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed!\n");
return (false);
}
// Initialize the spatial locator
if (!tree_)
{
if (input_->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
else
tree_.reset (new pcl::search::KdTree<PointInT> (false));
}
// If no search surface has been defined, use the input dataset as the search surface itself
if (!surface_)
surface_ = input_;
// Send the surface dataset to the spatial locator
tree_->setInputCloud (surface_);
// Do a fast check to see if the search parameters are well defined
if (search_radius_ <= 0.0)
{
PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0\n",
search_radius_);
return (false);
}
// Make sure the provided kernel implements the required interface
if (dynamic_cast<ConvolvingKernel<PointInT, PointOutT>* > (&kernel_) == 0)
{
PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed : ");
PCL_ERROR ("kernel_ must implement ConvolvingKernel interface\n!");
return (false);
}
kernel_.setInputCloud (surface_);
// Initialize convolving kernel
if (!kernel_.initCompute ())
{
PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
return (false);
}
return (true);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename KernelT> void
pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::convolve (PointCloudOut& output)
{
if (!initCompute ())
{
PCL_ERROR ("[pcl::filters::Convlution3D::convolve] init failed!\n");
return;
}
output.resize (surface_->size ());
output.width = surface_->width;
output.height = surface_->height;
output.is_dense = surface_->is_dense;
Indices nn_indices;
std::vector<float> nn_distances;
#pragma omp parallel for \
default(none) \
shared(output) \
firstprivate(nn_indices, nn_distances) \
num_threads(threads_) \
schedule(dynamic, 64)
for (std::int64_t point_idx = 0; point_idx < static_cast<std::int64_t> (surface_->size ()); ++point_idx)
{
const PointInT& point_in = surface_->points [point_idx];
PointOutT& point_out = output [point_idx];
if (isFinite (point_in) &&
tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
{
point_out = kernel_ (nn_indices, nn_distances);
}
else
{
kernel_.makeInfinite (point_out);
output.is_dense = false;
}
}
}
#endif
pcl::PCLBase< PointIn >::input_
PointCloudConstPtr input_
Definition pcl_base.h:147
pcl::PCLBase::initCompute
bool initCompute()
This method should get called before starting the actual computation.
Definition pcl_base.hpp:138
pcl::PCLBase< PointIn >::PCLBase
PCLBase()
Definition pcl_base.hpp:46
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition point_cloud.h:404
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition point_cloud.h:463
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition point_cloud.h:399
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition point_cloud.h:401
pcl::filters::Convolution3D::initCompute
bool initCompute()
initialize computation
Definition convolution_3d.hpp:188
pcl::filters::Convolution3D::kernel_
KernelT kernel_
convlving kernel
Definition convolution_3d.h:278
pcl::filters::Convolution3D::tree_
KdTreePtr tree_
A pointer to the spatial search object.
Definition convolution_3d.h:269
pcl::filters::Convolution3D::surface_
PointCloudInConstPtr surface_
An input point cloud describing the surface that is to be used for nearest neighbors estimation.
Definition convolution_3d.h:266
pcl::filters::Convolution3D::convolve
void convolve(PointCloudOut &output)
Convolve point cloud.
Definition convolution_3d.hpp:234
pcl::filters::Convolution3D::PointCloudOut
pcl::PointCloud< PointOut > PointCloudOut
Definition convolution_3d.h:203
pcl::filters::Convolution3D::Convolution3D
Convolution3D()
Constructor.
Definition convolution_3d.hpp:179
pcl::filters::Convolution3D::search_radius_
double search_radius_
The nearest neighbors search radius for each point.
Definition convolution_3d.h:272
pcl::filters::ConvolvingKernel
Class ConvolvingKernel base class for all convolving kernels.
Definition convolution_3d.h:56
pcl::filters::ConvolvingKernel< PointT, pcl::Normal >::ConvolvingKernel
ConvolvingKernel()
Definition convolution_3d.h:64
pcl::filters::ConvolvingKernel::makeInfinite
static void makeInfinite(PointOutT &p)
Utility function that annihilates a point making it fail the pcl::isFinite test.
Definition convolution_3d.h:100
pcl::filters::ConvolvingKernel::input_
PointCloudInConstPtr input_
source cloud
Definition convolution_3d.h:107
pcl::filters::GaussianKernel::threshold_
float threshold_
Definition convolution_3d.h:160
pcl::filters::GaussianKernel::sigma_coefficient_
boost::optional< float > sigma_coefficient_
Definition convolution_3d.h:161
pcl::filters::GaussianKernel::sigma_sqr_
float sigma_sqr_
Definition convolution_3d.h:159
pcl::filters::GaussianKernel::sigma_
float sigma_
Definition convolution_3d.h:158
pcl::filters::GaussianKernel::operator()
virtual PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
Definition convolution_3d.hpp:109
pcl::filters::GaussianKernel::initCompute
bool initCompute()
Must call this method before doing any computation.
Definition convolution_3d.hpp:84
pcl::filters::GaussianKernelRGB::operator()
PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
Definition convolution_3d.hpp:138
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
pcl::search::OrganizedNeighbor
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Definition organized.h:66
point_types.h
Defines all the PCL implemented PointT point type structures.
pcl::common
Definition generate.h:50
pcl::distances
Definition distances.h:50
pcl::filters
Definition convolution.h:47
pcl
Definition convolution.h:46
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition point_types.hpp:794
pcl::PointXY
A 2D point structure representing Euclidean xy coordinates.
Definition point_types.hpp:716
pcl::PointXY::y
float y
Definition point_types.hpp:723
pcl::PointXY::x
float x
Definition point_types.hpp:722