//TODO need fix a bug on this function
kernelType FBlur::GaussianBlur(float sigma){
int kSize = 2*std::round(std::sqrt(-std::log(0.3)*2*sigma*sigma)) + 1;
float squareSigma = sigma*sigma;
float constant = 2 * M_PI * squareSigma;
kernelType gaussianKernel(kSize, kernelRow(kSize));
for (int i = 0; i <= kSize; i++) {
for (int j = 0; j <= kSize; j++) {
gaussianKernel[i][j]
= (1 / constant) * std::exp(-(i * i + j * j ) / (squareSigma));
}
}
return gaussianKernel;
}
void
pcl::pcl_2d::kernel<PointT>::fetchKernel (pcl::PointCloud<PointT> &kernel)
{
switch (kernel_type_)
{
case SOBEL_X:
sobelKernelX (kernel);
break;
case SOBEL_Y:
sobelKernelY (kernel);
break;
case PREWITT_X:
prewittKernelX (kernel);
break;
case PREWITT_Y:
prewittKernelY (kernel);
break;
case ROBERTS_X:
robertsKernelX (kernel);
break;
case ROBERTS_Y:
robertsKernelY (kernel);
break;
case LOG:
loGKernel (kernel);
break;
case DERIVATIVE_CENTRAL_X:
derivativeXCentralKernel (kernel);
break;
case DERIVATIVE_FORWARD_X:
derivativeXForwardKernel (kernel);
break;
case DERIVATIVE_BACKWARD_X:
derivativeXBackwardKernel (kernel);
break;
case DERIVATIVE_CENTRAL_Y:
derivativeYCentralKernel (kernel);
break;
case DERIVATIVE_FORWARD_Y:
derivativeYForwardKernel (kernel);
break;
case DERIVATIVE_BACKWARD_Y:
derivativeYBackwardKernel (kernel);
break;
case GAUSSIAN:
gaussianKernel (kernel);
}
}
/*Compute a similartiy measure (specified in inputParams) between sparse vectors x and z*/
real similarity(SparseDim* x, SparseDim* z, InputParams* inputParams) {
if(!inputParams->simParams || (int)inputParams->simParams[1] == 1)
return dotProduct(x,z);
else {
switch((int)inputParams->simParams[1]) {
case 2 :
return invDistance(x,z,inputParams->simParams[2]);
case 3 :
return polyKernel(x,z,inputParams->simParams[2],
inputParams->simParams[3],
inputParams->simParams[4]);
case 4 :
return gaussianKernel(x,z,inputParams->simParams[2]);
}
}
fprintf(stderr,"Similarity error \n");
exit(EXIT_FAILURE);
}
// Compatible function
array gaussiankernel(const int rows, const int cols,
const double sig_r, const double sig_c)
{
return gaussianKernel(rows, cols, sig_r, sig_c);
}
void
pcl::pcl_2d::convolution_2d::gaussianSmooth (ImageType &input, ImageType &output, const int kernel_size, const float sigma){
ImageType kernel;
gaussianKernel(kernel_size, sigma, kernel);
conv(output, kernel, input);
}
/**
* The main program.
*
* \param argc The number of arguments
* \param argv An array containing the arguments as C-strings
*
* \return Exit code
*
* \author Jens Keiner
*/
int main (int argc, char **argv)
{
double **p; /* The array containing the parameter sets *
* for the kernel functions */
int *m; /* The array containing the cut-off degrees M */
int **ld; /* The array containing the numbers of source *
* and target nodes, L and D */
int ip; /* Index variable for p */
int im; /* Index variable for m */
int ild; /* Index variable for l */
int ipp; /* Index for kernel parameters */
int ip_max; /* The maximum index for p */
int im_max; /* The maximum index for m */
int ild_max; /* The maximum index for l */
int ipp_max; /* The maximum index for ip */
int tc_max; /* The number of testcases */
int m_max; /* The maximum cut-off degree M for the *
* current dataset */
int l_max; /* The maximum number of source nodes L for *
* the current dataset */
int d_max; /* The maximum number of target nodes D for *
* the current dataset */
long ld_max_prec; /* The maximum number of source and target *
* nodes for precomputation multiplied */
long l_max_prec; /* The maximum number of source nodes for *
* precomputation */
int tc; /* Index variable for testcases */
int kt; /* The kernel function */
int cutoff; /* The current NFFT cut-off parameter */
double threshold; /* The current NFSFT threshold parameter */
double t_d; /* Time for direct algorithm in seconds */
double t_dp; /* Time for direct algorithm with *
precomputation in seconds */
double t_fd; /* Time for fast direct algorithm in seconds */
double t_f; /* Time for fast algorithm in seconds */
double temp; /* */
double err_f; /* Error E_infty for fast algorithm */
double err_fd; /* Error E_\infty for fast direct algorithm */
ticks t0, t1; /* */
int precompute = NO; /* */
fftw_complex *ptr; /* */
double* steed; /* */
fftw_complex *b; /* The weights (b_l)_{l=0}^{L-1} */
fftw_complex *f_hat; /* The spherical Fourier coefficients */
fftw_complex *a; /* The Fourier-Legendre coefficients */
double *xi; /* Target nodes */
double *eta; /* Source nodes */
fftw_complex *f_m; /* Approximate function values */
fftw_complex *f; /* Exact function values */
fftw_complex *prec = NULL; /* */
nfsft_plan plan; /* NFSFT plan */
nfsft_plan plan_adjoint; /* adjoint NFSFT plan */
int i; /* */
int k; /* */
int n; /* */
int d; /* */
int l; /* */
int use_nfsft; /* */
int use_nfft; /* */
int use_fpt; /* */
int rinc; /* */
double constant; /* */
/* Read the number of testcases. */
fscanf(stdin,"testcases=%d\n",&tc_max);
fprintf(stdout,"%d\n",tc_max);
/* Process each testcase. */
for (tc = 0; tc < tc_max; tc++)
{
/* Check if the fast transform shall be used. */
fscanf(stdin,"nfsft=%d\n",&use_nfsft);
fprintf(stdout,"%d\n",use_nfsft);
if (use_nfsft != NO)
{
/* Check if the NFFT shall be used. */
fscanf(stdin,"nfft=%d\n",&use_nfft);
fprintf(stdout,"%d\n",use_nfft);
if (use_nfft != NO)
{
/* Read the cut-off parameter. */
fscanf(stdin,"cutoff=%d\n",&cutoff);
fprintf(stdout,"%d\n",cutoff);
}
else
{
/* TODO remove this */
/* Initialize unused variable with dummy value. */
cutoff = 1;
}
/* Check if the fast polynomial transform shall be used. */
fscanf(stdin,"fpt=%d\n",&use_fpt);
fprintf(stdout,"%d\n",use_fpt);
/* Read the NFSFT threshold parameter. */
fscanf(stdin,"threshold=%lf\n",&threshold);
fprintf(stdout,"%lf\n",threshold);
}
else
{
/* TODO remove this */
/* Set dummy values. */
cutoff = 3;
threshold = 1000000000000.0;
}
/* Initialize bandwidth bound. */
m_max = 0;
/* Initialize source nodes bound. */
l_max = 0;
/* Initialize target nodes bound. */
d_max = 0;
/* Initialize source nodes bound for precomputation. */
l_max_prec = 0;
/* Initialize source and target nodes bound for precomputation. */
ld_max_prec = 0;
/* Read the kernel type. This is one of KT_ABEL_POISSON, KT_SINGULARITY,
* KT_LOC_SUPP and KT_GAUSSIAN. */
fscanf(stdin,"kernel=%d\n",&kt);
fprintf(stdout,"%d\n",kt);
/* Read the number of parameter sets. */
fscanf(stdin,"parameter_sets=%d\n",&ip_max);
fprintf(stdout,"%d\n",ip_max);
/* Allocate memory for pointers to parameter sets. */
p = (double**) nfft_malloc(ip_max*sizeof(double*));
/* We now read in the parameter sets. */
/* Read number of parameters. */
fscanf(stdin,"parameters=%d\n",&ipp_max);
fprintf(stdout,"%d\n",ipp_max);
for (ip = 0; ip < ip_max; ip++)
{
/* Allocate memory for the parameters. */
p[ip] = (double*) nfft_malloc(ipp_max*sizeof(double));
/* Read the parameters. */
for (ipp = 0; ipp < ipp_max; ipp++)
{
/* Read the next parameter. */
fscanf(stdin,"%lf\n",&p[ip][ipp]);
fprintf(stdout,"%lf\n",p[ip][ipp]);
}
}
/* Read the number of cut-off degrees. */
fscanf(stdin,"bandwidths=%d\n",&im_max);
fprintf(stdout,"%d\n",im_max);
m = (int*) nfft_malloc(im_max*sizeof(int));
/* Read the cut-off degrees. */
for (im = 0; im < im_max; im++)
{
/* Read cut-off degree. */
fscanf(stdin,"%d\n",&m[im]);
fprintf(stdout,"%d\n",m[im]);
m_max = MAX(m_max,m[im]);
}
/* Read number of node specifications. */
fscanf(stdin,"node_sets=%d\n",&ild_max);
fprintf(stdout,"%d\n",ild_max);
ld = (int**) nfft_malloc(ild_max*sizeof(int*));
/* Read the run specification. */
for (ild = 0; ild < ild_max; ild++)
{
/* Allocate memory for the run parameters. */
ld[ild] = (int*) nfft_malloc(5*sizeof(int));
/* Read number of source nodes. */
fscanf(stdin,"L=%d ",&ld[ild][0]);
fprintf(stdout,"%d\n",ld[ild][0]);
l_max = MAX(l_max,ld[ild][0]);
/* Read number of target nodes. */
fscanf(stdin,"D=%d ",&ld[ild][1]);
fprintf(stdout,"%d\n",ld[ild][1]);
d_max = MAX(d_max,ld[ild][1]);
/* Determine whether direct and fast algorithm shall be compared. */
fscanf(stdin,"compare=%d ",&ld[ild][2]);
fprintf(stdout,"%d\n",ld[ild][2]);
/* Check if precomputation for the direct algorithm is used. */
if (ld[ild][2] == YES)
{
/* Read whether the precomputed version shall also be used. */
fscanf(stdin,"precomputed=%d\n",&ld[ild][3]);
fprintf(stdout,"%d\n",ld[ild][3]);
/* Read the number of repetitions over which measurements are
* averaged. */
fscanf(stdin,"repetitions=%d\n",&ld[ild][4]);
fprintf(stdout,"%d\n",ld[ild][4]);
/* Update ld_max_prec and l_max_prec. */
if (ld[ild][3] == YES)
{
/* Update ld_max_prec. */
ld_max_prec = MAX(ld_max_prec,ld[ild][0]*ld[ild][1]);
/* Update l_max_prec. */
l_max_prec = MAX(l_max_prec,ld[ild][0]);
/* Turn on the precomputation for the direct algorithm. */
precompute = YES;
}
}
else
{
/* Set default value for the number of repetitions. */
ld[ild][4] = 1;
}
}
/* Allocate memory for data structures. */
b = (fftw_complex*) nfft_malloc(l_max*sizeof(fftw_complex));
eta = (double*) nfft_malloc(2*l_max*sizeof(double));
f_hat = (fftw_complex*) nfft_malloc(NFSFT_F_HAT_SIZE(m_max)*sizeof(fftw_complex));
a = (fftw_complex*) nfft_malloc((m_max+1)*sizeof(fftw_complex));
xi = (double*) nfft_malloc(2*d_max*sizeof(double));
f_m = (fftw_complex*) nfft_malloc(d_max*sizeof(fftw_complex));
f = (fftw_complex*) nfft_malloc(d_max*sizeof(fftw_complex));
/* Allocate memory for precomputed data. */
if (precompute == YES)
{
prec = (fftw_complex*) nfft_malloc(ld_max_prec*sizeof(fftw_complex));
}
/* Generate random source nodes and weights. */
for (l = 0; l < l_max; l++)
{
b[l] = (((double)rand())/RAND_MAX) - 0.5;
eta[2*l] = (((double)rand())/RAND_MAX) - 0.5;
eta[2*l+1] = acos(2.0*(((double)rand())/RAND_MAX) - 1.0)/(K2PI);
}
/* Generate random target nodes. */
for (d = 0; d < d_max; d++)
{
xi[2*d] = (((double)rand())/RAND_MAX) - 0.5;
xi[2*d+1] = acos(2.0*(((double)rand())/RAND_MAX) - 1.0)/(K2PI);
}
/* Do precomputation. */
nfsft_precompute(m_max,threshold,
((use_nfsft==NO)?(NFSFT_NO_FAST_ALGORITHM):(0U/*NFSFT_NO_DIRECT_ALGORITHM*/)), 0U);
/* Process all parameter sets. */
for (ip = 0; ip < ip_max; ip++)
{
/* Compute kernel coeffcients up to the maximum cut-off degree m_max. */
switch (kt)
{
case KT_ABEL_POISSON:
/* Compute Fourier-Legendre coefficients for the Poisson kernel. */
for (k = 0; k <= m_max; k++)
a[k] = SYMBOL_ABEL_POISSON(k,p[ip][0]);
break;
case KT_SINGULARITY:
/* Compute Fourier-Legendre coefficients for the singularity
* kernel. */
for (k = 0; k <= m_max; k++)
a[k] = SYMBOL_SINGULARITY(k,p[ip][0]);
break;
case KT_LOC_SUPP:
/* Compute Fourier-Legendre coefficients for the locally supported
* kernel. */
a[0] = 1.0;
if (1 <= m_max)
a[1] = ((p[ip][1]+1+p[ip][0])/(p[ip][1]+2.0))*a[0];
for (k = 2; k <= m_max; k++)
a[k] = (1.0/(k+p[ip][1]+1))*((2*k-1)*p[ip][0]*a[k-1] -
(k-p[ip][1]-2)*a[k-2]);
break;
case KT_GAUSSIAN:
/* Fourier-Legendre coefficients */
steed = (double*) nfft_malloc((m_max+1)*sizeof(double));
smbi(2.0*p[ip][0],0.5,m_max+1,2,steed);
for (k = 0; k <= m_max; k++)
a[k] = K2PI*(sqrt(KPI/p[ip][0]))*steed[k];
nfft_free(steed);
break;
}
/* Normalize Fourier-Legendre coefficients. */
for (k = 0; k <= m_max; k++)
a[k] *= (2*k+1)/(K4PI);
/* Process all node sets. */
for (ild = 0; ild < ild_max; ild++)
{
/* Check if the fast algorithm shall be used. */
if (ld[ild][2] != NO)
{
/* Check if the direct algorithm with precomputation should be
* tested. */
if (ld[ild][3] != NO)
{
/* Get pointer to start of data. */
ptr = prec;
/* Calculate increment from one row to the next. */
rinc = l_max_prec-ld[ild][0];
/* Process al target nodes. */
for (d = 0; d < ld[ild][1]; d++)
{
/* Process all source nodes. */
for (l = 0; l < ld[ild][0]; l++)
{
/* Compute inner product between current source and target
* node. */
temp = innerProduct(eta[2*l],eta[2*l+1],xi[2*d],xi[2*d+1]);
/* Switch by the kernel type. */
switch (kt)
{
case KT_ABEL_POISSON:
/* Evaluate the Poisson kernel for the current value. */
*ptr++ = poissonKernel(temp,p[ip][0]);
break;
case KT_SINGULARITY:
/* Evaluate the singularity kernel for the current
* value. */
*ptr++ = singularityKernel(temp,p[ip][0]);
break;
case KT_LOC_SUPP:
/* Evaluate the localized kernel for the current
* value. */
*ptr++ = locallySupportedKernel(temp,p[ip][0],p[ip][1]);
break;
case KT_GAUSSIAN:
/* Evaluate the spherical Gaussian kernel for the current
* value. */
*ptr++ = gaussianKernel(temp,p[ip][0]);
break;
}
}
/* Increment pointer for next row. */
ptr += rinc;
}
/* Initialize cumulative time variable. */
t_dp = 0.0;
/* Initialize time measurement. */
t0 = getticks();
/* Cycle through all runs. */
for (i = 0; i < ld[ild][4]; i++)
{
/* Reset pointer to start of precomputed data. */
ptr = prec;
/* Calculate increment from one row to the next. */
rinc = l_max_prec-ld[ild][0];
/* Check if the localized kernel is used. */
if (kt == KT_LOC_SUPP)
{
/* Perform final summation */
/* Calculate the multiplicative constant. */
constant = ((p[ip][1]+1)/(K2PI*pow(1-p[ip][0],p[ip][1]+1)));
/* Process all target nodes. */
for (d = 0; d < ld[ild][1]; d++)
{
/* Initialize function value. */
f[d] = 0.0;
/* Process all source nodes. */
for (l = 0; l < ld[ild][0]; l++)
f[d] += b[l]*(*ptr++);
/* Multiply with the constant. */
f[d] *= constant;
/* Proceed to next row. */
ptr += rinc;
}
}
else
{
/* Process all target nodes. */
for (d = 0; d < ld[ild][1]; d++)
{
/* Initialize function value. */
f[d] = 0.0;
/* Process all source nodes. */
for (l = 0; l < ld[ild][0]; l++)
f[d] += b[l]*(*ptr++);
/* Proceed to next row. */
ptr += rinc;
}
}
}
/* Calculate the time needed. */
t1 = getticks();
t_dp = nfft_elapsed_seconds(t1,t0);
/* Calculate average time needed. */
t_dp = t_dp/((double)ld[ild][4]);
}
else
{
/* Initialize cumulative time variable with dummy value. */
t_dp = -1.0;
}
/* Initialize cumulative time variable. */
t_d = 0.0;
/* Initialize time measurement. */
t0 = getticks();
/* Cycle through all runs. */
for (i = 0; i < ld[ild][4]; i++)
{
/* Switch by the kernel type. */
switch (kt)
{
case KT_ABEL_POISSON:
/* Process all target nodes. */
for (d = 0; d < ld[ild][1]; d++)
{
/* Initialize function value. */
f[d] = 0.0;
/* Process all source nodes. */
for (l = 0; l < ld[ild][0]; l++)
{
/* Compute the inner product for the current source and
* target nodes. */
temp = innerProduct(eta[2*l],eta[2*l+1],xi[2*d],xi[2*d+1]);
/* Evaluate the Poisson kernel for the current value and add
* to the result. */
f[d] += b[l]*poissonKernel(temp,p[ip][0]);
}
}
break;
case KT_SINGULARITY:
/* Process all target nodes. */
for (d = 0; d < ld[ild][1]; d++)
{
/* Initialize function value. */
f[d] = 0.0;
/* Process all source nodes. */
for (l = 0; l < ld[ild][0]; l++)
{
/* Compute the inner product for the current source and
* target nodes. */
temp = innerProduct(eta[2*l],eta[2*l+1],xi[2*d],xi[2*d+1]);
/* Evaluate the Poisson kernel for the current value and add
* to the result. */
f[d] += b[l]*singularityKernel(temp,p[ip][0]);
}
}
break;
case KT_LOC_SUPP:
/* Calculate the multiplicative constant. */
constant = ((p[ip][1]+1)/(K2PI*pow(1-p[ip][0],p[ip][1]+1)));
/* Process all target nodes. */
for (d = 0; d < ld[ild][1]; d++)
{
/* Initialize function value. */
f[d] = 0.0;
/* Process all source nodes. */
for (l = 0; l < ld[ild][0]; l++)
{
/* Compute the inner product for the current source and
* target nodes. */
temp = innerProduct(eta[2*l],eta[2*l+1],xi[2*d],xi[2*d+1]);
/* Evaluate the Poisson kernel for the current value and add
* to the result. */
f[d] += b[l]*locallySupportedKernel(temp,p[ip][0],p[ip][1]);
}
/* Multiply result with constant. */
f[d] *= constant;
}
break;
case KT_GAUSSIAN:
/* Process all target nodes. */
for (d = 0; d < ld[ild][1]; d++)
{
/* Initialize function value. */
f[d] = 0.0;
/* Process all source nodes. */
for (l = 0; l < ld[ild][0]; l++)
{
/* Compute the inner product for the current source and
* target nodes. */
temp = innerProduct(eta[2*l],eta[2*l+1],xi[2*d],xi[2*d+1]);
/* Evaluate the Poisson kernel for the current value and add
* to the result. */
f[d] += b[l]*gaussianKernel(temp,p[ip][0]);
}
}
break;
}
}
/* Calculate and add the time needed. */
t1 = getticks();
t_d = nfft_elapsed_seconds(t1,t0);
/* Calculate average time needed. */
t_d = t_d/((double)ld[ild][4]);
}
else
{
/* Initialize cumulative time variable with dummy value. */
t_d = -1.0;
t_dp = -1.0;
}
/* Initialize error and cumulative time variables for the fast
* algorithm. */
err_fd = -1.0;
err_f = -1.0;
t_fd = -1.0;
t_f = -1.0;
/* Process all cut-off bandwidths. */
for (im = 0; im < im_max; im++)
{
/* Init transform plans. */
nfsft_init_guru(&plan_adjoint, m[im],ld[ild][0],
((use_nfft!=0)?(0U):(NFSFT_USE_NDFT)) |
((use_fpt!=0)?(0U):(NFSFT_USE_DPT)),
PRE_PHI_HUT | PRE_PSI | FFTW_INIT |
FFT_OUT_OF_PLACE, cutoff);
nfsft_init_guru(&plan,m[im],ld[ild][1],
((use_nfft!=0)?(0U):(NFSFT_USE_NDFT)) |
((use_fpt!=0)?(0U):(NFSFT_USE_DPT)),
PRE_PHI_HUT | PRE_PSI | FFTW_INIT |
FFT_OUT_OF_PLACE,
cutoff);
plan_adjoint.f_hat = f_hat;
plan_adjoint.x = eta;
plan_adjoint.f = b;
plan.f_hat = f_hat;
plan.x = xi;
plan.f = f_m;
nfsft_precompute_x(&plan_adjoint);
nfsft_precompute_x(&plan);
/* Check if direct algorithm shall also be tested. */
if (use_nfsft == BOTH)
{
/* Initialize cumulative time variable. */
t_fd = 0.0;
/* Initialize time measurement. */
t0 = getticks();
/* Cycle through all runs. */
for (i = 0; i < ld[ild][4]; i++)
{
/* Execute adjoint direct NDSFT transformation. */
nfsft_adjoint_direct(&plan_adjoint);
/* Multiplication with the Fourier-Legendre coefficients. */
for (k = 0; k <= m[im]; k++)
for (n = -k; n <= k; n++)
f_hat[NFSFT_INDEX(k,n,&plan_adjoint)] *= a[k];
/* Execute direct NDSFT transformation. */
nfsft_trafo_direct(&plan);
}
/* Calculate and add the time needed. */
t1 = getticks();
t_fd = nfft_elapsed_seconds(t1,t0);
/* Calculate average time needed. */
t_fd = t_fd/((double)ld[ild][4]);
/* Check if error E_infty should be computed. */
if (ld[ild][2] != NO)
{
/* Compute the error E_infinity. */
err_fd = X(error_l_infty_1_complex)(f, f_m, ld[ild][1], b,
ld[ild][0]);
}
}
/* Check if the fast NFSFT algorithm shall also be tested. */
if (use_nfsft != NO)
{
/* Initialize cumulative time variable for the NFSFT algorithm. */
t_f = 0.0;
}
else
{
/* Initialize cumulative time variable for the direct NDSFT
* algorithm. */
t_fd = 0.0;
}
/* Initialize time measurement. */
t0 = getticks();
/* Cycle through all runs. */
for (i = 0; i < ld[ild][4]; i++)
{
/* Check if the fast NFSFT algorithm shall also be tested. */
if (use_nfsft != NO)
{
/* Execute the adjoint NFSFT transformation. */
nfsft_adjoint(&plan_adjoint);
}
else
{
/* Execute the adjoint direct NDSFT transformation. */
nfsft_adjoint_direct(&plan_adjoint);
}
/* Multiplication with the Fourier-Legendre coefficients. */
for (k = 0; k <= m[im]; k++)
for (n = -k; n <= k; n++)
f_hat[NFSFT_INDEX(k,n,&plan_adjoint)] *= a[k];
/* Check if the fast NFSFT algorithm shall also be tested. */
if (use_nfsft != NO)
{
/* Execute the NFSFT transformation. */
nfsft_trafo(&plan);
}
else
{
/* Execute the NDSFT transformation. */
nfsft_trafo_direct(&plan);
}
}
/* Check if the fast NFSFT algorithm has been used. */
t1 = getticks();
if (use_nfsft != NO)
t_f = nfft_elapsed_seconds(t1,t0);
else
t_fd = nfft_elapsed_seconds(t1,t0);
/* Check if the fast NFSFT algorithm has been used. */
if (use_nfsft != NO)
{
/* Calculate average time needed. */
t_f = t_f/((double)ld[ild][4]);
}
else
{
/* Calculate average time needed. */
t_fd = t_fd/((double)ld[ild][4]);
}
/* Check if error E_infty should be computed. */
if (ld[ild][2] != NO)
{
/* Check if the fast NFSFT algorithm has been used. */
if (use_nfsft != NO)
{
/* Compute the error E_infinity. */
err_f = X(error_l_infty_1_complex)(f, f_m, ld[ild][1], b,
ld[ild][0]);
}
else
{
/* Compute the error E_infinity. */
err_fd = X(error_l_infty_1_complex)(f, f_m, ld[ild][1], b,
ld[ild][0]);
}
}
/* Print out the error measurements. */
fprintf(stdout,"%e\n%e\n%e\n%e\n%e\n%e\n\n",t_d,t_dp,t_fd,t_f,err_fd,
err_f);
/* Finalize the NFSFT plans */
nfsft_finalize(&plan_adjoint);
nfsft_finalize(&plan);
} /* for (im = 0; im < im_max; im++) - Process all cut-off
* bandwidths.*/
} /* for (ild = 0; ild < ild_max; ild++) - Process all node sets. */
} /* for (ip = 0; ip < ip_max; ip++) - Process all parameter sets. */
/* Delete precomputed data. */
nfsft_forget();
/* Check if memory for precomputed data of the matrix K has been
* allocated. */
if (precompute == YES)
{
/* Free memory for precomputed matrix K. */
nfft_free(prec);
}
/* Free data arrays. */
nfft_free(f);
nfft_free(f_m);
nfft_free(xi);
nfft_free(eta);
nfft_free(a);
nfft_free(f_hat);
nfft_free(b);
/* Free memory for node sets. */
for (ild = 0; ild < ild_max; ild++)
nfft_free(ld[ild]);
nfft_free(ld);
/* Free memory for cut-off bandwidths. */
nfft_free(m);
/* Free memory for parameter sets. */
for (ip = 0; ip < ip_max; ip++)
nfft_free(p[ip]);
nfft_free(p);
} /* for (tc = 0; tc < tc_max; tc++) - Process each testcase. */
/* Return exit code for successful run. */
return EXIT_SUCCESS;
}
////////////////////////////////////////////////////////////////////////////////
// Put laser data to the interface
void GazeboRosVelodyneLaser::putLaserData(common::Time &_updateTime)
{
int i, hja, hjb;
int j, vja, vjb;
double vb, hb;
int j1, j2, j3, j4; // four corners indices
double r1, r2, r3, r4, r; // four corner values + interpolated range
double intensity;
parent_ray_sensor_->SetActive(false);
#if GAZEBO_MAJOR_VERSION >= 7
math::Angle maxAngle = parent_ray_sensor_->AngleMax();
math::Angle minAngle = parent_ray_sensor_->AngleMin();
double maxRange = parent_ray_sensor_->RangeMax();
double minRange = parent_ray_sensor_->RangeMin();
int rayCount = parent_ray_sensor_->RayCount();
int rangeCount = parent_ray_sensor_->RangeCount();
int verticalRayCount = parent_ray_sensor_->VerticalRayCount();
int verticalRangeCount = parent_ray_sensor_->VerticalRangeCount();
math::Angle verticalMaxAngle = parent_ray_sensor_->VerticalAngleMax();
math::Angle verticalMinAngle = parent_ray_sensor_->VerticalAngleMin();
#else
math::Angle maxAngle = parent_ray_sensor_->GetAngleMax();
math::Angle minAngle = parent_ray_sensor_->GetAngleMin();
double maxRange = parent_ray_sensor_->GetRangeMax();
double minRange = parent_ray_sensor_->GetRangeMin();
int rayCount = parent_ray_sensor_->GetRayCount();
int rangeCount = parent_ray_sensor_->GetRangeCount();
int verticalRayCount = parent_ray_sensor_->GetVerticalRayCount();
int verticalRangeCount = parent_ray_sensor_->GetVerticalRangeCount();
math::Angle verticalMaxAngle = parent_ray_sensor_->GetVerticalAngleMax();
math::Angle verticalMinAngle = parent_ray_sensor_->GetVerticalAngleMin();
#endif
double yDiff = maxAngle.Radian() - minAngle.Radian();
double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();
/***************************************************************/
/* */
/* point scan from laser */
/* */
/***************************************************************/
boost::mutex::scoped_lock lock(lock_);
// Populate message fields
const uint32_t POINT_STEP = 32;
sensor_msgs::PointCloud2 msg;
msg.header.frame_id = frame_name_;
msg.header.stamp.sec = _updateTime.sec;
msg.header.stamp.nsec = _updateTime.nsec;
msg.fields.resize(5);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.fields[3].name = "intensity";
msg.fields[3].offset = 16;
msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[3].count = 1;
msg.fields[4].name = "ring";
msg.fields[4].offset = 20;
msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
msg.fields[4].count = 1;
msg.data.resize(verticalRangeCount * rangeCount * POINT_STEP);
const double MIN_RANGE = std::max(min_range_, minRange);
const double MAX_RANGE = std::min(max_range_, maxRange - minRange - 0.01);
uint8_t *ptr = msg.data.data();
for (j = 0; j<verticalRangeCount; j++) {
// interpolating in vertical direction
vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
vja = (int) floor(vb);
vjb = std::min(vja + 1, verticalRayCount - 1);
vb = vb - floor(vb); // fraction from min
assert(vja >= 0 && vja < verticalRayCount);
assert(vjb >= 0 && vjb < verticalRayCount);
for (i = 0; i<rangeCount; i++) {
// Interpolate the range readings from the rays in horizontal direction
hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
hja = (int) floor(hb);
hjb = std::min(hja + 1, rayCount - 1);
hb = hb - floor(hb); // fraction from min
assert(hja >= 0 && hja < rayCount);
assert(hjb >= 0 && hjb < rayCount);
// indices of 4 corners
j1 = hja + vja * rayCount;
j2 = hjb + vja * rayCount;
j3 = hja + vjb * rayCount;
j4 = hjb + vjb * rayCount;
// range readings of 4 corners
#if GAZEBO_MAJOR_VERSION >= 7
r1 = std::min(parent_ray_sensor_->LaserShape()->GetRange(j1) , maxRange-minRange);
r2 = std::min(parent_ray_sensor_->LaserShape()->GetRange(j2) , maxRange-minRange);
r3 = std::min(parent_ray_sensor_->LaserShape()->GetRange(j3) , maxRange-minRange);
r4 = std::min(parent_ray_sensor_->LaserShape()->GetRange(j4) , maxRange-minRange);
#else
r1 = std::min(parent_ray_sensor_->GetLaserShape()->GetRange(j1) , maxRange-minRange);
r2 = std::min(parent_ray_sensor_->GetLaserShape()->GetRange(j2) , maxRange-minRange);
r3 = std::min(parent_ray_sensor_->GetLaserShape()->GetRange(j3) , maxRange-minRange);
r4 = std::min(parent_ray_sensor_->GetLaserShape()->GetRange(j4) , maxRange-minRange);
#endif
// Range is linear interpolation if values are close,
// and min if they are very different
r = (1 - vb) * ((1 - hb) * r1 + hb * r2)
+ vb * ((1 - hb) * r3 + hb * r4);
if (gaussian_noise_ != 0.0) {
r += gaussianKernel(0,gaussian_noise_);
}
// Intensity is averaged
#if GAZEBO_MAJOR_VERSION >= 7
intensity = 0.25*(parent_ray_sensor_->LaserShape()->GetRetro(j1) +
parent_ray_sensor_->LaserShape()->GetRetro(j2) +
parent_ray_sensor_->LaserShape()->GetRetro(j3) +
parent_ray_sensor_->LaserShape()->GetRetro(j4));
#else
intensity = 0.25*(parent_ray_sensor_->GetLaserShape()->GetRetro(j1) +
parent_ray_sensor_->GetLaserShape()->GetRetro(j2) +
parent_ray_sensor_->GetLaserShape()->GetRetro(j3) +
parent_ray_sensor_->GetLaserShape()->GetRetro(j4));
#endif
// get angles of ray to get xyz for point
double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.Radian();
double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.Radian();
//pAngle is rotated by yAngle:
if ((MIN_RANGE < r) && (r < MAX_RANGE)) {
*((float*)(ptr + 0)) = r * cos(pAngle) * cos(yAngle);
*((float*)(ptr + 4)) = r * cos(pAngle) * sin(yAngle);
#if GAZEBO_MAJOR_VERSION > 2
*((float*)(ptr + 8)) = r * sin(pAngle);
#else
*((float*)(ptr + 8)) = -r * sin(pAngle);
#endif
*((float*)(ptr + 16)) = intensity;
#if GAZEBO_MAJOR_VERSION > 2
*((uint16_t*)(ptr + 20)) = j; // ring
#else
*((uint16_t*)(ptr + 20)) = verticalRangeCount - 1 - j; // ring
#endif
ptr += POINT_STEP;
}
}
}
parent_ray_sensor_->SetActive(true);
// Populate message with number of valid points
msg.point_step = POINT_STEP;
msg.row_step = ptr - msg.data.data();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
msg.is_bigendian = false;
msg.is_dense = true;
msg.data.resize(msg.row_step); // Shrink to actual size
// Publish output
pub_.publish(msg);
}
