2 * psychlops_FFTW_bridge.cpp
\r
3 * Psychlops Standard Library (Universal)
\r
5 * Last Modified 2010/03/05 by Kenchi HOSOKAWA
\r
6 * (C) 2010 Kenchi HOSOKAWA, Kazushi MARUYA, Takao SATO
\r
10 #include "../../../psychlops_core.h"
\r
11 #include "psychlops_fftw_bridge.h"
\r
16 namespace Psychlops {
\r
18 double sigmoid(double x, double d, double a) { return 1 / ( 1 + pow(Math::E, -(a*(x-d))) ); }
\r
19 double log2NormalDistibution(double log_x, double octave_mu, double octave_sigma) { return Math::normalDistibution(log(log_x)*Math::LOG2E, octave_mu, octave_sigma); }
\r
20 double cumulativeLog2NormalDistibution(double log_x, double octave_mu, double octave_sigma) { return Math::cumulativeNormalDistibution(log(log_x)*Math::LOG2E, octave_mu, octave_sigma); }
\r
21 enum { REAL, IMAGINARY };
\r
27 construct_default();
\r
31 construct_default();
\r
34 FFT1::FFT1(const Matrix &source)
\r
36 construct_default();
\r
43 void FFT1::construct_default()
\r
46 img_spc=0; frq_spc=0;
\r
48 void FFT1::release()
\r
50 if(img_spc!=0) { free(img_spc); img_spc = 0; }
\r
51 if(frq_spc!=0) { free(frq_spc); frq_spc = 0; }
\r
54 void FFT1::set(int wid)
\r
56 if(img_spc==0 || frq_spc==0 || (width_ != wid) ) {
\r
58 img_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid);
\r
59 frq_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid);
\r
65 void FFT1::set(const Matrix &source)
\r
67 if(source.getRows()==1) {
\r
68 set(source.getCols());
\r
69 for(int x=0; x<width_; x++) {
\r
70 img_spc[x][REAL] = source(1, x+1);
\r
71 img_spc[x][IMAGINARY] = 0.0;
\r
76 double FFT1::getDC()
\r
78 return sqrt(frq_spc[0][REAL]*frq_spc[0][REAL]+frq_spc[0][IMAGINARY]*frq_spc[0][IMAGINARY]);
\r
80 double FFT1::setDC(double l)
\r
82 frq_spc[0][REAL] = l;
\r
83 frq_spc[0][IMAGINARY] = 0;
\r
90 fftw_plan plan = fftw_plan_dft_1d(width_, img_spc, frq_spc, FFTW_FORWARD, FFTW_ESTIMATE);
\r
96 fftw_plan plan = fftw_plan_dft_1d(width_, frq_spc, img_spc, FFTW_BACKWARD, FFTW_ESTIMATE);
\r
99 void FFT1::normalizeFFT()
\r
101 double num = width_;
\r
102 for (int i=0; i<num; i++) {
\r
103 frq_spc[i][REAL] /= num;
\r
104 frq_spc[i][IMAGINARY] /= num;
\r
107 void FFT1::getImage(Matrix &target)
\r
111 if( (target.getCols()!=width_)) {
\r
113 target.set(1, width_);
\r
115 for (int x=0; x<width_; x++) {
\r
116 rl = img_spc[x][REAL];
\r
117 im = img_spc[x][IMAGINARY];
\r
118 target(1, x+1) = sqrt(rl*rl+im*im);
\r
121 void FFT1::getSpectrum(Matrix &target, double gamma)
\r
123 const int right = width_%2==0 ? left : left+1;
\r
126 if( (target.getCols()!=width_) ) {
\r
128 target.set(1, width_);
\r
131 for (int x=0; x<right; x++) {
\r
133 rl = frq_spc[x][REAL];
\r
134 im = frq_spc[x][IMAGINARY];
\r
135 target(v, h) = pow( sqrt(rl*rl+im*im), gamma );
\r
137 for (int x=right; x<width_; x++) {
\r
139 rl = frq_spc[x][REAL];
\r
140 im = frq_spc[x][IMAGINARY];
\r
141 target(v, h) = pow( sqrt(rl*rl+im*im), gamma );
\r
149 construct_default();
\r
151 FFT2::FFT2(int wid, int hei)
\r
153 construct_default();
\r
156 FFT2::FFT2(const Image &source)
\r
158 construct_default();
\r
161 FFT2::FFT2(const Matrix &source)
\r
163 construct_default();
\r
166 FFT2::FFT2(const Matrix &reali, const Matrix &imagi)
\r
168 width_=0; height_=0;
\r
175 void FFT2::construct_default()
\r
177 width_=0; height_=0;
\r
178 img_spc=0; frq_spc=0;
\r
180 void FFT2::release()
\r
182 if(img_spc!=0) { free(img_spc); img_spc = 0; }
\r
183 if(frq_spc!=0) { free(frq_spc); frq_spc = 0; }
\r
184 width_=0; height_=0;
\r
186 void FFT2::set(int wid, int hei)
\r
188 if(img_spc==0 || frq_spc==0 || (width_*height_ != wid*hei) ) {
\r
190 img_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei);
\r
191 frq_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei);
\r
200 void FFT2::set(const Image &source)
\r
202 set(source.getWidth(), source.getHeight());
\r
203 for(int y=0; y<height_; y++) {
\r
204 for(int x=0; x<width_; x++) {
\r
205 img_spc[x + width_*y][REAL] = source.getPix(x, y).getR();
\r
206 img_spc[x + width_*y][IMAGINARY] = 0.0;
\r
210 void FFT2::set(const Matrix &source)
\r
212 set(source.getCols(), source.getRows());
\r
213 for(int y=0; y<height_; y++) {
\r
214 for(int x=0; x<width_; x++) {
\r
215 img_spc[x + width_*y][REAL] = source(y+1, x+1);
\r
216 img_spc[x + width_*y][IMAGINARY] = 0.0;
\r
220 void FFT2::set(const Matrix &reali, const Matrix &imagi)
\r
222 set(reali.getCols(), reali.getRows());
\r
223 for(int y=0; y<height_; y++) {
\r
224 for(int x=0; x<width_; x++) {
\r
225 img_spc[x + width_*y][REAL] = reali(y+1, x+1);
\r
226 img_spc[x + width_*y][IMAGINARY] = imagi(y+1, x+1);
\r
230 void FFT2::setSpectrum(const Matrix &reali, const Matrix &imagi)
\r
232 set(reali.getCols(), reali.getRows());
\r
233 for(int y=0; y<height_; y++) {
\r
234 for(int x=0; x<width_; x++) {
\r
235 frq_spc[x + width_*y][REAL] = reali(y+1, x+1);
\r
236 frq_spc[x + width_*y][IMAGINARY] = imagi(y+1, x+1);
\r
241 int FFT2::freqX(int x) const
\r
243 return x<0 ? x_zero+x : x;
\r
245 int FFT2::freqY(int y) const
\r
247 return y<0 ? y_zero+y : y;
\r
250 double FFT2::pix(int x, int y, double l)
\r
252 return img_spc[x + width_*y][REAL] = l;
\r
254 double FFT2::pix(int x, int y, const Color &c)
\r
256 return img_spc[x + width_*y][REAL] = c.getR();
\r
258 double FFT2::getPix(int x, int y)
\r
260 return img_spc[x + width_*y][REAL];
\r
263 double FFT2::getDC()
\r
265 return sqrt(frq_spc[0][REAL]*frq_spc[0][REAL]+frq_spc[0][IMAGINARY]*frq_spc[0][IMAGINARY]);
\r
267 double FFT2::setDC(double l)
\r
269 frq_spc[0][REAL] = l;
\r
270 frq_spc[0][IMAGINARY] = 0;
\r
277 fftw_plan plan = fftw_plan_dft_2d(height_, width_, img_spc, frq_spc, FFTW_FORWARD, FFTW_ESTIMATE);
\r
278 fftw_execute(plan);
\r
283 fftw_plan plan = fftw_plan_dft_2d(height_, width_, frq_spc, img_spc, FFTW_BACKWARD, FFTW_ESTIMATE);
\r
284 fftw_execute(plan);
\r
286 void FFT2::normalizeFFT()
\r
288 double num = height_*width_;
\r
289 for (int i=0; i<num; i++) {
\r
290 frq_spc[i][REAL] /= num;
\r
291 frq_spc[i][IMAGINARY] /= num;
\r
294 void FFT2::getImage(Image &target)
\r
298 if( !target.hasInstance() || (target.getWidth()!=width_) || (target.getHeight()!=height_) ) {
\r
300 target.set(width_, height_);
\r
302 for (int y=0; y<height_; y++) {
\r
303 for (int x=0; x<width_; x++) {
\r
304 rl = img_spc[x + width_*y][REAL];
\r
305 im = img_spc[x + width_*y][IMAGINARY];
\r
306 col.set(sqrt(rl*rl+im*im));
\r
307 target.pix(x, y, col);
\r
311 void FFT2::getSpectrum(Image &target, double gamma)
\r
313 const int bottom = height_%2==0 ? top : top+1;
\r
314 const int right = width_%2==0 ? left : left+1;
\r
317 if( !target.hasInstance() || (target.getWidth()!=width_) || (target.getHeight()!=height_) ) {
\r
319 target.set(width_, height_);
\r
322 for (int y=0; y<height_; y++) {
\r
323 v = (y<bottom ? y+top : y-bottom);
\r
324 for (int x=0; x<right; x++) {
\r
326 rl = frq_spc[x + width_*y][REAL];
\r
327 im = frq_spc[x + width_*y][IMAGINARY];
\r
328 col.set(pow( sqrt(rl*rl+im*im), gamma ));
\r
329 target.pix(h, v, col);
\r
331 for (int x=right; x<width_; x++) {
\r
333 rl = frq_spc[x + width_*y][REAL];
\r
334 im = frq_spc[x + width_*y][IMAGINARY];
\r
335 col.set(pow( sqrt(rl*rl+im*im), gamma ));
\r
336 target.pix(h, v, col);
\r
340 void FFT2::getImage(Matrix &target)
\r
344 if( (target.getCols()!=width_) || (target.getRows()!=height_) ) {
\r
346 target.set(height_, width_);
\r
348 for (int y=0; y<height_; y++) {
\r
350 for (int x=0; x<width_; x++) {
\r
351 rl = img_spc[x + width_*y][REAL];
\r
352 im = img_spc[x + width_*y][IMAGINARY];
\r
353 target(v, x+1) = sqrt(rl*rl+im*im);
\r
357 void FFT2::getSpectrum(Matrix &target, double gamma)
\r
359 const int bottom = height_%2==0 ? top : top+1;
\r
360 const int right = width_%2==0 ? left : left+1;
\r
363 if( (target.getCols()!=width_) || (target.getRows()!=height_) ) {
\r
365 target.set(height_, width_);
\r
368 for (int y=0; y<height_; y++) {
\r
369 v = (y<bottom ? y+top : y-bottom) + 1;
\r
370 for (int x=0; x<right; x++) {
\r
372 rl = frq_spc[x + width_*y][REAL];
\r
373 im = frq_spc[x + width_*y][IMAGINARY];
\r
374 target(v, h) = pow( sqrt(rl*rl+im*im), gamma );
\r
376 for (int x=right; x<width_; x++) {
\r
378 rl = frq_spc[x + width_*y][REAL];
\r
379 im = frq_spc[x + width_*y][IMAGINARY];
\r
380 target(v, h) = pow( sqrt(rl*rl+im*im), gamma );
\r
383 /*for (int y=0; y<height_; y++) {
\r
385 for (int x=0; x<width_; x++) {
\r
386 rl = frq_spc[x + width_*y][REAL];
\r
387 im = frq_spc[x + width_*y][IMAGINARY];
\r
389 target(v, x+1) = pow( sqrt(rl*rl+im*im), gamma );
\r
393 void FFT2::getImage(Matrix &reali, Matrix &imagi)
\r
395 if( (reali.getCols()!=width_) || (reali.getRows()!=height_) ) { reali.release(); reali.set(height_, width_); }
\r
396 if( (imagi.getCols()!=width_) || (imagi.getRows()!=height_) ) { imagi.release(); imagi.set(height_, width_); }
\r
397 for (int y=0; y<height_; y++) {
\r
398 for (int x=0; x<width_; x++) {
\r
399 reali(y+1, x+1) = img_spc[x + width_*y][REAL];
\r
400 imagi(y+1, x+1) = img_spc[x + width_*y][IMAGINARY];
\r
404 void FFT2::getSpectrum(Matrix &reali, Matrix &imagi)
\r
406 const int bottom = height_%2==0 ? top : top+1;
\r
407 const int right = width_%2==0 ? left : left+1;
\r
408 if( (reali.getCols()!=width_) || (reali.getRows()!=height_) ) { reali.release(); reali.set(height_, width_); }
\r
409 if( (imagi.getCols()!=width_) || (imagi.getRows()!=height_) ) { imagi.release(); imagi.set(height_, width_); }
\r
411 for (int y=0; y<height_; y++) {
\r
412 v = (y<bottom ? y+top : y-bottom) + 1;
\r
413 for (int x=0; x<width_; x++) {
\r
414 reali(y+1, x+1) = frq_spc[x + width_*y][REAL];
\r
415 imagi(y+1, x+1) = frq_spc[x + width_*y][IMAGINARY];
\r
419 void FFT2::filtering(const Matrix &filter)
\r
421 const int bottom = height_%2==0 ? top : top+1;
\r
422 const int right = width_%2==0 ? left : left+1;
\r
425 //std::cout << width_ << ":" << left << " " << height_ << ":" << top << "*" << bottom << std::endl;
\r
426 for (int y=0; y<height_; y++) {
\r
427 v = (y<bottom ? y+top : y-bottom) + 1;
\r
428 //std::cout << v << ":" << y << " ";
\r
429 if(v<=0 || v>height_) exit(v);
\r
430 for (int x=0; x<right; x++) {
\r
432 //std::cout << h << ":" << x << " ";
\r
433 factor = filter(v, h);
\r
434 frq_spc[x + width_*y][REAL] *= factor;
\r
435 frq_spc[x + width_*y][IMAGINARY] *= factor;
\r
437 for (int x=right; x<width_; x++) {
\r
439 //std::cout << h << ":" << x << " ";
\r
440 factor = filter(v, h);
\r
441 frq_spc[x + width_*y][REAL] *= factor;
\r
442 frq_spc[x + width_*y][IMAGINARY] *= factor;
\r
444 //std::cout << std::endl;
\r
447 void FFT2::filtering(double cutoff_lambda1, double cutoff_lambda2, double slope)
\r
449 Matrix filter = makeFilter(height_, width_, cutoff_lambda1, cutoff_lambda2, slope);
\r
453 double gaussian(const double x, const double sigma) { return exp( -(x*x) / (2.0*sigma*sigma) ); }
\r
454 void FFT2::getSpectrumExec(FFT2 &tmp, Image &result, double gamma)
\r
457 for (int y=0; y<tmp.height_; y++) {
\r
458 yy = (double)(y-tmp.top)/tmp.height_;
\r
459 for (int x=0; x<tmp.width_; x++) {
\r
460 xx = (double)(x-tmp.left)/tmp.width_;
\r
461 tmp.img_spc[x + tmp.width_*y][REAL] = (tmp.img_spc[x + tmp.width_*y][REAL]-.5) * gaussian(sqrt(xx*xx+yy*yy), 0.125) + 0.5;
\r
465 tmp.getSpectrum(result, gamma);
\r
467 void FFT2::getSpectrum(const Image &source, Image &result, double gamma)
\r
470 getSpectrumExec(tmp, result, gamma);
\r
472 void FFT2::getSpectrum(const Matrix &source, Image &result, double gamma)
\r
475 getSpectrumExec(tmp, result, gamma);
\r
477 void FFT2::getRawSpectrumExec(FFT2 &tmp, Image &reali, Image &imagi, double gamma)
\r
480 tmp.getSpectrum(reali, imagi, gamma);
\r
482 void FFT2::getSpectrum(const Image &source, Image &reali, Image &imagi, double gamma)
\r
485 getRawSpectrumExec(tmp, reali, imagi, gamma);
\r
487 void FFT2::getSpectrum(const Matrix &source, Image &reali, Image &imagi, double gamma)
\r
490 getRawSpectrumExec(tmp, reali, imagi, gamma);
\r
493 void FFT2::filterImageExec(FFT2 &tmp, Image &result, const Matrix &carnel)
\r
496 tmp.filtering(carnel);
\r
498 tmp.getImage(result);
\r
500 void FFT2::filterImage(const Image &source, Image &result, const Matrix &carnel)
\r
503 filterImageExec(tmp, result, carnel);
\r
505 void FFT2::filterImage(const Matrix &source, Image &result, const Matrix &carnel)
\r
508 filterImageExec(tmp, result, carnel);
\r
510 void FFT2::filterImage(const Image &source, Image &result, double cutoff_lambda1, double cutoff_lambda2, double slope)
\r
513 Matrix carnel = makeFilter(tmp.height_, tmp.width_, cutoff_lambda1, cutoff_lambda2, slope);
\r
514 filterImageExec(tmp, result, carnel);
\r
516 void FFT2::filterImage(const Matrix &source, Image &result, double cutoff_lambda1, double cutoff_lambda2, double slope)
\r
519 Matrix carnel = makeFilter(tmp.height_, tmp.width_, cutoff_lambda1, cutoff_lambda2, slope);
\r
520 filterImageExec(tmp, result, carnel);
\r
523 Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double slope)
\r
525 Matrix filter(height, width);
\r
527 const double originY = (height)/2+1;
\r
528 const double originX = (width )/2+1;
\r
529 const double cutoffL = cutoff_lambda1;
\r
530 const double cutoffH = cutoff_lambda2;
\r
531 const double half = (cutoffL + cutoffH)/2.0;
\r
532 double modulus = 0.0;
\r
533 double sslope = slope<=0.0 ? 0.0000000000000001 : slope;
\r
536 if(cutoffL<=cutoffH) {
\r
537 for(int y=1; y<=height; y++) {
\r
538 for(int x=1; x<=width; x++) {
\r
539 modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
\r
540 if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope);
\r
541 else if(modulus<=cutoffH) filter(y, x) = 1.0;
\r
542 else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope);
\r
543 //if(filter(y,x)<0)filter(y,x)=0;
\r
547 for(int y=1; y<=height; y++) {
\r
548 for(int x=1; x<=width; x++) {
\r
549 modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
\r
550 if(modulus<cutoffH) value = 1.0;
\r
551 else if(modulus<half) value = (pow(1.0/(modulus-cutoffH+1.0), 1.0/sslope));
\r
552 else if(modulus<cutoffL) value = (pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope));
\r
554 filter(y, x) = value;
\r
555 //if(filter(y,x)<0)filter(y,x)=0;
\r
559 filter(originY, originX) = 1.0;
\r
562 Matrix FFT2::makeFilterSector(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double theta1, double theta2, double half_power_width, double half_power_width_ori)
\r
564 Matrix filter(height, width);
\r
566 const double originY = (height)/2+1;
\r
567 const double originX = (width )/2+1;
\r
568 const double cutoffL = cutoff_lambda1;
\r
569 const double cutoffH = cutoff_lambda2;
\r
570 const double half = (cutoffL + cutoffH)/2;
\r
571 double modulus = 0.0;
\r
572 double sslope = half_power_width<=0.0 ? 0.0000000001 : half_power_width;
\r
573 double oslope = half_power_width<=0.0 ? 0.0000000001 : half_power_width_ori;
\r
574 const double cutoffOL = 0, cutoffOH = Math::mod(theta2-theta1, PI), halfO = Math::mod(cutoffOH + (PI-cutoffOH)/2, PI);
\r
575 double yy, xx, phy, sec_factor;
\r
578 if(cutoffL<=cutoffH) {
\r
579 for(int y=1; y<=height; y++) {
\r
581 for(int x=1; x<=width; x++) {
\r
583 phy = Math::mod(atan2(yy, xx) - theta1, PI);
\r
584 if(phy<cutoffOH) sec_factor = 1.0;
\r
585 else if(phy<halfO) sec_factor = pow(1.0/(-cutoffOH+phy+1.0), 1.0/oslope);
\r
586 else sec_factor = pow(1.0/(PI-phy+1.0), 1.0/oslope);
\r
587 //if(isinf(sec_factor)) { sec_factor = 1.0; }
\r
588 //if(isnan(sec_factor)) { sec_factor = 0.0; }
\r
589 modulus = sqrt(yy*yy + xx*xx);
\r
590 if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope) * sec_factor;
\r
591 else if(modulus<cutoffH) filter(y, x) = 1.0 * sec_factor;
\r
592 else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope) * sec_factor;
\r
593 //if(filter(y,x)<0)filter(y,x)=0;
\r
597 for(int y=1; y<=height; y++) {
\r
599 for(int x=1; x<=width; x++) {
\r
601 phy = Math::mod(atan2(yy, xx) - theta1, PI);
\r
602 if(phy<cutoffOH) sec_factor = 1.0;
\r
603 else if(phy<halfO) sec_factor = pow(1.0/(-cutoffOH+phy+1.0), 1.0/oslope);
\r
604 else sec_factor = pow(1.0/(PI-phy+1.0), 1.0/oslope);
\r
606 modulus = sqrt(yy*yy + xx*xx);
\r
607 if(modulus<cutoffH) filter(y, x) = 1.0 * sec_factor;
\r
608 else if(modulus<half) filter(y, x) = (pow(1.0/(modulus-cutoffH+1.0), 1.0/sslope)) * sec_factor;
\r
609 else if(modulus<cutoffL) filter(y, x) = (pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope)) * sec_factor;
\r
610 else filter(y, x) = 1.0 * sec_factor;
\r
611 //if(filter(y,x)<0)filter(y,x)=0;
\r
616 filter(originY, originX) = 1.0;
\r
620 Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double cutoff_lambda3, double cutoff_lambda4, double theta1, double theta2, double slope)
\r
622 Matrix filter(height, width);
\r
624 const double originY = (height)/2+1;
\r
625 const double originX = (width )/2+1;
\r
626 double modulus = 0.0, phyL = 0.0, phyH = 0.0;
\r
627 double sslope = slope<=0.0 ? 0.0000000001 : slope;
\r
628 double cutoffL, cutoffH, half;
\r
631 if(cutoff_lambda1<=cutoff_lambda2) {
\r
632 for(int y=1; y<=height; y++) {
\r
633 for(int x=1; x<=width; x++) {
\r
634 phyL = atan2(y-originY, x-originX);
\r
635 phyH = Math::mod(phyL - theta2, 2*PI);
\r
636 phyL = Math::mod(phyL - theta1, 2*PI);
\r
637 modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
\r
638 cutoffL = cutoff_lambda1*cutoff_lambda3 / p.set(cutoff_lambda3*cos(phyL), cutoff_lambda1*sin(phyL)).length();
\r
639 cutoffH = cutoff_lambda2*cutoff_lambda4 / p.set(cutoff_lambda4*cos(phyH), cutoff_lambda2*sin(phyH)).length();
\r
640 if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope);
\r
641 else if(modulus<cutoffH) filter(y, x) = 1.0;
\r
642 else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope);
\r
643 //if(filter(y,x)<0)filter(y,x)=0;
\r
647 for(int y=1; y<=height; y++) {
\r
648 for(int x=1; x<=width; x++) {
\r
649 phyL = atan2(y-originY, x-originX);
\r
650 phyH = Math::mod(phyL - theta2, 2*PI);
\r
651 phyL = Math::mod(phyL - theta1, 2*PI);
\r
652 modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
\r
653 cutoffL = cutoff_lambda1*cutoff_lambda3 / p.set(cutoff_lambda3*cos(phyL), cutoff_lambda1*sin(phyL)).length();
\r
654 cutoffH = cutoff_lambda2*cutoff_lambda4 / p.set(cutoff_lambda4*cos(phyH), cutoff_lambda2*sin(phyH)).length();
\r
655 half = (cutoffL + cutoffH)/2;
\r
656 if(modulus<cutoffH) filter(y, x) = 1.0;
\r
657 else if(modulus<half) filter(y, x) = (pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope));
\r
658 else if(modulus<cutoffL) filter(y, x) = (pow(1.0/(+modulus-cutoffL+1.0), 1.0/sslope));
\r
659 else filter(y, x) = 1.0;
\r
660 //if(filter(y,x)<0)filter(y,x)=0;
\r
664 filter(originY, originX) = 1.0;
\r
669 void FFT2::copyBoxSpectrum(FFT2 &source, int s_left, int s_top, FFT2 &target, int t_left, int t_top, int width, int height)
\r
671 int t_width = target.width_, s_width = source.width_;
\r
672 int sv, sh, tv, th;
\r
674 //std::cout << width_ << ":" << left << " " << height_ << ":" << top << "*" << bottom << std::endl;
\r
675 for (int y=0; y<height; y++) {
\r
678 if(sv<0 || sv>=source.height_) throw Exception("FFT2::copyBoxSpectrum: sv :");
\r
679 if(tv<0 || tv>=target.height_) throw Exception("FFT2::copyBoxSpectrum: tv :");
\r
680 for (int x=0; x<width; x++) {
\r
681 //std::cout << h << ":" << x << " ";
\r
682 target.frq_spc[(t_left+x) + t_width*tv][REAL] = source.frq_spc[(s_left+x) + s_width*sv][REAL];
\r
683 target.frq_spc[(t_left+x) + t_width*tv][IMAGINARY] = source.frq_spc[(s_left+x) + s_width*sv][IMAGINARY];
\r
685 //std::cout << std::endl;
\r
690 void FFT2::resizeImage(const Image &source, Image &result, int new_width, int new_height)
\r
694 Matrix source_buf[4];
\r
695 Matrix result_buf[4];
\r
697 //src.getSpectrum(buf_r, buf_i);
\r
699 const int s_width = source.getWidth();
\r
700 const int s_height = source.getHeight();
\r
701 const int cp_width = Math::min(source.getWidth(), new_width);
\r
702 const int cp_height = Math::min(source.getHeight(), new_height);
\r
704 int s_right_b = 0, s_left_b, s_bottom_b = 0, s_top_b;
\r
705 int t_right_b = 0, t_left_b, t_bottom_b = 0, t_top_b;
\r
706 int right_length, left_length, top_length, bottom_length;
\r
709 if(s_width>=new_width) {
\r
710 right_length = cp_width - originFor(cp_width);
\r
711 left_length = originFor(cp_width);
\r
714 right_length = cp_width - originFor(cp_width);
\r
715 left_length = originFor(cp_width);
\r
717 s_left_b = s_width - left_length;
\r
718 t_left_b = new_width - left_length;
\r
720 if(s_width>=new_width) {
\r
721 bottom_length = cp_width - originFor(cp_width);
\r
722 top_length = originFor(cp_width);
\r
725 bottom_length = cp_height - originFor(cp_height);
\r
726 top_length = originFor(cp_height);
\r
728 s_top_b = s_height - top_length;
\r
729 t_top_b = new_height - top_length;
\r
732 switch (source.getComponentKind()) {
\r
734 source.to(source_buf[0]);
\r
738 source.to(source_buf[0], source_buf[1], source_buf[2]);
\r
742 source.to(source_buf[0], source_buf[1], source_buf[2], source_buf[3]);
\r
746 throw Exception("FFT2::resizeImage");
\r
748 rst.set(new_width, new_height);
\r
749 for(int i=0; i<itr; i++) {
\r
750 src.set(source_buf[i]);
\r
752 std::cout << src.width_ << " " << src.height_ << std::endl;
\r
753 copyBoxSpectrum(src, s_right_b, s_top_b, rst, t_right_b, t_top_b, right_length, top_length);
\r
754 copyBoxSpectrum(src, s_left_b, s_top_b, rst, t_left_b, t_top_b, left_length, top_length);
\r
755 copyBoxSpectrum(src, s_right_b, s_bottom_b, rst, t_right_b, t_bottom_b, right_length, bottom_length);
\r
756 copyBoxSpectrum(src, s_left_b, s_bottom_b, rst, t_left_b, t_bottom_b, left_length, bottom_length);
\r
758 rst.getImage(result_buf[i]);
\r
759 result_buf[i] = result_buf[i];
\r
761 switch (source.getComponentKind()) {
\r
763 result.from(result_buf[0]);
\r
766 result.from(result_buf[0], result_buf[1], result_buf[2]);
\r
769 result.from(result_buf[0], result_buf[1], result_buf[2], result_buf[3]);
\r
772 throw Exception("FFT2::resizeImage");
\r
777 int FFT2::originFor(int size)
\r
783 Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double slope)
\r
785 Matrix filter(height, width);
\r
787 double originY = (height-1)/2+1;
\r
788 double originX = (width -1)/2+1;
\r
789 double modulus = 0.0;
\r
790 double cutoffL = Math::log2(cutoff_lambda1);
\r
791 double cutoffH = Math::log2(cutoff_lambda2);
\r
793 if(cutoffL<cutoffH) {
\r
794 for(int y=1; y<=height; y++) {
\r
795 for(int x=1; x<=width; x++) {
\r
796 modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
\r
797 if(modulus!=0) modulus = Math::log2(modulus);
\r
798 if(modulus<cutoffL) filter(y, x) = 1.0-slope*(cutoffL-modulus);
\r
799 else if(modulus>cutoffH) filter(y, x) = 1.0-slope*(-cutoffH+modulus);
\r
801 filter(y, x) = 1.0;
\r
802 //filter(y, x) = sigmoid(-modulus, -frequency, 1);
\r
803 if(filter(y,x)<0)filter(y,x)=0;
\r
807 for(int y=1; y<=height; y++) {
\r
808 for(int x=1; x<=width; x++) {
\r
809 modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
\r
810 if(modulus!=0) modulus = Math::log2(modulus);
\r
811 if(modulus<cutoffL) filter(y, x) = slope*(cutoffL-modulus);
\r
812 else if(modulus>cutoffH) filter(y, x) = slope*(-cutoffH+modulus);
\r
814 filter(y, x) = 0.0;
\r
815 //filter(y, x) = sigmoid(-modulus, -frequency, 1);
\r
816 if(filter(y,x)<0)filter(y,x)=0;
\r
820 filter(originY, originX) = 1.0;
\r
826 void FFT2::makeNoise()
\r
828 double num = height_*width_;
\r
829 for (int i=0; i<num; i++) {
\r
830 frq_spc[i][REAL] = random(0.0,1.0);
\r
831 frq_spc[i][IMAGINARY] = 0;
\r
841 FFTW3D::FFTW3D(int wid, int hei, int dep)
\r
843 set(wid, hei, dep);
\r
845 FFTW3D::FFTW3D(const Image *source, int framsize)
\r
847 set(source, framsize);
\r
849 FFTW3D::FFTW3D(const Matrix *source, int framsize)
\r
851 set(source, framsize);
\r
855 if(img_spc==0) free(img_spc);
\r
856 if(frq_spc==0) free(frq_spc);
\r
858 void FFTW3D::set(int wid, int hei, int dep)
\r
863 image_size_ = width_*height_;
\r
864 img_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei * dep);
\r
865 frq_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei * dep);
\r
873 void FFTW3D::set(const Image *source, int framsize)
\r
875 set(source[0].getWidth(), source[0].getHeight(), framsize);
\r
876 for(int z=0; z<depth_; z++) {
\r
877 for(int y=0; y<height_; y++) {
\r
878 for(int x=0; x<width_; x++) {
\r
879 img_spc[x + width_*y + image_size_*z][REAL] = source[z].getPix(x, y).getR();
\r
880 img_spc[x + width_*y + image_size_*z][IMAGINARY] = 0.0;
\r
885 void FFTW3D::set(const Matrix *source, int framsize)
\r
887 set(source[0].getCols(), source[0].getRows(), framsize);
\r
888 for(int z=0; z<depth_; z++) {
\r
889 for(int y=0; y<height_; y++) {
\r
890 for(int x=0; x<width_; x++) {
\r
891 img_spc[x + width_*y + image_size_*z][REAL] = source[z](y+1, x+1);
\r
892 img_spc[x + width_*y + image_size_*z][IMAGINARY] = 0.0;
\r
898 int FFTW3D::freqX(int x) const
\r
900 return 0;//imgs[0].freqX(x);
\r
902 int FFTW3D::freqY(int y) const
\r
904 return 0;//imgs[0].freqY(y);
\r
906 int FFTW3D::freqT(int t) const
\r
908 return 0;//t<0 ? t_zero+t : t;
\r
910 double FFTW3D::pix(int x, int y, int z, double l)
\r
912 return img_spc[x + width_*y + image_size_*z][REAL] = l;
\r
914 double FFTW3D::pix(int x, int y, int z, const Color &c)
\r
916 return img_spc[x + width_*y + image_size_*z][REAL] = c.getR();
\r
918 double FFTW3D::getPix(int x, int y, int z)
\r
920 return img_spc[x + width_*y + image_size_*z][REAL];
\r
923 double FFTW3D::getDC()
\r
925 return sqrt(frq_spc[0][REAL]*frq_spc[0][REAL]+frq_spc[0][IMAGINARY]*frq_spc[0][IMAGINARY]);
\r
927 double FFTW3D::setDC(double l)
\r
929 frq_spc[0][REAL] = l;
\r
930 frq_spc[0][IMAGINARY] = 0;
\r
937 fftw_plan plan = fftw_plan_dft_3d(depth_, height_, width_, img_spc, frq_spc, FFTW_FORWARD, FFTW_ESTIMATE);
\r
938 fftw_execute(plan);
\r
941 void FFTW3D::ifft()
\r
943 fftw_plan plan = fftw_plan_dft_3d(depth_, height_, width_, frq_spc, img_spc, FFTW_BACKWARD, FFTW_ESTIMATE);
\r
944 fftw_execute(plan);
\r
946 void FFTW3D::normalizeFFT()
\r
948 double num = depth_*height_*width_;
\r
949 for (int i=0; i<num; i++) {
\r
950 frq_spc[i][REAL] /= num;
\r
951 frq_spc[i][IMAGINARY] /= num;
\r
956 void FFTW3D::drawCloudImage(Drawable &target)
\r
961 for (int z=0; z<depth_; z++) {
\r
963 for (int y=0; y<height_; y++) {
\r
965 for (int x=0; x<width_; x++) {
\r
967 rl = img_spc[x + width_*y + image_size_*z][REAL];
\r
968 //im = img_spc[x + width_*y + image_size_*z][IMAGINARY];
\r
970 // target.pix(p, col);
\r
975 void FFTW3D::drawCloudSpectrum(Point c, double gamma, Drawable &target)
\r
981 for (int z=0; z<depth_; z++) {
\r
982 d = z<front ? z+front : z-front;
\r
984 for (int y=0; y<height_; y++) {
\r
985 v = y<top ? y+top : y-top;
\r
987 for (int x=0; x<left; x++) {
\r
990 rl = frq_spc[x + width_*y + image_size_*z][REAL];
\r
991 im = frq_spc[x + width_*y + image_size_*z][IMAGINARY];
\r
992 col.set(pow( sqrt(rl*rl+im*im), gamma ));
\r
993 // target.pix(p, col);
\r
995 for (int x=left; x<width_; x++) {
\r
998 rl = frq_spc[x + width_*y + image_size_*z][REAL];
\r
999 im = frq_spc[x + width_*y + image_size_*z][IMAGINARY];
\r
1000 col.set(pow( sqrt(rl*rl+im*im), gamma ));
\r
1001 // target.pix(p, col);
\r
1007 } /* <- namespace Psycholops */
\r