IQSSBDemodulator::IQSSBDemodulator()
{
_sideband = USB;
ippsFFTInitAlloc_C_32f(&_fft_specs, ORDER, IPP_FFT_DIV_BY_SQRTN, ippAlgHintNone);
int fft_bufsize;
ippsFFTGetBufSize_C_32f(_fft_specs, &fft_bufsize);
_fft_buf = ippsMalloc_8u(fft_bufsize);
// allocate buffers
_in_re = ippsMalloc_32f(NFFT);
_in_im = ippsMalloc_32f(NFFT);
_in_p = ippsMalloc_32f(NFFT);
_in_m = ippsMalloc_32f(NFFT);
_residual_re = ippsMalloc_32f(NFFT);
_residual_im = ippsMalloc_32f(NFFT);
// prepare for ippsDeinterleave
_iq[0] = _in_re;
_iq[1] = _in_im;
// design initial filter
_taps_len = BLKSIZE;
_fir_taps_re = ippsMalloc_32f(NFFT);
_fir_taps_im= ippsMalloc_32f(NFFT);
_fir_taps_m = ippsMalloc_32f(NFFT); // magnitude
_fir_taps_p = ippsMalloc_32f(NFFT); // phase
_lowFreq = 200.0/48000.0;
_highFreq = 3000.0/48000.0;
this->setFilter(_lowFreq, _highFreq);
_residual_length = NFFT - _taps_len -1;
}
roiDetector::roiDetector(const CameraPrx& camera , RoboCompVision::TCamParams params_, int nCam_, QObject *parent):
QObject(parent), _camera(camera), nCam(nCam_), params(params_)
{
qDebug() << "roiDetector::roiDetector() -> Initiating roiDetector number " << nCam;
Umbral_Harris=UMBRAL_HARRIS;
Umbral_Harris_Min=UMBRAL_HARRIS_MIN;
Umbral_Hess=UMBRAL_HESS;
Umbral_Laplacian=(uchar)UMBRAL_LAPLACIAN;
sizePir=(IppiSize *) malloc(params.pyrLevels*sizeof(IppiSize));
infoPrisma=(PrismaROI *) malloc(params.pyrLevels*sizeof(PrismaROI));
imagen = ippsMalloc_8u(params.size);
imagenV.resize(params.size);
iniPiramide();
IppiSize sizeImage;
sizeImage.width=PAN_RANK;
sizeImage.height=TILT_RANK;
Mapa_Umbral_Harris1=(Ipp32f *) ippsMalloc_32f(PAN_RANK*TILT_RANK*sizeof(Ipp32f));
Mapa_Umbral_Harris2=(Ipp32f *) ippsMalloc_32f(PAN_RANK*TILT_RANK*sizeof(Ipp32f));
ippiSet_32f_C1R(-1.,Mapa_Umbral_Harris1,PAN_RANK*sizeof(Ipp32f),sizeImage);
ippiSet_32f_C1R(-1.,Mapa_Umbral_Harris2,PAN_RANK*sizeof(Ipp32f),sizeImage);
Mapa_Umbral_Harris_Act=Mapa_Umbral_Harris1;
Mapa_Umbral_Harris_Ant=Mapa_Umbral_Harris2;
//LogPolar
// lp = new LPolar(64,200,height,height);
}
void IQSSBDemodulator::agc(Ipp32f* b, int len)
{
const Ipp32f MAX_GAIN = 1000.0f;
const int AGC_HANG = 10;
const int AGC_ATTACK_COUNT = 48;
static Ipp32f prev_gain = 0;
static int agcloop = 0;
static Ipp32f* gain_mult = ippsMalloc_32f(BLKSIZE);
static Ipp32f* gain_factor = ippsMalloc_32f(AGC_HANG);
agcloop = (agcloop+1)%AGC_HANG;
Ipp32f Vpk;
ippsMax_32f(b, len, &Vpk);
Ipp32f gain = MAX_GAIN;
if (Vpk>(100*IPP_MINABS_32F))
{
gain_factor[agcloop] = 0.5/Vpk;
ippsMin_32f(gain_factor, AGC_HANG, &gain);
}
if (gain>MAX_GAIN)
{
gain = MAX_GAIN;
}
ippsSet_32f(gain, gain_mult, len);
Ipp32f gain_step = (gain-prev_gain)/(AGC_ATTACK_COUNT+0.0);
for(int n=0; n<AGC_ATTACK_COUNT; n++)
{
gain_mult[n] = prev_gain+n*gain_step;
}
prev_gain = gain;
ippsMul_32f_I(gain_mult, b, len);
}
void OGGFileAudioSink::playSamples(Ipp32f* samples)
{
static int phase = 0;
static Ipp32f* downsamples = ippsMalloc_32f(_count);
int downcount;
ippsSampleDown_32f(samples, _count, downsamples, &downcount, 6, &phase);
sf_count_t relcnt = sf_write_float(_f, downsamples, downcount);
}
void IQSSBDemodulator::fftshift(Ipp32f* b, int len)
{
static Ipp32f* tmp = ippsMalloc_32f(len/2);
ippsCopy_32f(b, tmp, len/2);
ippsCopy_32f(b+len/2, b, len/2);
ippsCopy_32f(tmp, b+len/2, len/2);
}
Ipp32f* CRunDeconvFFT::createKernel(CImage* pImg)
{
IppiSize size = pImg->GetActualSize();
int channels = pImg->Channels();
int kernelSize = GetKernelSize(pImg);
if ((kernelSize == size.width) && (kernelSize*4*channels == pImg->Step()))
return (Ipp32f*)pImg->GetRoiPtr();
Ipp32f* pKernel = ippsMalloc_32f(kernelSize*kernelSize*channels);
Ipp8u* pSrc = (Ipp8u*)pImg->GetRoiPtr();
Ipp32f* pDst = pKernel;
for (int y=0; y < size.height; y++)
{
ippsCopy_32f((Ipp32f*)pSrc, pDst, kernelSize*channels);
pSrc += pImg->Step();
pDst += kernelSize*channels;
}
return pKernel;
}
void IQSSBDemodulator::circshift(Ipp32f* b, int len, int lo)
{
fftshift(b, len);
static Ipp32f* tmp = ippsMalloc_32f(len);
if(lo>=0)
{
ippsCopy_32f(b, tmp, lo);
ippsMove_32f(b+lo, b, len-lo);
ippsCopy_32f(tmp, b+len-lo, lo);
}
else
{
int abs_lo = -lo;
ippsCopy_32f(b+len-abs_lo, tmp, abs_lo);
ippsMove_32f(b, b+abs_lo, len-abs_lo);
ippsCopy_32f(tmp, b, abs_lo);
}
fftshift(b, len);
}
Ipp32f (*ParticleStatistics(Image2D &image_localmax, Image2D &image_in,
const int mask_radius, const int feature_radius, int &counter))[8]
{
IppStatus status;
//setup kernels
Convolution_Kernel ConvolutionKernels(mask_radius, image_localmax.get_width(), image_localmax.get_length());
//Convert 32-bit FP image data in image_localmax to 8-bit integer data
Ipp8u *localmaxdata = ippsMalloc_8u((int) image_localmax.get_numberofpixels());
int stepsize = image_localmax.get_width();
IppiSize fullROI = image_localmax.get_ROIfull();
status = ippiConvert_32f8u_C1R(image_localmax.get_image2D(), image_localmax.get_stepsize(),
localmaxdata, stepsize, fullROI, ippRndNear);
//Determine the number of particles (raw; not yet excluded from border region)
Ipp64f numberofmaxima = 0;
status = ippiSum_8u_C1R(localmaxdata, stepsize, image_localmax.get_ROIfull(), &numberofmaxima);
//Set up structures for counting particle data
int numberofpixels = image_localmax.get_numberofpixels();
Ipp32f (*particledata)[8] = new Ipp32f[(int) numberofmaxima + 1][8];
//border region (local maxima here are excluded from future operations)
int border = feature_radius; //minimum distance from the border, in pixels
int minx = border, miny = border;
int maxx = image_localmax.get_width() - minx;
int maxy = image_localmax.get_height() - miny;
int xval = 0;
int yval = 0;
counter = 0; //index that keeps track of which particle
int imagewidth = image_in.get_width();
//determine integer x and y values (in pixels) for each local maximum outside
//of border exclusion area
for(int j = 0; j < numberofpixels; j++) {
if(localmaxdata[j] == 1) {
xval = image_in.getx(j);
yval = image_in.gety(j);
if (xval > minx && xval < maxx && yval > miny && yval < maxy) {
particledata[counter][0] = j;
particledata[counter][1] = xval;
particledata[counter][2] = yval;
counter++;
}
}
}
//extract local region around each maximum
int extract_radius = mask_radius;
int extract_diameter = 2 * extract_radius + 1;
int extract_size = extract_diameter * extract_diameter;
int extract_step = 4 * extract_diameter;
IppiSize extract_ROI = {extract_diameter, extract_diameter};
int extract_offset = extract_radius * (1 + image_in.get_width()); //calculate _relative_ offset (in pixels) of the array index
Ipp32f *extracted_square = ippsMalloc_32f(extract_size);
Ipp32f *multiply_result = ippsMalloc_32f(extract_size);
Ipp32f *inputimage = ippsMalloc_32f(image_in.get_numberofpixels());
status = ippiCopy_32f_C1R(image_in.get_image2D(), image_in.get_stepsize(),
inputimage, image_in.get_stepsize(), image_in.get_ROIfull());
for(int i = 0; i < counter; i++) {
int extract_index = particledata[i][0];
int copy_offset = extract_index - extract_offset; //this is the starting point for ROI (in pixels!!)
status = ippiCopy_32f_C1R(inputimage + copy_offset, image_in.get_stepsize(),
extracted_square, extract_step, extract_ROI);
//Calculate mass
Ipp64f total_mass = 0;
status = ippiMul_32f_C1R(extracted_square, extract_step, ConvolutionKernels.get_circle_kernel(), ConvolutionKernels.get_kernel_step(),
multiply_result, extract_step, extract_ROI);
status = ippiSum_32f_C1R(multiply_result, extract_step, extract_ROI, &total_mass, ippAlgHintNone);
//Calculate x-offset (to be added to integral position)
Ipp64f x_sum = 0;
status = ippiMul_32f_C1R(extracted_square, extract_step, ConvolutionKernels.get_ramp_x_kernel(), ConvolutionKernels.get_kernel_step(),
multiply_result, extract_step, extract_ROI);
status = ippiSum_32f_C1R(multiply_result, extract_step, extract_ROI, &x_sum, ippAlgHintNone);
Ipp32f x_offset = (x_sum / total_mass) - extract_radius - 1;
//Calculate y-offset (to be added to integral position)
Ipp64f y_sum = 0;
status = ippiMul_32f_C1R(extracted_square, extract_step, ConvolutionKernels.get_ramp_y_kernel(), ConvolutionKernels.get_kernel_step(),
multiply_result, extract_step, extract_ROI);
status = ippiSum_32f_C1R(multiply_result, extract_step, extract_ROI, &y_sum, ippAlgHintNone);
Ipp32f y_offset = (y_sum / total_mass) - extract_radius - 1;
//Calculate r^2
Ipp64f r2_sum = 0;
status = ippiMul_32f_C1R(extracted_square, extract_step, ConvolutionKernels.get_r2_kernel(), ConvolutionKernels.get_kernel_step(),
multiply_result, extract_step, extract_ROI);
status = ippiSum_32f_C1R(multiply_result, extract_step, extract_ROI, &r2_sum, ippAlgHintNone);
Ipp32f r2_val = r2_sum / total_mass;
//Calculate "multiplicity": how many particles are within the area calculated by the masks
Ipp64f multiplicity = 0;
status = ippiSum_32f_C1R(image_localmax.get_image2D()+copy_offset, image_localmax.get_stepsize(),
extract_ROI, &multiplicity, ippAlgHintNone);
Ipp32f multiplicity_val = multiplicity;
//assign values to particle data array
if (total_mass > 0) {
particledata[i][1] += x_offset;
particledata[i][2] += y_offset;
particledata[i][3] = x_offset;
particledata[i][4] = y_offset;
particledata[i][5] = total_mass;
particledata[i][6] = r2_val;
particledata[i][7] = multiplicity_val;
}
}
//Cleanup memory (this part is critical---program otherwise crashes!!)
ippsFree(extracted_square);
extracted_square = NULL;
ippsFree(multiply_result);
multiply_result = NULL;
ippsFree(inputimage);
inputimage = NULL;
ippsFree(localmaxdata);
localmaxdata = NULL;
return particledata;
}
void roiDetector::iniPiramide()
{
int i;
int tamBuftmpHarris;
ippiPyrDownGetBufSize_Gauss5x5(params.width,ipp8u,1,&tamBuftmpPirG);
buftmpPirG = (Ipp8u *) ippsMalloc_8u(tamBuftmpPirG);
sizePir[0].width=params.width;
sizePir[0].height=params.height;
sizeWholePir=params.size;
ApiramideG=(TipoPiramide *) malloc(sizeof(TipoPiramide));
BpiramideG=(TipoPiramide *) malloc(sizeof(TipoPiramide));
LecpiramideG=ApiramideG;
EscpiramideG=BpiramideG;
(* ApiramideG)[0] = (Ipp8u *) ippsMalloc_8u(sizePir[0].width*sizePir[0].height);
(* BpiramideG)[0] = (Ipp8u *) ippsMalloc_8u(sizePir[0].width*sizePir[0].height);
respHarris[0] = (Ipp32f *) ippsMalloc_32f(sizePir[0].width*sizePir[0].height*sizeof(Ipp32f));
mapaHarris[0] = (Ipp8u *) ippsMalloc_8u(sizePir[0].width*sizePir[0].height);
for (i=1; i<params.pyrLevels; i++) {
sizePir[i].width=(sizePir[i-1].width)/2;
sizePir[i].height=(sizePir[i-1].height)/2;
sizeWholePir+=sizePir[i].width*sizePir[i].height;
(* ApiramideG)[i] = (Ipp8u *) ippsMalloc_8u(sizePir[i].width*sizePir[i].height);
(* BpiramideG)[i] = (Ipp8u *) ippsMalloc_8u(sizePir[i].width*sizePir[i].height);
respHarris[i] = (Ipp32f *) ippsMalloc_32f(sizePir[i].width*sizePir[i].height*sizeof(Ipp32f));
mapaHarris[i] = (Ipp8u *) ippsMalloc_8u(sizePir[i].width*sizePir[i].height);
}
ippiPyrUpGetBufSize_Gauss5x5(sizePir[0].width,ipp8u,1,&tamBuftmpPirL);
buftmpPirL = (Ipp8u *) ippsMalloc_8u(tamBuftmpPirL);
for (i=0; i<params.pyrLevels; i++)
piramideL[i] = (Ipp8u *) ippsMalloc_8u(sizePir[i].width*sizePir[i].height);
//Inicializaci� datos para esquinas
AROIList=new TipoROIList;
BROIList=new TipoROIList;
LecROIList=AROIList;
EscROIList=BROIList;
A=true;
for (i=0; i<params.pyrLevels; i++) {
AROIList->tambufROIs[i]=10*sizeof(int)*NFEAT;
AROIList->numROIs[i]=0;
AROIList->ROIList[i]=(int *) calloc(AROIList->tambufROIs[i],1);
BROIList->tambufROIs[i]=10*sizeof(int)*NFEAT;
BROIList->numROIs[i]=0;
BROIList->ROIList[i]=(int *) calloc(BROIList->tambufROIs[i],1);
tambufesqHarris[i]=10*sizeof(int)*2;
numesqHarris[i]=0;
esqHarris[i]=(int *) malloc(tambufesqHarris[i]);
ippiMinEigenValGetBufferSize_32f_C1R(sizePir[i], WHARRIS, WHARRIS, &tamBuftmpHarris);
buftmpHarris[i] = (Ipp8u *) ippsMalloc_8u(tamBuftmpHarris);
}
//Inicializaci� informaci� para el prisma (el ltimo nivel no se utiliza)
for (i=params.pyrLevels-1; i>=0; i--) {
infoPrisma[i].sizeImg.width=sizePir[i].width;
infoPrisma[i].sizeImg.height=sizePir[i].height;
if (i>=params.pyrLevels-2) {
infoPrisma[i].sizeROI.width=sizePir[i].width; //tama� del prisma=tama� del penltimo nivel
infoPrisma[i].sizeROI.height=sizePir[i].height;
} else {
infoPrisma[i].sizeROI.width=sizePir[params.pyrLevels-3].width; //tama� del prisma=tama� del penltimo nivel
infoPrisma[i].sizeROI.height=sizePir[params.pyrLevels-3].height;
}
infoPrisma[i].xIni=(infoPrisma[i].sizeImg.width-infoPrisma[i].sizeROI.width)/2;
infoPrisma[i].yIni=(infoPrisma[i].sizeImg.height-infoPrisma[i].sizeROI.height)/2;
infoPrisma[i].despIni=infoPrisma[i].yIni*infoPrisma[i].sizeImg.width+infoPrisma[i].xIni;
}
}
float *allocate(size_t count)
{
float *ptr = ippsMalloc_32f(count);
if (!ptr) throw (std::bad_alloc());
return ptr;
}