772 lines
18 KiB
C++
772 lines
18 KiB
C++
#include "WD_convolveGauss.h"
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
|
|
// TODO: Auto-generated Javadoc
|
|
/**
|
|
* The Class Normal.
|
|
*/
|
|
static double SQRT_2_PI_INV = 0.398942280401432677939946059935;
|
|
|
|
/** The Constant MAX_SIZE_MASK_0. */
|
|
static double MAX_SIZE_MASK_0 = 3.09023230616781; /* Size for Gaussian mask */
|
|
|
|
/** The Constant MAX_SIZE_MASK_1. */
|
|
static double MAX_SIZE_MASK_1 = 3.46087178201605; /* Size for 1st derivative mask */
|
|
|
|
/** The Constant MAX_SIZE_MASK_2. */
|
|
static double MAX_SIZE_MASK_2 = 3.82922419517181; /* Size for 2nd derivative mask */
|
|
|
|
/**
|
|
* Mask size.
|
|
*
|
|
* @param MAX
|
|
* the max
|
|
* @param sigma
|
|
* the sigma
|
|
* @return the int
|
|
*/
|
|
static int MASK_SIZE(double MAX, double sigma)
|
|
{
|
|
return (int)ceil(MAX * sigma); /* Maximum mask index */
|
|
}
|
|
|
|
double getNormal(double x)
|
|
{
|
|
|
|
/** The Constant SQRTPI. */
|
|
static double SQRTPI = 1.772453850905516027;
|
|
|
|
/** The Constant UPPERLIMIT. */
|
|
static double UPPERLIMIT = 20.0;
|
|
|
|
/** The Constant P10. */
|
|
static double P10 = 242.66795523053175;
|
|
|
|
/** The Constant P11. */
|
|
static double P11 = 21.979261618294152;
|
|
|
|
/** The Constant P12. */
|
|
static double P12 = 6.9963834886191355;
|
|
|
|
/** The Constant P13. */
|
|
static double P13 = -.035609843701815385;
|
|
|
|
/** The Constant Q10. */
|
|
static double Q10 = 215.05887586986120;
|
|
|
|
/** The Constant Q11. */
|
|
static double Q11 = 91.164905404514901;
|
|
|
|
/** The Constant Q12. */
|
|
static double Q12 = 15.082797630407787;
|
|
|
|
/** The Constant Q13. */
|
|
static double Q13 = 1.0;
|
|
|
|
/** The Constant P20. */
|
|
static double P20 = 300.4592610201616005;
|
|
|
|
/** The Constant P21. */
|
|
static double P21 = 451.9189537118729422;
|
|
|
|
/** The Constant P22. */
|
|
static double P22 = 339.3208167343436870;
|
|
|
|
/** The Constant P23. */
|
|
static double P23 = 152.9892850469404039;
|
|
|
|
/** The Constant P24. */
|
|
static double P24 = 43.16222722205673530;
|
|
|
|
/** The Constant P25. */
|
|
static double P25 = 7.211758250883093659;
|
|
|
|
/** The Constant P26. */
|
|
static double P26 = .5641955174789739711;
|
|
|
|
/** The Constant P27. */
|
|
static double P27 = -.0000001368648573827167067;
|
|
|
|
/** The Constant Q20. */
|
|
static double Q20 = 300.4592609569832933;
|
|
|
|
/** The Constant Q21. */
|
|
static double Q21 = 790.9509253278980272;
|
|
|
|
/** The Constant Q22. */
|
|
static double Q22 = 931.3540948506096211;
|
|
|
|
/** The Constant Q23. */
|
|
static double Q23 = 638.9802644656311665;
|
|
|
|
/** The Constant Q24. */
|
|
static double Q24 = 277.5854447439876434;
|
|
|
|
/** The Constant Q25. */
|
|
static double Q25 = 77.00015293522947295;
|
|
|
|
/** The Constant Q26. */
|
|
static double Q26 = 12.78272731962942351;
|
|
|
|
/** The Constant Q27. */
|
|
static double Q27 = 1.0;
|
|
|
|
/** The Constant P30. */
|
|
static double P30 = -.00299610707703542174;
|
|
|
|
/** The Constant P31. */
|
|
static double P31 = -.0494730910623250734;
|
|
|
|
/** The Constant P32. */
|
|
static double P32 = -.226956593539686930;
|
|
|
|
/** The Constant P33. */
|
|
static double P33 = -.278661308609647788;
|
|
|
|
/** The Constant P34. */
|
|
static double P34 = -.0223192459734184686;
|
|
|
|
/** The Constant Q30. */
|
|
static double Q30 = .0106209230528467918;
|
|
|
|
/** The Constant Q31. */
|
|
static double Q31 = .191308926107829841;
|
|
|
|
/** The Constant Q32. */
|
|
static double Q32 = 1.05167510706793207;
|
|
|
|
/** The Constant Q33. */
|
|
static double Q33 = 1.98733201817135256;
|
|
|
|
/** The Constant Q34. */
|
|
static double Q34 = 1.0;
|
|
|
|
/** The Constant SQRT2. */
|
|
static double SQRT2 = 1.41421356237309504880;
|
|
|
|
int sn;
|
|
double R1, R2, y, y2, y3, y4, y5, y6, y7;
|
|
double erf, erfc, z, z2, z3, z4;
|
|
double phi;
|
|
|
|
if (x < -UPPERLIMIT)
|
|
return 0.0;
|
|
if (x > UPPERLIMIT)
|
|
return 1.0;
|
|
|
|
y = x / SQRT2;
|
|
if (y < 0) {
|
|
y = -y;
|
|
sn = -1;
|
|
}
|
|
else
|
|
sn = 1;
|
|
|
|
y2 = y * y;
|
|
y4 = y2 * y2;
|
|
y6 = y4 * y2;
|
|
|
|
if (y < 0.46875) {
|
|
R1 = P10 + P11 * y2 + P12 * y4 + P13 * y6;
|
|
R2 = Q10 + Q11 * y2 + Q12 * y4 + Q13 * y6;
|
|
erf = y * R1 / R2;
|
|
if (sn == 1)
|
|
phi = 0.5 + 0.5 * erf;
|
|
else
|
|
phi = 0.5 - 0.5 * erf;
|
|
}
|
|
else if (y < 4.0) {
|
|
y3 = y2 * y;
|
|
y5 = y4 * y;
|
|
y7 = y6 * y;
|
|
R1 = P20 + P21 * y + P22 * y2 + P23 * y3 + P24 * y4 + P25 * y5 + P26 * y6 + P27 * y7;
|
|
R2 = Q20 + Q21 * y + Q22 * y2 + Q23 * y3 + Q24 * y4 + Q25 * y5 + Q26 * y6 + Q27 * y7;
|
|
erfc = exp(-y2) * R1 / R2;
|
|
if (sn == 1)
|
|
phi = 1.0 - 0.5 * erfc;
|
|
else
|
|
phi = 0.5 * erfc;
|
|
}
|
|
else {
|
|
z = y4;
|
|
z2 = z * z;
|
|
z3 = z2 * z;
|
|
z4 = z2 * z2;
|
|
R1 = P30 + P31 * z + P32 * z2 + P33 * z3 + P34 * z4;
|
|
R2 = Q30 + Q31 * z + Q32 * z2 + Q33 * z3 + Q34 * z4;
|
|
erfc = (exp(-y2) / y) * (1.0 / SQRTPI + R1 / (R2 * y2));
|
|
if (sn == 1)
|
|
phi = 1.0 - 0.5 * erfc;
|
|
else
|
|
phi = 0.5 * erfc;
|
|
}
|
|
|
|
return phi;
|
|
}
|
|
|
|
/**
|
|
* Phi 0.
|
|
*
|
|
* @param x
|
|
* the x
|
|
* @param sigma
|
|
* the sigma
|
|
* @return the double
|
|
*/
|
|
/* Integral of the Gaussian function */
|
|
double phi0(double x, double sigma) {
|
|
return getNormal(x / sigma);
|
|
}
|
|
|
|
/**
|
|
* Phi 1.
|
|
*
|
|
* @param x
|
|
* the x
|
|
* @param sigma
|
|
* the sigma
|
|
* @return the double
|
|
*/
|
|
/* The Gaussian function */
|
|
double phi1(double x, double sigma) {
|
|
double t;
|
|
t = x / sigma;
|
|
return SQRT_2_PI_INV / sigma * exp(-0.5 * t * t);
|
|
}
|
|
|
|
/**
|
|
* Phi 2.
|
|
*
|
|
* @param x
|
|
* the x
|
|
* @param sigma
|
|
* the sigma
|
|
* @return the double
|
|
*/
|
|
/* First derivative of the Gaussian function */
|
|
double phi2(double x, double sigma) {
|
|
double t;
|
|
t = x / sigma;
|
|
return -x * SQRT_2_PI_INV / pow(sigma, 3.0) * exp(-0.5 * t * t);
|
|
}
|
|
|
|
/* Gaussian smoothing mask */
|
|
|
|
/**
|
|
* Compute gauss mask 0.
|
|
*
|
|
* @param num
|
|
* the num
|
|
* @param sigma
|
|
* the sigma
|
|
* @return the double[]
|
|
*/
|
|
/*
|
|
* num ist eigentlich pointer - aufrufende Funkion nimmt an, dass num geändert
|
|
* wird. Übergebe es deswegen als MutableDouble aus CommonsLang
|
|
*/
|
|
double* compute_gauss_mask_0(int* num, double sigma) {
|
|
|
|
int i, n;
|
|
double limit;
|
|
|
|
limit = MASK_SIZE(MAX_SIZE_MASK_0, sigma); /* Error < 0.001 on each side */
|
|
n = (int)limit;
|
|
double* h = (double*)malloc(sizeof(double)*(2*n+1));//h = new double[2 * n + 1];
|
|
for (i = -n + 1; i <= n - 1; i++)
|
|
h[n + i] = phi0(-i + 0.5, sigma) - phi0(-i - 0.5, sigma);
|
|
h[0] = 1.0 - phi0(n - 0.5, sigma);
|
|
h[2 * n] = phi0(-n + 0.5, sigma);
|
|
*num = n;
|
|
return h;
|
|
}
|
|
|
|
/* First derivative of Gaussian smoothing mask */
|
|
/**
|
|
* Compute gauss mask 1.
|
|
*
|
|
* @param num
|
|
* the num
|
|
* @param sigma
|
|
* the sigma
|
|
* @return the double[]
|
|
*/
|
|
/*
|
|
* num ist eigentlich pointer - aufrufende Funkion nimmt an, dass num geändert
|
|
* wird. Übergebe es deswegen als MutableDouble aus CommonsLang
|
|
*/
|
|
double* compute_gauss_mask_1(int* num, double sigma) {
|
|
int i, n;
|
|
double limit = MASK_SIZE(MAX_SIZE_MASK_1, sigma); /* Error < 0.001 on each side */
|
|
n = (int)limit;
|
|
double* h = (double*)malloc(sizeof(double) * (2 * n + 1)); //h = new double[2 * n + 1];
|
|
|
|
for (i = -n + 1; i <= n - 1; i++)
|
|
h[n + i] = phi1(-i + 0.5, sigma) - phi1(-i - 0.5, sigma);
|
|
h[0] = -phi1(n - 0.5, sigma);
|
|
h[2 * n] = phi1(-n + 0.5, sigma);
|
|
*num = n;
|
|
return h;
|
|
}
|
|
|
|
int compute_gauss_mask_1_scale(std::vector<double>& gaussMask1st, double sigma, float scale) {
|
|
int i, n;
|
|
double limit = MASK_SIZE(MAX_SIZE_MASK_1, sigma) * (1.0f / scale); /* Error < 0.001 on each side */
|
|
n = (int)limit;
|
|
gaussMask1st.resize(n * 2 + 1);
|
|
//double* h = (double*)malloc(sizeof(double) * (2 * n + 1)); //h = new double[2 * n + 1];
|
|
float winSize = 0.5 * scale;
|
|
//for (i = -n + 1; i <= n - 1; i++)
|
|
for (i = -n; i <= n; i++)
|
|
{
|
|
float winCenter = i * scale;
|
|
gaussMask1st[n + i] = phi1(-winCenter + winSize, sigma) - phi1(-winCenter - winSize, sigma);
|
|
}
|
|
//h[0] = -phi1( (n - 0.5)*scale, sigma);
|
|
//h[2 * n] = phi1((-n + 0.5)*scale, sigma);
|
|
return n;
|
|
}
|
|
|
|
/* Second derivative of Gaussian smoothing mask */
|
|
/**
|
|
* Compute gauss mask 2.
|
|
*
|
|
* @param num
|
|
* the num
|
|
* @param sigma
|
|
* the sigma
|
|
* @return the double[]
|
|
*/
|
|
/*
|
|
* num ist eigentlich pointer - aufrufende Funkion nimmt an, dass num geändert
|
|
* wird. Übergebe es deswegen als MutableDouble aus CommonsLang
|
|
*/
|
|
double* compute_gauss_mask_2(int* num, double sigma) {
|
|
int i, n;
|
|
double limit = MASK_SIZE(MAX_SIZE_MASK_2, sigma); /* Error < 0.001 on each side */
|
|
n = (int)limit;
|
|
double* h = (double*)malloc(sizeof(double) * (2 * n + 1)); //h = new double[2 * n + 1];
|
|
|
|
for (i = -n + 1; i <= n - 1; i++)
|
|
h[n + i] = phi2(-i + 0.5, sigma) - phi2(-i - 0.5, sigma);
|
|
h[0] = -phi2(n - 0.5, sigma);
|
|
h[2 * n] = phi2(-n + 0.5, sigma);
|
|
*num = n;
|
|
return h;
|
|
}
|
|
|
|
int compute_gauss_mask_2_scale(std::vector<double>& gaussMask2nd, double sigma, float scale) {
|
|
int i, n;
|
|
double limit = MASK_SIZE(MAX_SIZE_MASK_2, sigma) * (1.0f / scale); /* Error < 0.001 on each side */
|
|
n = (int)limit;
|
|
gaussMask2nd.resize(n * 2 + 1);
|
|
//double* h = (double*)malloc(sizeof(double) * (2 * n + 1)); //h = new double[2 * n + 1];
|
|
|
|
float winSize = 0.5 * scale;
|
|
for (i = -n + 1; i <= n - 1; i++)
|
|
{
|
|
float winCenter = i * scale;
|
|
gaussMask2nd[n + i] = phi2(-winCenter + winSize, sigma) - phi2(-winCenter - winSize, sigma);
|
|
}
|
|
gaussMask2nd[0] = -phi2( (n - 0.5)*scale, sigma);
|
|
gaussMask2nd[2 * n] = phi2( (-n + 0.5)*scale, sigma);
|
|
return n;
|
|
}
|
|
|
|
/**
|
|
* Lincoor.
|
|
*
|
|
* @param row
|
|
* the row
|
|
* @param col
|
|
* the col
|
|
* @param width
|
|
* the width
|
|
* @return the int
|
|
*/
|
|
/*
|
|
* Translate row and column coordinates of an image into an index into its
|
|
* one-dimensional array.
|
|
*/
|
|
int LINCOOR(int row, int col, int width) {
|
|
return row * width + col;
|
|
}
|
|
|
|
/**
|
|
* Br.
|
|
*
|
|
* @param row
|
|
* the row
|
|
* @param height
|
|
* the height
|
|
* @return the int
|
|
*/
|
|
/*
|
|
* Mirror the row coordinate at the borders of the image; height must be a
|
|
* defined variable in the calling function containing the image height.
|
|
*/
|
|
int BR(int row, int height) {
|
|
return ((row) < 0 ? -(row) : (row) >= height ? height - (row)+height - 2 : (row));
|
|
}
|
|
|
|
/**
|
|
* Bc.
|
|
*
|
|
* @param col
|
|
* the col
|
|
* @param width
|
|
* the width
|
|
* @return the int
|
|
*/
|
|
/*
|
|
* Mirror the column coordinate at the borders of the image; width must be a
|
|
* defined variable in the calling function containing the image width.
|
|
*/
|
|
int BC(int col, int width) {
|
|
return ((col) < 0 ? -(col) : (col) >= width ? width - (col)+width - 2 : (col));
|
|
}
|
|
|
|
/*
|
|
* Convolve an image with the derivatives of a Gaussian smoothing kernel. Since
|
|
* all of the masks are separable, this is done in two steps in the function
|
|
* convolve_gauss. Firstly, the rows of the image are convolved by an
|
|
* appropriate one-dimensional mask in convolve_rows_gauss, yielding an
|
|
* intermediate float-image h. Then the columns of this image are convolved by
|
|
* another appropriate mask in convolve_cols_gauss to yield the final result k.
|
|
* At the border of the image the gray values are mirrored.
|
|
*/
|
|
|
|
/**
|
|
* Convolve rows gauss.
|
|
*
|
|
* @param image
|
|
* the image
|
|
* @param mask
|
|
* the mask
|
|
* @param n
|
|
* the n
|
|
* @param h
|
|
* the h
|
|
* @param width
|
|
* the width
|
|
* @param height
|
|
* the height
|
|
*/
|
|
/* Convolve the rows of an image with the derivatives of a Gaussian. */
|
|
void convolve_rows_gauss(float* image, double* mask, int n, float* h, int width, int height)
|
|
{
|
|
int j, r, c, l;
|
|
double sum;
|
|
|
|
/* Inner region */
|
|
for (r = n; r < height - n; r++)
|
|
{
|
|
for (c = 0; c < width; c++)
|
|
{
|
|
l = LINCOOR(r, c, width);
|
|
sum = 0.0;
|
|
for (j = -n; j <= n; j++)
|
|
sum += (double)(image[(l + j * width)]) * mask[(j + n)];
|
|
h[l] = (float)sum;
|
|
}
|
|
}
|
|
/* Border regions */
|
|
for (r = 0; r < n; r++)
|
|
{
|
|
for (c = 0; c < width; c++)
|
|
{
|
|
l = LINCOOR(r, c, width);
|
|
sum = 0.0;
|
|
for (j = -n; j <= n; j++)
|
|
sum += (double)(image[LINCOOR(BR(r + j, height), c, width)]) * mask[(j + n)];
|
|
h[l] = (float)sum;
|
|
}
|
|
}
|
|
for (r = height - n; r < height; r++)
|
|
{
|
|
for (c = 0; c < width; c++)
|
|
{
|
|
l = LINCOOR(r, c, width);
|
|
sum = 0.0;
|
|
for (j = -n; j <= n; j++)
|
|
sum += (double)(image[LINCOOR(BR(r + j, height), c, width)]) * mask[(j + n)];
|
|
h[l] = (float)sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Convolve cols gauss.
|
|
*
|
|
* @param h
|
|
* the h
|
|
* @param mask
|
|
* the mask
|
|
* @param n
|
|
* the n
|
|
* @param k
|
|
* the k
|
|
* @param width
|
|
* the width
|
|
* @param height
|
|
* the height
|
|
*/
|
|
/* Convolve the columns of an image with the derivatives of a Gaussian. */
|
|
void convolve_cols_gauss(float* h, double* mask, int n, float* k, int width, int height) {
|
|
int j, r, c, l;
|
|
double sum;
|
|
|
|
/* Inner region */
|
|
for (r = 0; r < height; r++) {
|
|
for (c = n; c < width - n; c++) {
|
|
l = LINCOOR(r, c, width);
|
|
sum = 0.0;
|
|
for (j = -n; j <= n; j++)
|
|
sum += h[(l + j)] * mask[(j + n)];
|
|
k[l] = (float)sum;
|
|
}
|
|
}
|
|
/* Border regions */
|
|
for (r = 0; r < height; r++) {
|
|
for (c = 0; c < n; c++) {
|
|
l = LINCOOR(r, c, width);
|
|
sum = 0.0;
|
|
for (j = -n; j <= n; j++)
|
|
sum += h[LINCOOR(r, BC(c + j, width), width)] * mask[(j + n)];
|
|
k[l] = (float)sum;
|
|
}
|
|
}
|
|
for (r = 0; r < height; r++) {
|
|
for (c = width - n; c < width; c++) {
|
|
l = LINCOOR(r, c, width);
|
|
sum = 0.0;
|
|
for (j = -n; j <= n; j++)
|
|
sum += h[LINCOOR(r, BC(c + j, width), width)] * mask[(j + n)];
|
|
k[l] = (float)sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Convolve gauss.
|
|
*
|
|
* @param image
|
|
* the image
|
|
* @param k
|
|
* the k
|
|
* @param width
|
|
* the width
|
|
* @param height
|
|
* the height
|
|
* @param sigma
|
|
* the sigma
|
|
* @param deriv_type
|
|
* the deriv type
|
|
*/
|
|
/* Convolve an image with a derivative of the Gaussian. */
|
|
void convolve_gauss(float* image, float* k, int width, int height, double sigma, DERIV_TYPE deriv_type) {
|
|
double *hr = NULL, *hc = NULL;
|
|
double* maskr=NULL, *maskc=NULL;
|
|
int nr=0, nc=0;
|
|
float* h;
|
|
|
|
h = (float*)malloc(sizeof(float)*width * height);
|
|
|
|
switch (deriv_type) {
|
|
case DERIV_R:
|
|
hr = compute_gauss_mask_1(&nr, sigma);
|
|
hc = compute_gauss_mask_0(&nc, sigma);
|
|
break;
|
|
case DERIV_C:
|
|
hr = compute_gauss_mask_0(&nr, sigma);
|
|
hc = compute_gauss_mask_1(&nc, sigma);
|
|
break;
|
|
case DERIV_RR:
|
|
hr = compute_gauss_mask_2(&nr, sigma);
|
|
hc = compute_gauss_mask_0(&nc, sigma);
|
|
break;
|
|
case DERIV_RC:
|
|
hr = compute_gauss_mask_1(&nr, sigma);
|
|
hc = compute_gauss_mask_1(&nc, sigma);
|
|
break;
|
|
case DERIV_CC:
|
|
hr = compute_gauss_mask_0(&nr, sigma);
|
|
hc = compute_gauss_mask_2(&nc, sigma);
|
|
break;
|
|
}
|
|
|
|
maskr = hr;// + nr; Wird ersetzt in den eigentlichen Funktionen, indem ich z.B. in
|
|
// convolve_rows_gauss immer beim Zugriff auf mask n dazuaddiere
|
|
maskc = hc;// + nc;
|
|
|
|
convolve_rows_gauss(image, maskr, nr, h, width, height);
|
|
convolve_cols_gauss(h, maskc, nc, k, width, height);
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
* Convolve cols gauss.
|
|
*
|
|
* @param laserLine
|
|
* the source laser line
|
|
* @param mask
|
|
* the mask
|
|
* @param n
|
|
* the n
|
|
* @param result
|
|
* the result
|
|
* @param width
|
|
* the width
|
|
* @param height
|
|
* the height
|
|
*/
|
|
/* Convolve the columns of an image with the derivatives of a Gaussian. */
|
|
void convolve_laserLine_gauss(
|
|
std::vector<SVzNL3DPosition>& a_line,
|
|
int startId, int endId,
|
|
std::vector<double>&mask, int maskSize,
|
|
std::vector<double>& convolveResult)
|
|
{
|
|
convolveResult.resize(a_line.size());
|
|
int width = endId - startId + 1;//a_line->m_3DPointCount;
|
|
std::vector<SVzNL3DPosition> dataBuff;
|
|
dataBuff.insert(dataBuff.end(), a_line.begin() + startId, a_line.begin() + endId+1); //范围是左闭右开
|
|
double* result = &convolveResult[startId];
|
|
/* Inner region */
|
|
for (int c = maskSize; c < width - maskSize; c++)
|
|
{
|
|
double sum = 0.0;
|
|
for (int j = -maskSize; j <= maskSize; j++)
|
|
sum += dataBuff[(c + j)].pt3D.z * mask[(j + maskSize)];
|
|
result[c] = (float)sum;
|
|
}
|
|
|
|
/* Border regions */
|
|
for (int c = 0; c < maskSize; c++)
|
|
{
|
|
double sum = 0.0;
|
|
for (int j = -maskSize; j <= maskSize; j++)
|
|
{
|
|
int mirror = BC(c + j, width);
|
|
sum += dataBuff[mirror].pt3D.z * mask[(j + maskSize)];
|
|
}
|
|
result[c] = (float)sum;
|
|
}
|
|
for (int c = width - maskSize; c < width; c++) {
|
|
|
|
double sum = 0.0;
|
|
for (int j = -maskSize; j <= maskSize; j++)
|
|
{
|
|
int mirror = BC(c + j, width);
|
|
sum += dataBuff[mirror].pt3D.z * mask[(j + maskSize)];
|
|
}
|
|
result[c] = sum;
|
|
}
|
|
return;
|
|
}
|
|
|
|
|
|
void convolve_laserLine_gauss_scale(
|
|
std::vector<SVzNL3DPosition>& a_line,
|
|
int startId, int endId,
|
|
std::vector<double>& mask, int maskSize,
|
|
float scale,
|
|
std::vector<double>& conResult)
|
|
{
|
|
int ptNum = endId - startId + 1; //a_line->m_3DPointCount;
|
|
std::vector<SVzNL3DPosition> dataBuff;
|
|
dataBuff.insert(dataBuff.end(), a_line.begin() + startId, a_line.begin() + endId + 1);
|
|
|
|
conResult.resize(a_line.size());
|
|
double* result = &conResult[startId];
|
|
|
|
float halfWidth = (float)maskSize * scale;
|
|
SVzNL3DPoint startPt = dataBuff[0].pt3D;
|
|
SVzNL3DPoint endPt = dataBuff[ptNum-1].pt3D;
|
|
|
|
for (int i = 0; i < ptNum; i++)
|
|
{
|
|
if ((i == 200) || (i == 280) || (i == 360) || (i == 850) ||(i==950) || (i == 1050))
|
|
int kkk = 1;
|
|
SVzNL3DPoint currPt = dataBuff[i].pt3D;
|
|
double convolveResult = mask[maskSize] * currPt.z;
|
|
//向前chkWidth/2
|
|
int id = i;
|
|
double dist = 0;
|
|
SVzNL3DPoint preConvolvePt = currPt;
|
|
int preMaskPos = maskSize;
|
|
while (1)
|
|
{
|
|
id--;
|
|
int ptId = id < 0 ? -id : id;
|
|
SVzNL3DPoint convolvePt = dataBuff[ptId].pt3D;
|
|
dist = dist + fabs(convolvePt.y - preConvolvePt.y);
|
|
if (dist > halfWidth)
|
|
break;
|
|
int maskPos = (halfWidth - dist) / scale;
|
|
int diffId = abs(preMaskPos - maskPos);
|
|
convolveResult += mask[maskPos] * convolvePt.z * (double)diffId;
|
|
|
|
preConvolvePt = convolvePt;
|
|
preMaskPos = maskPos;
|
|
}
|
|
|
|
//向后chkWidth/2
|
|
id = i;
|
|
dist = 0;
|
|
preMaskPos = maskSize;
|
|
preConvolvePt = currPt;
|
|
while (1)
|
|
{
|
|
id++;
|
|
int ptId = id >= ptNum ? (ptNum * 2 - 2 - id) : id;
|
|
SVzNL3DPoint convolvePt = dataBuff[ptId].pt3D;
|
|
dist = dist + fabs(convolvePt.y - preConvolvePt.y);
|
|
if (dist > halfWidth)
|
|
break;
|
|
int maskPos = (halfWidth + dist) / scale;
|
|
int diffId = abs(preMaskPos - maskPos);
|
|
convolveResult += mask[maskPos] * convolvePt.z * (double)diffId;
|
|
|
|
preConvolvePt = convolvePt;
|
|
preMaskPos = maskPos;
|
|
}
|
|
|
|
result[i] = convolveResult;
|
|
}
|
|
return;
|
|
}
|
|
|
|
void wd_convolveGauss_laserLine(
|
|
std::vector<SVzNL3DPosition>& a_line,
|
|
int startId, int endId,
|
|
std::vector<double>& gaussConvolve_1st,
|
|
std::vector<double>& gaussConvolve_2nd,
|
|
float chkWidth, float scale)
|
|
{
|
|
double sigma = chkWidth / 1.7;
|
|
std::vector<double> mask_hc;
|
|
int mask_hc_size = compute_gauss_mask_1_scale(mask_hc, sigma, scale);
|
|
if (mask_hc_size == 0)
|
|
return;
|
|
|
|
std::vector<double> mask_hcc;
|
|
int mask_hcc_size = compute_gauss_mask_2_scale(mask_hcc, sigma, scale);
|
|
if (mask_hcc_size == 0)
|
|
return;
|
|
|
|
#if 1
|
|
convolve_laserLine_gauss(a_line, startId, endId, mask_hc, mask_hc_size, gaussConvolve_1st);
|
|
convolve_laserLine_gauss(a_line, startId, endId, mask_hcc, mask_hcc_size, gaussConvolve_2nd);
|
|
#else
|
|
convolve_laserLine_gauss_scale(a_line, startId, endId, mask_hc, mask_hc_size, scale, gaussConvolve_1st);
|
|
convolve_laserLine_gauss_scale(a_line, startId, endId, mask_hcc, mask_hcc_size, scale, gaussConvolve_2nd);
|
|
#endif
|
|
|
|
return;
|
|
}
|
|
|
|
|