void
pcl::GaussianKernel::compute (float sigma,
Eigen::VectorXf &kernel,
Eigen::VectorXf &derivative,
unsigned kernel_width) const
{
assert (kernel_width %2 == 1);
assert (sigma >= 0);
kernel.resize (kernel_width);
derivative.resize (kernel_width);
const float factor = 0.01f;
float max_gauss = 1.0f, max_deriv = float (sigma * exp (-0.5));
int hw = kernel_width / 2;
float sigma_sqr = 1.0f / (2.0f * sigma * sigma);
for (int i = -hw, j = 0, k = kernel_width - 1; i < 0 ; i++, j++, k--)
{
kernel[k] = kernel[j] = expf (-static_cast<float>(i) * static_cast<float>(i) * sigma_sqr);
derivative[j] = -static_cast<float>(i) * kernel[j];
derivative[k] = -derivative[j];
}
kernel[hw] = 1;
derivative[hw] = 0;
// Compute kernel and derivative true width
unsigned g_width = kernel_width;
unsigned d_width = kernel_width;
for (unsigned i = 0; fabs (derivative[i]/max_deriv) < factor; i++, d_width-= 2) ;
for (unsigned i = 0; fabs (kernel[i]/max_gauss) < factor; i++, g_width-= 2) ;
if (g_width == kernel_width || d_width == kernel_width)
{
PCL_THROW_EXCEPTION (KernelWidthTooSmallException,
"kernel width " << kernel_width
<< "is too small for the given sigma " << sigma);
return;
}
// Shift and resize if width less than kernel_width
// Kernel
unsigned shift = (kernel_width - g_width)/2;
for (unsigned i =0; i < g_width; i++)
kernel[i] = kernel[i + shift];
// Normalize kernel
kernel.conservativeResize (g_width);
kernel/= kernel.sum ();
// Derivative
shift = (kernel_width - d_width)/2;
for (unsigned i =0; i < d_width; i++)
derivative[i] = derivative[i + shift];
derivative.conservativeResize (d_width);
// Normalize derivative
hw = d_width / 2;
float den = 0;
for (int i = -hw ; i <= hw ; i++)
den -= static_cast<float>(i) * derivative[i+hw];
derivative/= den;
}
void
pcl::GaussianKernel::compute (float sigma,
Eigen::VectorXf &kernel,
unsigned kernel_width) const
{
assert (kernel_width %2 == 1);
assert (sigma >= 0);
kernel.resize (kernel_width);
static const float factor = 0.01f;
static const float max_gauss = 1.0f;
const int hw = kernel_width / 2;
for (int i = -hw, j = 0, k = kernel_width - 1; i < 0 ; i++, j++, k--)
kernel[k] = kernel[j] = float (exp (-i*i / (2*sigma*sigma)));
kernel[hw] = 1;
unsigned g_width = kernel_width;
for (unsigned i = 0; fabs (kernel[i]/max_gauss) < factor; i++, g_width-= 2);
if (g_width == kernel_width)
{
PCL_THROW_EXCEPTION (pcl::KernelWidthTooSmallException,
"kernel width " << kernel_width
<< "is too small for the given sigma " << sigma);
return;
}
// Shift and resize if width less than kernel_width
unsigned shift = (kernel_width - g_width)/2;
for (unsigned i =0; i < g_width; i++)
kernel[i] = kernel[i + shift];
kernel.conservativeResize (g_width);
// Normalize
kernel/= kernel.sum ();
}