Matrix2 psf2otf(Matrix& psf, Matrix& outSize)
{
Matrix psfSize(1, 2);
psfSize(0, 0) = psf.size1();
psfSize(0, 1) = psf.size2();
Matrix padSize(1, 2);
padSize = outSize - psfSize;
psf = padarray(psf, padSize);
psf = circshift(psf, -Matfloor(psfSize / 2));
Matrix2 otf = fft2(psf);
return otf;
}
void fwt2_cpu(struct wavelet_plan_s* plan, data_t* coeff, data_t* inImage)
{
circshift(plan,inImage);
data_t* origInImage = inImage;
data_t* HxLy = coeff + plan->waveSizes_tr[0]*plan->waveSizes_tr[1];
int l;
for (l = 1; l <= plan->numLevels_tr; ++l){
HxLy += 3*plan->waveSizes_tr[0 + 2*l]*plan->waveSizes_tr[1 + 2*l];
}
int dx = plan->imSize_tr[0];
int dy = plan->imSize_tr[1];
int dxNext = plan->waveSizes_tr[0 + 2*plan->numLevels_tr];
int dyNext = plan->waveSizes_tr[1 + 2*plan->numLevels_tr];
int blockSize = dxNext*dyNext;
// Allocate Memory
data_t* LxLy = plan->tmp_mem_tr;
data_t* tempy = LxLy+blockSize;
data_t* tempxy = tempy+dx*dyNext;
for (l = plan->numLevels_tr; l >= 1; --l)
{
dxNext = plan->waveSizes_tr[0 + 2*l];
dyNext = plan->waveSizes_tr[1 + 2*l];
blockSize = dxNext*dyNext;
HxLy = HxLy - 3*blockSize;
data_t* LxHy = HxLy + blockSize;
data_t* HxHy = LxHy + blockSize;
int newdy = (dy + plan->filterLen-1) / 2;
// Ly
conv_down_2d(tempy, inImage, dy, dx, dx, 1, plan->lod,plan->filterLen);
conv_down_2d(LxLy, tempy, dx, 1, newdy, dx, plan->lod,plan->filterLen);
conv_down_2d(HxLy, tempy, dx, 1, newdy, dx, plan->hid,plan->filterLen);
// Hy
conv_down_2d(tempy, inImage, dy, dx, dx, 1, plan->hid,plan->filterLen);
conv_down_2d(LxHy, tempy, dx, 1, newdy, dx, plan->lod,plan->filterLen);
conv_down_2d(HxHy, tempy, dx, 1, newdy, dx, plan->hid,plan->filterLen);
memcpy(tempxy, LxLy, blockSize*sizeof(data_t));
inImage = tempxy;
dx = dxNext;
dy = dyNext;
}
memcpy(coeff, inImage, plan->waveSizes_tr[0]*plan->waveSizes_tr[1]*sizeof(data_t));
circunshift(plan,origInImage);
}
cv::Mat psf2otf_64F(const cv::Mat &psf, const cv::Size &outSize) {
cv::Size psfSize = psf.size();
cv::Mat new_psf = cv::Mat(outSize, CV_64FC2);
new_psf.setTo(0);
//new_psf(cv::Rect(0,0,psfSize.width, psfSize.height)).setTo(psf);
for (int i = 0; i < psfSize.height; i++) {
for (int j = 0; j < psfSize.width; j++) {
new_psf.at<cv::Vec2d>(i,j)[0] = psf.at<double>(i,j);
}
}
circshift(new_psf, -1*int(floor(psfSize.height*0.5)), -1*int(floor(psfSize.width*0.5)));
cv::Mat otf;
cv::dft(new_psf, otf, cv::DFT_COMPLEX_OUTPUT);
return otf;
}
/********************************************//**
\brief Circular conolution of a 1D array with a 1D filter
\param x : input array (1D)
\param h : filter array (1D)
\return convolved array
***********************************************/
std::vector<float> FWT::cconv(const std::vector<float>& x,
const std::deque<float>& h)
{
int p = h.size();
int pc = (p+1) / 2;
if(p % 2 == 0) pc = p / 2;
std::vector<float> y(x.size(), 0.0f);
for(unsigned int i = 0; i < h.size(); ++i)
{
// Circular shift of x
std::vector<float> circX = circshift(x, i-pc);
// Multiply circX by h[i]
std::transform(circX.begin(), circX.end(), circX.begin(), std::bind2nd(std::multiplies<float>(), h[i]));
// Add result to y
std::transform(circX.begin(), circX.end(), y.begin(), y.begin(), std::plus<float>());
}
return y;
}
//base filter
void L0Smoothing_64F(cv::Mat &im8uc3, cv::Mat& dest, double lambda = 2e-2, double kappa = 2.0)
{
// convert the image to double format
int row = im8uc3.rows, col = im8uc3.cols;
cv::Mat S;
im8uc3.convertTo(S, CV_64FC3, 1./255.);
cv::Mat fx(1,2,CV_64FC1);
cv::Mat fy(2,1,CV_64FC1);
fx.at<double>(0) = 1; fx.at<double>(1) = -1;
fy.at<double>(0) = 1; fy.at<double>(1) = -1;
cv::Size sizeI2D = im8uc3.size();
cv::Mat otfFx = psf2otf_64F(fx, sizeI2D);
cv::Mat otfFy = psf2otf_64F(fy, sizeI2D);
cv::Mat Normin1[3];
cv::Mat single_channel[3];
cv::split(S, single_channel);
for (int k = 0; k < 3; k++) {
cv::dft(single_channel[k], Normin1[k], cv::DFT_COMPLEX_OUTPUT);
}
cv::Mat Denormin2(row, col, CV_64FC1);
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
cv::Vec2d &c1 = otfFx.at<cv::Vec2d>(i,j), &c2 = otfFy.at<cv::Vec2d>(i,j);
Denormin2.at<double>(i,j) = sqr(c1[0]) + sqr(c1[1]) + sqr(c2[0]) + sqr(c2[1]);
}
}
double beta = 2.0*lambda;
double betamax = 1e5;
while (beta < betamax) {
cv::Mat Denormin = 1.0 + beta*Denormin2;
// h-v subproblem
cv::Mat dx[3], dy[3];
for (int k = 0; k < 3; k++) {
cv::Mat shifted_x = single_channel[k].clone();
circshift(shifted_x, 0, -1);
dx[k] = shifted_x - single_channel[k];
cv::Mat shifted_y = single_channel[k].clone();
circshift(shifted_y, -1, 0);
dy[k] = shifted_y - single_channel[k];
}
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
double val =
sqr(dx[0].at<double>(i,j)) + sqr(dy[0].at<double>(i,j)) +
sqr(dx[1].at<double>(i,j)) + sqr(dy[1].at<double>(i,j)) +
sqr(dx[2].at<double>(i,j)) + sqr(dy[2].at<double>(i,j));
if (val < lambda / beta) {
dx[0].at<double>(i,j) = dx[1].at<double>(i,j) = dx[2].at<double>(i,j) = 0.0;
dy[0].at<double>(i,j) = dy[1].at<double>(i,j) = dy[2].at<double>(i,j) = 0.0;
}
}
}
// S subproblem
for (int k = 0; k < 3; k++) {
cv::Mat shift_dx = dx[k].clone();
circshift(shift_dx, 0, 1);
cv::Mat ddx = shift_dx - dx[k];
cv::Mat shift_dy = dy[k].clone();
circshift(shift_dy, 1, 0);
cv::Mat ddy = shift_dy - dy[k];
cv::Mat Normin2 = ddx + ddy;
cv::Mat FNormin2;
cv::dft(Normin2, FNormin2, cv::DFT_COMPLEX_OUTPUT);
cv::Mat FS = Normin1[k] + beta*FNormin2;
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
FS.at<cv::Vec2d>(i,j)[0] /= Denormin.at<double>(i,j);
FS.at<cv::Vec2d>(i,j)[1] /= Denormin.at<double>(i,j);
}
}
cv::Mat ifft;
cv::idft(FS, ifft, cv::DFT_SCALE | cv::DFT_COMPLEX_OUTPUT);
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
single_channel[k].at<double>(i,j) = ifft.at<cv::Vec2d>(i,j)[0];
}
}
}
beta *= kappa;
}
cv::merge(single_channel, 3, dest);
}
void L0Smoothing(cv::Mat &im8uc3, cv::Mat& dest, const float lambda, const float kappa)
{
// convert the image to double format
int row = im8uc3.rows, col = im8uc3.cols;
int size = row*col;
cv::Mat S;
im8uc3.convertTo(S, CV_32FC3, 1./255.);
cv::Mat fx(1,2,CV_32FC1);
cv::Mat fy(2,1,CV_32FC1);
fx.at<float>(0) = 1; fx.at<float>(1) = -1;
fy.at<float>(0) = 1; fy.at<float>(1) = -1;
cv::Size sizeI2D = im8uc3.size();
cv::Mat otfFx = psf2otf(fx, sizeI2D);
cv::Mat otfFy = psf2otf(fy, sizeI2D);
cv::Mat Normin1[3];
cv::Mat single_channel[3];
cv::split(S, single_channel);
cv::Mat buffer(S.size(),CV_32F);
for (int k = 0; k < 3; k++)
{
cv::dft(single_channel[k], Normin1[k], cv::DFT_COMPLEX_OUTPUT);
}
cv::Mat Denormin2(row, col, CV_32FC1);
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col; j++)
{
cv::Vec2f &c1 = otfFx.at<cv::Vec2f>(i,j), &c2 = otfFy.at<cv::Vec2f>(i,j);
Denormin2.at<float>(i,j) = SQR(c1[0]) + SQR(c1[1]) + SQR(c2[0]) + SQR(c2[1]);
}
}
// the bigger beta the more time iteration
float beta = 4.f*lambda;
// the smaller betamax the less segmentation count
double betamax = 1e5;
//float betamax = 3e1;
cv::Mat Denormin;
cv::Mat shifted_x;
cv::Mat shifted_y;
cv::Mat dx[3], dy[3];
cv::Mat FNormin2;
while (beta < betamax)
{
addWeighted(Mat::ones(Denormin2.size(),Denormin2.type()), 1.0, Denormin2, beta, 0.0, Denormin);
Denormin = 1.f/Denormin;
// h-v subproblem
for (int k = 0; k < 3; k++)
{
single_channel[k].copyTo(shifted_x);
circshift(shifted_x, 0, -1, buffer);
dx[k] = shifted_x - single_channel[k];
single_channel[k].copyTo(shifted_y);
circshift(shifted_y, -1, 0, buffer);
dy[k] = shifted_y - single_channel[k];
}
const float lb = lambda/beta;
float* dx0 = dx[0].ptr<float>(0);
float* dx1 = dx[1].ptr<float>(0);
float* dx2 = dx[2].ptr<float>(0);
float* dy0 = dy[0].ptr<float>(0);
float* dy1 = dy[1].ptr<float>(0);
float* dy2 = dy[2].ptr<float>(0);
const __m128 mlb = _mm_set1_ps(lb);
cv::Mat buff(4,1,CV_32F);
float* b = (float*)buff.ptr<float>(0);
int i=0;
for(;i<=size-4;i+=4)
{
__m128 x = _mm_load_ps(dx0+i);
__m128 v = _mm_mul_ps(x,x);
x = _mm_load_ps(dx1+i);
v = _mm_add_ps(v, _mm_mul_ps(x,x));
x = _mm_load_ps(dx2+i);
v = _mm_add_ps(v, _mm_mul_ps(x,x));
x = _mm_load_ps(dy0+i);
v = _mm_add_ps(v, _mm_mul_ps(x,x));
x = _mm_load_ps(dy1+i);
v = _mm_add_ps(v, _mm_mul_ps(x,x));
x = _mm_load_ps(dy2+i);
v = _mm_add_ps(v, _mm_mul_ps(x,x));
_mm_store_ps(b,v);
if(b[0]< lb)
{
dx0[i]=dx1[i]=dx2[i]=dy0[i]=dy1[i]=dy2[i]=0.f;
}
if(b[1]< lb)
{
dx0[i+1]=dx1[i+1]=dx2[i+1]=dy0[i+1]=dy1[i+1]=dy2[i+1]=0.f;
}
if(b[2]< lb)
{
dx0[i+2]=dx1[i+2]=dx2[i+2]=dy0[i+2]=dy1[i+2]=dy2[i+2]=0.f;
}
if(b[3]< lb)
{
dx0[i+3]=dx1[i+3]=dx2[i+3]=dy0[i+3]=dy1[i+3]=dy2[i+3]=0.f;
}
}
for(;i<size;i++)
{
float v = dx0[i]*dx0[i]+dx1[i]*dx1[i]+dx2[i]*dx2[i]+dy0[i]*dy0[i]+dy1[i]*dy1[i]+dy2[i]*dy2[i];
if(v < lb)
{
dx0[i]=dx1[i]=dx2[i]=dy0[i]=dy1[i]=dy2[i]=0.f;
}
}
// S subproblem
for (int k = 0; k < 3; k++)
{
dx[k].copyTo(shifted_x);
circshift(shifted_x, 0, 1, buffer);
dy[k].copyTo(shifted_y);
circshift(shifted_y, 1, 0, buffer);
cv::Mat Normin2 = shifted_x - dx[k] + shifted_y - dy[k];
cv::dft(Normin2, FNormin2, cv::DFT_COMPLEX_OUTPUT);
//cv::Mat FS = Normin1[k] + beta*FNormin2;
//FS*=real(Denormin);
float* n1 = (float*)Normin1[k].ptr<Vec2f>(0);
float* fn2 = (float*)FNormin2.ptr<Vec2f>(0);
float* D = Denormin.ptr<float>(0);
const __m128 mbeta = _mm_set1_ps(beta);
int i=0;
for(;i<=size*2-4;i+=4)
{
__m128 mfn2 =_mm_add_ps(_mm_loadu_ps(n1+i), _mm_mul_ps(mbeta,_mm_loadu_ps(fn2+i)));
__m128 mn1 = _mm_loadu_ps(D+(i>>1));
mn1 = _mm_shuffle_ps(mn1,mn1,_MM_SHUFFLE(1, 1, 0, 0));
mfn2 = _mm_mul_ps(mn1,mfn2);
_mm_storeu_ps(fn2+i,mfn2);
}
for(;i<size*2;i+=2)
{
const float dd = D[(i>>1)];
fn2[i] = dd*(n1[i] + beta*fn2[i]);
fn2[i+1] = dd*(n1[i+1] + beta*fn2[i+1]);
}
cv::idft(FNormin2, single_channel[k], cv::DFT_SCALE | cv::DFT_REAL_OUTPUT);
}
beta *= kappa;
}
cv::merge(single_channel, 3, S);
S.convertTo(dest, CV_8UC3, 255.f);
}
void fwt3_cpu(struct wavelet_plan_s* plan, data_t* coeff, data_t* inImage)
{
circshift(plan,inImage);
data_t* origInImage = inImage;
data_t* HxLyLz = coeff + plan->waveSizes_tr[0]*plan->waveSizes_tr[1]*plan->waveSizes_tr[2];
int l;
for (l = 1; l <= plan->numLevels_tr; ++l){
HxLyLz += 7*plan->waveSizes_tr[0 + 3*l]*plan->waveSizes_tr[1 + 3*l]*plan->waveSizes_tr[2 + 3*l];
}
int dx = plan->imSize_tr[0];
int dy = plan->imSize_tr[1];
int dz = plan->imSize_tr[2];
int dxNext = plan->waveSizes_tr[0 + 3*plan->numLevels_tr];
int dyNext = plan->waveSizes_tr[1 + 3*plan->numLevels_tr];
int dzNext = plan->waveSizes_tr[2 + 3*plan->numLevels_tr];
int blockSize = dxNext*dyNext*dzNext;
data_t* LxLyLz = plan->tmp_mem_tr;
data_t* tempz = LxLyLz + blockSize;
data_t* tempyz = tempz + dx*dy*dzNext;
data_t* tempxyz = tempyz + dx*dyNext*dzNext;
for (l = plan->numLevels_tr; l >= 1; --l)
{
dxNext = plan->waveSizes_tr[0 + 3*l];
dyNext = plan->waveSizes_tr[1 + 3*l];
dzNext = plan->waveSizes_tr[2 + 3*l];
blockSize = dxNext*dyNext*dzNext;
HxLyLz = HxLyLz - 7*blockSize;
data_t* LxHyLz = HxLyLz + blockSize;
data_t* HxHyLz = LxHyLz + blockSize;
data_t* LxLyHz = HxHyLz + blockSize;
data_t* HxLyHz = LxLyHz + blockSize;
data_t* LxHyHz = HxLyHz + blockSize;
data_t* HxHyHz = LxHyHz + blockSize;
int dxy = dx*dy;
int newdz = (dz + plan->filterLen-1) / 2;
int newdy = (dy + plan->filterLen-1) / 2;
int newdxy = dx*newdy;
// Lz
conv_down_3d(tempz, inImage, dz, dxy, dx, 1, dy, dx, plan->lod,plan->filterLen);
// LyLz
conv_down_3d(tempyz, tempz, dy, dx, dx, 1, newdz, dxy, plan->lod,plan->filterLen);
conv_down_3d(LxLyLz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->lod,plan->filterLen);
conv_down_3d(HxLyLz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->hid,plan->filterLen);
// HyLz
conv_down_3d(tempyz, tempz, dy, dx, dx, 1, newdz, dxy, plan->hid,plan->filterLen);
conv_down_3d(LxHyLz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->lod,plan->filterLen);
conv_down_3d(HxHyLz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->hid,plan->filterLen);
// Hz
conv_down_3d(tempz, inImage, dz, dxy, dx, 1, dy, dx, plan->hid,plan->filterLen);
// LyHz
conv_down_3d(tempyz, tempz, dy, dx, dx, 1, newdz, dxy, plan->lod,plan->filterLen);
conv_down_3d(LxLyHz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->lod,plan->filterLen);
conv_down_3d(HxLyHz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->hid,plan->filterLen);
// HyHz
conv_down_3d(tempyz, tempz, dy, dx, dx, 1, newdz, dxy, plan->hid,plan->filterLen);
conv_down_3d(LxHyHz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->lod,plan->filterLen);
conv_down_3d(HxHyHz, tempyz, dx, 1, newdy, dx, newdz, newdxy, plan->hid,plan->filterLen);
memcpy(tempxyz, LxLyLz, blockSize*sizeof(data_t));
inImage = tempxyz;
dx = dxNext;
dy = dyNext;
dz = dzNext;
}
// Final LxLyLz
memcpy(coeff, inImage, plan->waveSizes_tr[0]*plan->waveSizes_tr[1]*plan->waveSizes_tr[2]*sizeof(data_t));
circunshift(plan,origInImage);
}
void IQSSBDemodulator::process_block(Ipp32f* iq_block, Ipp32f* out_block)
{
static bool flip=false;
// Deinterleave to real and imaginary (I and Q) buffers
ippsDeinterleave_32f(iq_block, 2, BLKSIZE, _iq);
ippsZero_32f(_in_re+BLKSIZE, NFFT-BLKSIZE);
ippsZero_32f(_in_im+BLKSIZE, NFFT-BLKSIZE);
// _in_re now contains the real/I part and
// _in_im now contains the imaginary/Q part
ippsFFTFwd_CToC_32f_I(_in_re, _in_im,
_fft_specs, _fft_buf);
ippsCartToPolar_32f(_in_re, _in_im,
_in_m, _in_p,
NFFT);
// layout of frequency bins is
// NFFT/2 to NFFT-1 and then continues from 0 to NFFT/2-1
// shift desired part to 0Hz
int lo = _lo*NFFT;
circshift(_in_m, NFFT, lo);
circshift(_in_p, NFFT, lo);
// zero out undesired sideband
if(_sideband == USB)
{
// zero out LSB, that is NFFT/2 to NFFT-1
ippsZero_32f(_in_m+NFFT/2, NFFT/2);
ippsZero_32f(_in_p+NFFT/2, NFFT/2);
}
else // _sideband must be LSB
{
// zero out USB, that is 0 to NFFT/2-1
ippsZero_32f(_in_m, NFFT/2);
ippsZero_32f(_in_p, NFFT/2);
}
// filter the passband
ippsMul_32f_I(_fir_taps_m, _in_m, NFFT);
ippsAdd_32f_I(_fir_taps_p, _in_p, NFFT);
// return to time domain
ippsPolarToCart_32f(_in_m, _in_p,
_in_re, _in_im,
NFFT);
ippsFFTInv_CToC_32f_I(_in_re, _in_im,
_fft_specs, _fft_buf);
// do overlap/add
//
// 1) add the residual from last round
ippsAdd_32f_I(_residual_re, _in_re, _residual_length);
ippsAdd_32f_I(_residual_im, _in_im, _residual_length);
// 2) Store the new residual
if(flip)
{
ippsMulC_32f_I(-1.0, _in_re, NFFT);
ippsMulC_32f_I(-1.0, _in_im, NFFT);
flip=!flip;
}
ippsCopy_32f(_in_re+BLKSIZE, _residual_re, _residual_length);
ippsCopy_32f(_in_im+BLKSIZE, _residual_im, _residual_length);
// agc
agc(_in_re, BLKSIZE);
// deliver the result
ippsCopy_32f(_in_re, out_block, BLKSIZE);
}
