void RaspiVoice::initTestImage()
{
if (verbose)
{
if (use_bw_test_image)
{
std::cout << "Using B/W test image" << std::endl;
}
else
{
std::cout << "Using grayscale test image" << std::endl;
}
}
// Set hard-coded image
for (int j = 0; j < columns; j++)
{
for (int i = 0; i < rows; i++)
{
if (use_bw_test_image)
{
if (P_bw[rows - i - 1][j] != '#')
{
(*image)[IDX2D(i, j)] = 1.0; // white;
}
else
{
(*image)[IDX2D(i, j)] = 0.0; // black;
}
}
else if (P_grayscale[i][j] > 'a')
{
(*image)[IDX2D(rows - i - 1, j)] = pow(10.0, (P_grayscale[i][j] - 'a' - 15) / 10.0); // 2dB steps
}
else
{
(*image)[IDX2D(rows - i - 1, j)] = 0.0;
}
}
}
}
static int
bilinear_deriv_x(const void * state, const double xarr[],
const double yarr[], const double zarr[],
size_t xsize, size_t ysize, double x, double y,
gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_p)
{
double xmin, xmax, ymin, ymax, zminmin, zminmax, zmaxmin, zmaxmax;
double dx, dy;
double dt, u;
size_t xi, yi;
if (xa != NULL)
xi = gsl_interp_accel_find(xa, xarr, xsize, x);
else
xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);
if (ya != NULL)
yi = gsl_interp_accel_find(ya, yarr, ysize, y);
else
yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);
xmin = xarr[xi];
xmax = xarr[xi + 1];
ymin = yarr[yi];
ymax = yarr[yi + 1];
zminmin = zarr[IDX2D(xi, yi, xsize, ysize)];
zminmax = zarr[IDX2D(xi, yi + 1, xsize, ysize)];
zmaxmin = zarr[IDX2D(xi + 1, yi, xsize, ysize)];
zmaxmax = zarr[IDX2D(xi + 1, yi + 1, xsize, ysize)];
dx = xmax - xmin;
dy = ymax - ymin;
dt = 1./dx; /* partial t / partial x */
u = (y - ymin)/dy;
*z_p = dt*(-(1.-u)*zminmin + (1.-u)*zmaxmin - u*zminmax + u*zmaxmax);
return GSL_SUCCESS;
}
static int
bilinear_eval(const void * state, const double xarr[], const double yarr[],
const double zarr[], size_t xsize, size_t ysize,
double x, double y, gsl_interp_accel * xa,
gsl_interp_accel * ya, double * z)
{
double xmin, xmax, ymin, ymax, zminmin, zminmax, zmaxmin, zmaxmax;
double dx, dy;
double t, u;
size_t xi, yi;
if (xa != NULL)
xi = gsl_interp_accel_find(xa, xarr, xsize, x);
else
xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);
if (ya != NULL)
yi = gsl_interp_accel_find(ya, yarr, ysize, y);
else
yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);
xmin = xarr[xi];
xmax = xarr[xi + 1];
ymin = yarr[yi];
ymax = yarr[yi + 1];
zminmin = zarr[IDX2D(xi, yi, xsize, ysize)];
zminmax = zarr[IDX2D(xi, yi + 1, xsize, ysize)];
zmaxmin = zarr[IDX2D(xi + 1, yi, xsize, ysize)];
zmaxmax = zarr[IDX2D(xi + 1, yi + 1, xsize, ysize)];
dx = xmax - xmin;
dy = ymax - ymin;
t = (x - xmin)/dx;
u = (y - ymin)/dy;
*z = (1.-t)*(1.-u)*zminmin + t*(1.-u)*zmaxmin + (1.-t)*u*zminmax + t*u*zmaxmax;
return GSL_SUCCESS;
}
static int
bicubic_init(void * vstate, const double xa[], const double ya[],
const double za[], size_t xsize, size_t ysize)
{
bicubic_state_t *state = (bicubic_state_t *) vstate;
gsl_interp_accel *acc = gsl_interp_accel_alloc();
gsl_spline *spline;
gsl_vector *x;
gsl_vector *y;
size_t i, j;
x = gsl_vector_alloc(xsize);
y = gsl_vector_alloc(xsize);
spline = gsl_spline_alloc(gsl_interp_cspline, xsize);
for (j = 0; j <= ysize - 1; j++)
{
for (i = 0; i <= xsize - 1; i++)
{
gsl_vector_set(x, i, xa[i]);
gsl_vector_set(y, i, za[IDX2D(i, j, state)]);
}
gsl_spline_init(spline, x->data, y->data, xsize);
for (i = 0; i <= xsize - 1; i++)
{
state->zx[IDX2D(i, j, state)] = gsl_spline_eval_deriv(spline, xa[i], acc);
}
}
gsl_vector_free(x);
gsl_vector_free(y);
gsl_spline_free(spline);
gsl_interp_accel_reset(acc);
x = gsl_vector_alloc(ysize);
y = gsl_vector_alloc(ysize);
spline = gsl_spline_alloc(gsl_interp_cspline, ysize);
for (i = 0; i <= xsize - 1; i++)
{
for (j = 0; j <= ysize - 1; j++)
{
gsl_vector_set(x, j, ya[j]);
gsl_vector_set(y, j, za[IDX2D(i, j, state)]);
}
gsl_spline_init(spline, x->data, y->data, ysize);
for (j = 0; j <= ysize - 1; j++)
{
state->zy[IDX2D(i, j, state)] = gsl_spline_eval_deriv(spline, ya[j], acc);
}
}
gsl_vector_free(x);
gsl_vector_free(y);
gsl_spline_free(spline);
gsl_interp_accel_reset(acc);
x = gsl_vector_alloc(xsize);
y = gsl_vector_alloc(xsize);
spline = gsl_spline_alloc(gsl_interp_cspline, xsize);
for (j = 0; j <= ysize - 1; j++)
{
for (i = 0; i <= xsize - 1; i++)
{
gsl_vector_set(x, i, xa[i]);
gsl_vector_set(y, i, state->zy[IDX2D(i, j, state)]);
}
gsl_spline_init(spline, x->data, y->data, xsize);
for (i = 0; i <= xsize - 1; i++)
{
state->zxy[IDX2D(i, j, state)] = gsl_spline_eval_deriv(spline, xa[i], acc);
}
}
gsl_vector_free(x);
gsl_vector_free(y);
gsl_spline_free(spline);
gsl_interp_accel_free(acc);
return GSL_SUCCESS;
} /* bicubic_init() */
static int
bicubic_deriv_yy(const void * vstate, const double xarr[], const double yarr[],
const double zarr[], size_t xsize, size_t ysize,
double x, double y,
gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_pp)
{
bicubic_state_t *state = (bicubic_state_t *) vstate;
double xmin, xmax, ymin, ymax;
double zminmin, zminmax, zmaxmin, zmaxmax;
double zxminmin, zxminmax, zxmaxmin, zxmaxmax;
double zyminmin, zyminmax, zymaxmin, zymaxmax;
double zxyminmin, zxyminmax, zxymaxmin, zxymaxmax;
double dx, dy; /* size of the grid cell */
double dt, du;
/*
* t and u are the positions within the grid cell at which we are computing
* the interpolation, in units of grid cell size
*/
double t, u;
double t0, t1, t2, t3, u0, u1;
double v;
size_t xi, yi;
/* first compute the indices into the data arrays where we are interpolating */
if (xa != NULL)
xi = gsl_interp_accel_find(xa, xarr, xsize, x);
else
xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);
if (ya != NULL)
yi = gsl_interp_accel_find(ya, yarr, ysize, y);
else
yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);
/* find the minimum and maximum values on the grid cell in each dimension */
xmin = xarr[xi];
xmax = xarr[xi + 1];
ymin = yarr[yi];
ymax = yarr[yi + 1];
zminmin = zarr[IDX2D(xi, yi, state)];
zminmax = zarr[IDX2D(xi, yi + 1, state)];
zmaxmin = zarr[IDX2D(xi + 1, yi, state)];
zmaxmax = zarr[IDX2D(xi + 1, yi + 1, state)];
/* get the width and height of the grid cell */
dx = xmax - xmin;
dy = ymax - ymin;
t = (x - xmin)/dx;
u = (y - ymin)/dy;
dt = 1./dx; /* partial t / partial x */
du = 1./dy; /* partial u / partial y */
zxminmin = state->zx[IDX2D(xi, yi, state)]/dt;
zxminmax = state->zx[IDX2D(xi, yi + 1, state)]/dt;
zxmaxmin = state->zx[IDX2D(xi + 1, yi, state)]/dt;
zxmaxmax = state->zx[IDX2D(xi + 1, yi + 1, state)]/dt;
zyminmin = state->zy[IDX2D(xi, yi, state)]/du;
zyminmax = state->zy[IDX2D(xi, yi + 1, state)]/du;
zymaxmin = state->zy[IDX2D(xi + 1, yi, state)]/du;
zymaxmax = state->zy[IDX2D(xi + 1, yi + 1, state)]/du;
zxyminmin = state->zxy[IDX2D(xi, yi, state)]/(dt*du);
zxyminmax = state->zxy[IDX2D(xi, yi + 1, state)]/(dt*du);
zxymaxmin = state->zxy[IDX2D(xi + 1, yi, state)]/(dt*du);
zxymaxmax = state->zxy[IDX2D(xi + 1, yi + 1, state)]/(dt*du);
t0 = 1;
t1 = t;
t2 = t*t;
t3 = t*t2;
u0 = 1;
u1 = u;
*z_pp = 0;
v = -3*zminmin + 3*zminmax - 2*zyminmin - zyminmax;
*z_pp += 2*v*t0*u0;
v = 2*zminmin-2*zminmax + zyminmin + zyminmax;
*z_pp += 6*v*t0*u1;
v = -3*zxminmin + 3*zxminmax - 2*zxyminmin - zxyminmax;
*z_pp += 2*v*t1*u0;
v = 2*zxminmin - 2*zxminmax + zxyminmin + zxyminmax;
*z_pp += 6*v*t1*u1;
v = 9*zminmin - 9*zmaxmin + 9*zmaxmax - 9*zminmax + 6*zxminmin + 3*zxmaxmin - 3*zxmaxmax - 6*zxminmax + 6*zyminmin - 6*zymaxmin - 3*zymaxmax + 3*zyminmax + 4*zxyminmin + 2*zxymaxmin + zxymaxmax + 2*zxyminmax;
*z_pp += 2*v*t2*u0;
v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 4*zxminmin - 2*zxmaxmin + 2*zxmaxmax + 4*zxminmax - 3*zyminmin + 3*zymaxmin + 3*zymaxmax - 3*zyminmax - 2*zxyminmin - zxymaxmin - zxymaxmax - 2*zxyminmax;
*z_pp += 6*v*t2*u1;
v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 3*zxminmin - 3*zxmaxmin + 3*zxmaxmax + 3*zxminmax - 4*zyminmin + 4*zymaxmin + 2*zymaxmax - 2*zyminmax - 2*zxyminmin - 2*zxymaxmin - zxymaxmax - zxyminmax;
*z_pp += 2*v*t3*u0;
v = 4*zminmin - 4*zmaxmin + 4*zmaxmax - 4*zminmax + 2*zxminmin + 2*zxmaxmin - 2*zxmaxmax - 2*zxminmax + 2*zyminmin - 2*zymaxmin - 2*zymaxmax + 2*zyminmax + zxyminmin + zxymaxmin + zxymaxmax + zxyminmax;
*z_pp += 6*v*t3*u1;
*z_pp *= du*du;
return GSL_SUCCESS;
}
void RaspiVoice::processImage(cv::Mat rawImage)
{
cv::Mat processedImage = rawImage;
if (verbose)
{
printtime("ProcessImage start");
}
if ((image_source > 0) || (opt.input_filename != ""))
{
if (opt.foveal_mapping)
{
cv::Matx33f cameraMatrix(100, 0, processedImage.cols / 2, 0, 100, processedImage.rows / 2, 0, 0, 1);
cv::Matx41f distCoeffs(5.0, 5.0, 0, 0);
cv::Mat processedImage2;
cv::undistort(processedImage, processedImage2, cameraMatrix, distCoeffs);
float clipzoom = 1.8; //horizontal zoom to remove blinders, decreases resolution if > 1.0
cv::Rect roi(processedImage.cols / 2 - columns / 2 / clipzoom, processedImage.rows / 2 - rows / 2, columns / clipzoom, rows);
processedImage = processedImage2(roi);
}
if (opt.zoom > 1.0)
{
int w = processedImage.cols;
int h = processedImage.rows;
float z = opt.zoom;
cv::Rect roi((w / 2.0) - w / (2.0*z), (h / 2.0) - h / (2.0*z), w/z, h/z);
processedImage = processedImage(roi);
}
//Bring to size needed by ImageToSoundscape:
if (processedImage.rows != rows || processedImage.cols != columns)
{
cv::resize(processedImage, processedImage, cv::Size(columns, rows));
}
if ((opt.blinders > 0) && (opt.blinders < columns/2))
{
processedImage(cv::Rect(0, 0, opt.blinders, rows - 1)).setTo(0);
processedImage(cv::Rect(columns - 1 - opt.blinders, 0, opt.blinders, rows - 1)).setTo(0);
}
if (opt.contrast != 0.0)
{
float avg = 0.0;
for (int y = 0; y < processedImage.rows; y++)
{
for (int x = 0; x < processedImage.cols; x++)
{
avg += processedImage.at<uchar>(y, x);
}
}
avg = avg / (processedImage.rows * processedImage.cols);
for (int y = 0; y < processedImage.rows; y++)
{
for (int x = 0; x < processedImage.cols; x++)
{
int mVal = processedImage.at<uchar>(y, x);
processedImage.at<uchar>(y, x) = cv::saturate_cast<uchar>(mVal + opt.contrast*(mVal - avg));
}
}
}
if (opt.threshold > 0)
{
if (opt.threshold < 255)
{
cv::threshold(processedImage, processedImage, opt.threshold, 255, cv::THRESH_BINARY);
}
else
{
//Auto threshold:
cv::threshold(processedImage, processedImage, 127, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
}
}
if (opt.negative_image)
{
cv::Mat sub_mat = cv::Mat::ones(processedImage.size(), processedImage.type()) * 255;
cv::subtract(sub_mat, processedImage, processedImage);
}
if (opt.edge_detection_opacity > 0.0)
{
cv::Mat blurImage;
cv::Mat edgesImage;
int ratio = 3;
int kernel_size = 3;
int lowThreshold = opt.edge_detection_threshold;
if (lowThreshold <= 0)
{
lowThreshold = 127;
}
cv::blur(processedImage, blurImage, cv::Size(3, 3));
cv::Canny(blurImage, edgesImage, lowThreshold, lowThreshold*ratio, kernel_size);
double alpha = opt.edge_detection_opacity;
if (alpha > 1.0)
{
alpha = 1.0;
}
double beta = (1.0 - alpha);
cv::addWeighted(edgesImage, alpha, processedImage, beta, 0.0, processedImage);
}
if ((opt.flip >= 1) && (opt.flip <= 3))
{
int flipCode;
if (opt.flip == 1) //h
{
flipCode = 1;
}
else if (opt.flip == 2) //v
{
flipCode = 0;
}
else if (opt.flip == 3) //h+v
{
flipCode = -1;
}
cv::flip(processedImage, processedImage, flipCode);
}
if (preview)
{
//Screen views
//imwrite("raspivoice_capture_raw.jpg", rawImage);
//imwrite("raspivoice_capture_scaled_gray.jpg", processedImage);
cv::imshow("RaspiVoice Preview", processedImage);
cv::waitKey(200);
}
/* Set live camera image */
for (int j = 0; j < columns; j++)
{
for (int i = 0; i < rows; i++)
{
int mVal = processedImage.at<uchar>(rows - 1 - i, j) / 16;
if (mVal == 0)
{
(*image)[IDX2D(i, j)] = 0;
}
else
{
(*image)[IDX2D(i, j)] = pow(10.0, (mVal - 15) / 10.0); // 2dB steps
}
}
}
}
}
void ImageToSoundscapeConverter::processStereo(const std::vector<float> &image)
{
float tau1 = 0.5 / omega[rows - 1];
float tau2 = 0.25 * tau1*tau1;
float yl = 0.0, yr = 0.0;
float zl = 0.0, zr = 0.0;
for (int sample = 0; sample < sampleCount; sample++)
{
float q, q2, f1, f2;
if (use_bspline)
{
q = 1.0 * (sample % samplesPerColumn) / (samplesPerColumn - 1);
q2 = 0.5 * q * q;
f1 = (q2 - q + 0.5);
f2 = (0.5 + q - q*q);
}
int j = sample / samplesPerColumn;
if (j > columns - 1)
{
j = columns - 1;
}
float r = 1.0 * sample / (sampleCount - 1); // Binaural attenuation/delay parameter
float theta = (r - 0.5) * TwoPi / 3;
float x = 0.5 * acoustical_size_of_head_m * (theta + sin(theta));
float tl = sample * timePerSample_s;
float tr = tl;
if (use_delay)
{
tr += x / speed_of_sound_m_s; // Time delay model
}
x = fabs(x);
float sl = 0.0, sr = 0.0;
const float *im1, *im2, *im3;
if (j > 0)
{
im1 = &image[IDX2D(0, j - 1)];
}
im2 = &image[IDX2D(0, j)];
if (j < columns - 1)
{
im3 = &image[IDX2D(0, j + 1)];
}
for (int i = 0; i < rows; i++)
{
float a;
if (use_bspline)
{
if (j == 0)
{
a = (1.0 - q2)*im2[i] + q2*im3[i];
}
else if (j == columns - 1)
{
a = f1*im1[i] + f2*im2[i];
}
else
{
a = f1*im1[i] + f2*im2[i] + q2*im3[i];
}
}
else
{
a = im2[i];
}
sl += a * waveformCacheLeftChannel[(sample * rows) + i];
sr += a * waveformCacheRightChannel[(sample * rows) + i];
}
if (sample < sampleCount / (5 * columns))
{
sl = (2.0*rnd() - 1.0) / scale; // Left "click"
}
if (tl < 0.0)
{
sl = 0.0;
}
if (tr < 0.0)
{
sr = 0.0;
}
float ypl = yl;
yl = tau1 / timePerSample_s + tau2 / (timePerSample_s*timePerSample_s);
yl = (sl + yl * ypl + tau2 / timePerSample_s * zl) / (1.0 + yl);
zl = (yl - ypl) / timePerSample_s;
float ypr = yr;
yr = tau1 / timePerSample_s + tau2 / (timePerSample_s*timePerSample_s);
yr = (sr + yr * ypr + tau2 / timePerSample_s * zr) / (1.0 + yr);
zr = (yr - ypr) / timePerSample_s;
uint16_t* sampleBuffer = audioData.Data();
int32_t l = 0.5 + scale * 32768.0 * yl;
if (l > 32767)
{
l = 32767;
} else if (l < -32768)
{
l = -32768;
}
sampleBuffer[2 * sample] = (uint16_t)l;
l = 0.5 + scale * 32768.0 * yr;
if (l > 32767)
{
l = 32767;
}
else if (l < -32768)
{
l = -32768;
}
sampleBuffer[2 * sample + 1] = (uint16_t)l;
}
}