angle_shift.cpp
Go to the documentation of this file.
00001 
00002 #include <iostream>
00003 #include <gsl/gsl_errno.h>
00004 #include <gsl/gsl_fft_real.h>
00005 #include <gsl/gsl_fft_halfcomplex.h>
00006 #include <gsl/gsl_fft_complex.h>
00007 
00008 #include "pm_fourier/angle_shift.h"
00009 
00010 using std::vector;
00011 
00012 vector<double> fft2(const vector<double> &data);
00013 
00014 int getAngleShiftFFT(const vector<double> &fft1, const vector<double> &ftd2, double &maxVal) 
00015 {
00016         gsl_complex_packed_array cpl = new double[fft1.size()];
00017 
00018 
00019         if (fft1.size() > ftd2.size())
00020   {
00021                 std::cerr << __FUNCTION__ << "fft1 and fft(scan) have bad lengths!: fft1.size=" << fft1.size() << ", fft2.size=" << ftd2.size() << "!\n";
00022                 return 0;
00023         }
00024 
00025         for (size_t i = 0; i < fft1.size() / 2; i++)
00026   {
00027                 cpl[2 * i] = fft1[2 * i] * ftd2[2 * i] + fft1[2 * i + 1] * ftd2[2 * i + 1];
00028                 cpl[2*i+1] = fft1[2 * i] * ftd2[2 * i + 1] - fft1[2 * i + 1] * ftd2[2 * i];
00029         }
00030 
00031 
00032         gsl_fft_complex_workspace *work = 
00033                 gsl_fft_complex_workspace_alloc(fft1.size() / 2);
00034 
00035         gsl_fft_complex_wavetable *wave = 
00036                 gsl_fft_complex_wavetable_alloc(fft1.size() / 2);
00037         gsl_fft_complex_inverse(cpl, 1, fft1.size() / 2, wave, work);
00038 
00039         gsl_fft_complex_wavetable_free(wave);
00040         gsl_fft_complex_workspace_free(work);
00041 
00042         double d;
00043         double maxd = -1;
00044         int maxdi = -1;
00045         for(size_t i = 0; i < fft1.size() / 2; i++)
00046   {
00047                 d = cpl[2 * i] * cpl[2 * i] + cpl[2 * i + 1] * cpl[2 * i + 1];
00048                 if (d > maxd || i == 0)
00049     {
00050                         maxdi = i;
00051                         maxd = d;
00052                 }
00053         }
00054         delete [] cpl;
00055         maxVal = maxd;
00056 
00057         return maxdi;
00058 }
00059 
00060 int getAngleShiftFFT(const vector<double> &fft1, const vector<double> &ftd2) 
00061 {
00062         double maxVal;
00063         int res = getAngleShiftFFT(fft1, ftd2, maxVal);
00064         return res;     
00065 }
00066 
00067 
00069 vector<double> fft2(const vector<double> &data)
00070 {
00071         double *d = new double[data.size()];
00072 
00073         gsl_fft_real_wavetable *real;
00074         gsl_fft_real_workspace *work;
00075 
00076         for(size_t i = 0; i < data.size(); i++)
00077   {
00078                 d[i] = data[i];
00079         }
00080 
00081         work = gsl_fft_real_workspace_alloc(data.size());
00082         real = gsl_fft_real_wavetable_alloc(data.size());
00083 
00084         gsl_fft_real_transform(d, 1, data.size(), real, work);
00085         gsl_fft_real_wavetable_free(real);
00086 
00087         gsl_complex_packed_array cpl = new double[data.size() * 2];
00088 
00089         gsl_fft_halfcomplex_unpack(d, cpl, 1, data.size());
00090 
00091         // jen pro kresleni
00092         
00093         gsl_fft_halfcomplex_wavetable *hc = 
00094                 gsl_fft_halfcomplex_wavetable_alloc(data.size());
00095         for(size_t i = 100; i < data.size(); i++)
00096   {
00097                 d[i] = 0;
00098         }
00099         gsl_fft_halfcomplex_inverse(d, 1, data.size(), hc, work);
00100         gsl_fft_halfcomplex_wavetable_free(hc);
00101 
00102         
00103   //    std::ofstream ofs("scanifft.dat");
00104   //    ofs << "#ifft from fft2\n";
00105   //    for(int i=0;i<data.size();i++)
00106   //            ofs << d[i] << "\n";
00107   //    ofs.close();
00108 
00109         vector<double> tmpd;
00110         for (size_t i = 0; i < data.size(); i++)
00111   {
00112                 tmpd.push_back(d[i]);
00113         }
00114   // saveScan("fft1.dat",tmpd,maxScanPhi);
00115         
00116 
00117         vector<double> result;
00118         result.reserve(data.size());
00119         
00120         for(size_t i = 0; i < data.size() * 2; i++)
00121   {
00122                 result.push_back(cpl[i]);
00123         }
00124 
00125         delete [] cpl;
00126         delete [] d;
00127 
00128         gsl_fft_real_workspace_free(work);
00129   // gsl_fft_real_wavetable_free(real);
00130         return result;
00131 }
00132 
00133 


place_matcher_fourier
Author(s): Gaël Ecorchard , Karel Košnar , Vojtěch Vonásek
autogenerated on Sat Jun 8 2019 19:53:01