ei_kissfft_impl.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Mark Borgerding mark a borgerding net
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 namespace Eigen {
11 
12 namespace internal {
13 
14  // This FFT implementation was derived from kissfft http:sourceforge.net/projects/kissfft
15  // Copyright 2003-2009 Mark Borgerding
16 
17 template <typename _Scalar>
19 {
20  typedef _Scalar Scalar;
21  typedef std::complex<Scalar> Complex;
22  std::vector<Complex> m_twiddles;
23  std::vector<int> m_stageRadix;
24  std::vector<int> m_stageRemainder;
25  std::vector<Complex> m_scratchBuf;
26  bool m_inverse;
27 
28  inline void make_twiddles(int nfft, bool inverse)
29  {
30  using numext::sin;
31  using numext::cos;
33  m_twiddles.resize(nfft);
34  double phinc = 0.25 * double(EIGEN_PI) / nfft;
35  Scalar flip = inverse ? Scalar(1) : Scalar(-1);
36  m_twiddles[0] = Complex(Scalar(1), Scalar(0));
37  if ((nfft&1)==0)
38  m_twiddles[nfft/2] = Complex(Scalar(-1), Scalar(0));
39  int i=1;
40  for (;i*8<nfft;++i)
41  {
42  Scalar c = Scalar(cos(i*8*phinc));
43  Scalar s = Scalar(sin(i*8*phinc));
44  m_twiddles[i] = Complex(c, s*flip);
45  m_twiddles[nfft-i] = Complex(c, -s*flip);
46  }
47  for (;i*4<nfft;++i)
48  {
49  Scalar c = Scalar(cos((2*nfft-8*i)*phinc));
50  Scalar s = Scalar(sin((2*nfft-8*i)*phinc));
51  m_twiddles[i] = Complex(s, c*flip);
52  m_twiddles[nfft-i] = Complex(s, -c*flip);
53  }
54  for (;i*8<3*nfft;++i)
55  {
56  Scalar c = Scalar(cos((8*i-2*nfft)*phinc));
57  Scalar s = Scalar(sin((8*i-2*nfft)*phinc));
58  m_twiddles[i] = Complex(-s, c*flip);
59  m_twiddles[nfft-i] = Complex(-s, -c*flip);
60  }
61  for (;i*2<nfft;++i)
62  {
63  Scalar c = Scalar(cos((4*nfft-8*i)*phinc));
64  Scalar s = Scalar(sin((4*nfft-8*i)*phinc));
65  m_twiddles[i] = Complex(-c, s*flip);
66  m_twiddles[nfft-i] = Complex(-c, -s*flip);
67  }
68  }
69 
70  void factorize(int nfft)
71  {
72  //start factoring out 4's, then 2's, then 3,5,7,9,...
73  int n= nfft;
74  int p=4;
75  do {
76  while (n % p) {
77  switch (p) {
78  case 4: p = 2; break;
79  case 2: p = 3; break;
80  default: p += 2; break;
81  }
82  if (p*p>n)
83  p=n;// impossible to have a factor > sqrt(n)
84  }
85  n /= p;
86  m_stageRadix.push_back(p);
87  m_stageRemainder.push_back(n);
88  if ( p > 5 )
89  m_scratchBuf.resize(p); // scratchbuf will be needed in bfly_generic
90  }while(n>1);
91  }
92 
93  template <typename _Src>
94  inline
95  void work( int stage,Complex * xout, const _Src * xin, size_t fstride,size_t in_stride)
96  {
97  int p = m_stageRadix[stage];
98  int m = m_stageRemainder[stage];
99  Complex * Fout_beg = xout;
100  Complex * Fout_end = xout + p*m;
101 
102  if (m>1) {
103  do{
104  // recursive call:
105  // DFT of size m*p performed by doing
106  // p instances of smaller DFTs of size m,
107  // each one takes a decimated version of the input
108  work(stage+1, xout , xin, fstride*p,in_stride);
109  xin += fstride*in_stride;
110  }while( (xout += m) != Fout_end );
111  }else{
112  do{
113  *xout = *xin;
114  xin += fstride*in_stride;
115  }while(++xout != Fout_end );
116  }
117  xout=Fout_beg;
118 
119  // recombine the p smaller DFTs
120  switch (p) {
121  case 2: bfly2(xout,fstride,m); break;
122  case 3: bfly3(xout,fstride,m); break;
123  case 4: bfly4(xout,fstride,m); break;
124  case 5: bfly5(xout,fstride,m); break;
125  default: bfly_generic(xout,fstride,m,p); break;
126  }
127  }
128 
129  inline
130  void bfly2( Complex * Fout, const size_t fstride, int m)
131  {
132  for (int k=0;k<m;++k) {
133  Complex t = Fout[m+k] * m_twiddles[k*fstride];
134  Fout[m+k] = Fout[k] - t;
135  Fout[k] += t;
136  }
137  }
138 
139  inline
140  void bfly4( Complex * Fout, const size_t fstride, const size_t m)
141  {
142  Complex scratch[6];
143  int negative_if_inverse = m_inverse * -2 +1;
144  for (size_t k=0;k<m;++k) {
145  scratch[0] = Fout[k+m] * m_twiddles[k*fstride];
146  scratch[1] = Fout[k+2*m] * m_twiddles[k*fstride*2];
147  scratch[2] = Fout[k+3*m] * m_twiddles[k*fstride*3];
148  scratch[5] = Fout[k] - scratch[1];
149 
150  Fout[k] += scratch[1];
151  scratch[3] = scratch[0] + scratch[2];
152  scratch[4] = scratch[0] - scratch[2];
153  scratch[4] = Complex( scratch[4].imag()*negative_if_inverse , -scratch[4].real()* negative_if_inverse );
154 
155  Fout[k+2*m] = Fout[k] - scratch[3];
156  Fout[k] += scratch[3];
157  Fout[k+m] = scratch[5] + scratch[4];
158  Fout[k+3*m] = scratch[5] - scratch[4];
159  }
160  }
161 
162  inline
163  void bfly3( Complex * Fout, const size_t fstride, const size_t m)
164  {
165  size_t k=m;
166  const size_t m2 = 2*m;
167  Complex *tw1,*tw2;
168  Complex scratch[5];
169  Complex epi3;
170  epi3 = m_twiddles[fstride*m];
171 
172  tw1=tw2=&m_twiddles[0];
173 
174  do{
175  scratch[1]=Fout[m] * *tw1;
176  scratch[2]=Fout[m2] * *tw2;
177 
178  scratch[3]=scratch[1]+scratch[2];
179  scratch[0]=scratch[1]-scratch[2];
180  tw1 += fstride;
181  tw2 += fstride*2;
182  Fout[m] = Complex( Fout->real() - Scalar(.5)*scratch[3].real() , Fout->imag() - Scalar(.5)*scratch[3].imag() );
183  scratch[0] *= epi3.imag();
184  *Fout += scratch[3];
185  Fout[m2] = Complex( Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() );
186  Fout[m] += Complex( -scratch[0].imag(),scratch[0].real() );
187  ++Fout;
188  }while(--k);
189  }
190 
191  inline
192  void bfly5( Complex * Fout, const size_t fstride, const size_t m)
193  {
194  Complex *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
195  size_t u;
196  Complex scratch[13];
197  Complex * twiddles = &m_twiddles[0];
198  Complex *tw;
199  Complex ya,yb;
200  ya = twiddles[fstride*m];
201  yb = twiddles[fstride*2*m];
202 
203  Fout0=Fout;
204  Fout1=Fout0+m;
205  Fout2=Fout0+2*m;
206  Fout3=Fout0+3*m;
207  Fout4=Fout0+4*m;
208 
209  tw=twiddles;
210  for ( u=0; u<m; ++u ) {
211  scratch[0] = *Fout0;
212 
213  scratch[1] = *Fout1 * tw[u*fstride];
214  scratch[2] = *Fout2 * tw[2*u*fstride];
215  scratch[3] = *Fout3 * tw[3*u*fstride];
216  scratch[4] = *Fout4 * tw[4*u*fstride];
217 
218  scratch[7] = scratch[1] + scratch[4];
219  scratch[10] = scratch[1] - scratch[4];
220  scratch[8] = scratch[2] + scratch[3];
221  scratch[9] = scratch[2] - scratch[3];
222 
223  *Fout0 += scratch[7];
224  *Fout0 += scratch[8];
225 
226  scratch[5] = scratch[0] + Complex(
227  (scratch[7].real()*ya.real() ) + (scratch[8].real() *yb.real() ),
228  (scratch[7].imag()*ya.real()) + (scratch[8].imag()*yb.real())
229  );
230 
231  scratch[6] = Complex(
232  (scratch[10].imag()*ya.imag()) + (scratch[9].imag()*yb.imag()),
233  -(scratch[10].real()*ya.imag()) - (scratch[9].real()*yb.imag())
234  );
235 
236  *Fout1 = scratch[5] - scratch[6];
237  *Fout4 = scratch[5] + scratch[6];
238 
239  scratch[11] = scratch[0] +
240  Complex(
241  (scratch[7].real()*yb.real()) + (scratch[8].real()*ya.real()),
242  (scratch[7].imag()*yb.real()) + (scratch[8].imag()*ya.real())
243  );
244 
245  scratch[12] = Complex(
246  -(scratch[10].imag()*yb.imag()) + (scratch[9].imag()*ya.imag()),
247  (scratch[10].real()*yb.imag()) - (scratch[9].real()*ya.imag())
248  );
249 
250  *Fout2=scratch[11]+scratch[12];
251  *Fout3=scratch[11]-scratch[12];
252 
253  ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
254  }
255  }
256 
257  /* perform the butterfly for one stage of a mixed radix FFT */
258  inline
260  Complex * Fout,
261  const size_t fstride,
262  int m,
263  int p
264  )
265  {
266  int u,k,q1,q;
267  Complex * twiddles = &m_twiddles[0];
268  Complex t;
269  int Norig = static_cast<int>(m_twiddles.size());
270  Complex * scratchbuf = &m_scratchBuf[0];
271 
272  for ( u=0; u<m; ++u ) {
273  k=u;
274  for ( q1=0 ; q1<p ; ++q1 ) {
275  scratchbuf[q1] = Fout[ k ];
276  k += m;
277  }
278 
279  k=u;
280  for ( q1=0 ; q1<p ; ++q1 ) {
281  int twidx=0;
282  Fout[ k ] = scratchbuf[0];
283  for (q=1;q<p;++q ) {
284  twidx += static_cast<int>(fstride) * k;
285  if (twidx>=Norig) twidx-=Norig;
286  t=scratchbuf[q] * twiddles[twidx];
287  Fout[ k ] += t;
288  }
289  k += m;
290  }
291  }
292  }
293 };
294 
295 template <typename _Scalar>
297 {
298  typedef _Scalar Scalar;
299  typedef std::complex<Scalar> Complex;
300 
301  void clear()
302  {
303  m_plans.clear();
304  m_realTwiddles.clear();
305  }
306 
307  inline
308  void fwd( Complex * dst,const Complex *src,int nfft)
309  {
310  get_plan(nfft,false).work(0, dst, src, 1,1);
311  }
312 
313  inline
314  void fwd2( Complex * dst,const Complex *src,int n0,int n1)
315  {
320  }
321 
322  inline
323  void inv2( Complex * dst,const Complex *src,int n0,int n1)
324  {
329  }
330 
331  // real-to-complex forward FFT
332  // perform two FFTs of src even and src odd
333  // then twiddle to recombine them into the half-spectrum format
334  // then fill in the conjugate symmetric half
335  inline
336  void fwd( Complex * dst,const Scalar * src,int nfft)
337  {
338  if ( nfft&3 ) {
339  // use generic mode for odd
340  m_tmpBuf1.resize(nfft);
341  get_plan(nfft,false).work(0, &m_tmpBuf1[0], src, 1,1);
342  std::copy(m_tmpBuf1.begin(),m_tmpBuf1.begin()+(nfft>>1)+1,dst );
343  }else{
344  int ncfft = nfft>>1;
345  int ncfft2 = nfft>>2;
346  Complex * rtw = real_twiddles(ncfft2);
347 
348  // use optimized mode for even real
349  fwd( dst, reinterpret_cast<const Complex*> (src), ncfft);
350  Complex dc(dst[0].real() + dst[0].imag());
351  Complex nyquist(dst[0].real() - dst[0].imag());
352  int k;
353  for ( k=1;k <= ncfft2 ; ++k ) {
354  Complex fpk = dst[k];
355  Complex fpnk = conj(dst[ncfft-k]);
356  Complex f1k = fpk + fpnk;
357  Complex f2k = fpk - fpnk;
358  Complex tw= f2k * rtw[k-1];
359  dst[k] = (f1k + tw) * Scalar(.5);
360  dst[ncfft-k] = conj(f1k -tw)*Scalar(.5);
361  }
362  dst[0] = dc;
363  dst[ncfft] = nyquist;
364  }
365  }
366 
367  // inverse complex-to-complex
368  inline
369  void inv(Complex * dst,const Complex *src,int nfft)
370  {
371  get_plan(nfft,true).work(0, dst, src, 1,1);
372  }
373 
374  // half-complex to scalar
375  inline
376  void inv( Scalar * dst,const Complex * src,int nfft)
377  {
378  if (nfft&3) {
379  m_tmpBuf1.resize(nfft);
380  m_tmpBuf2.resize(nfft);
381  std::copy(src,src+(nfft>>1)+1,m_tmpBuf1.begin() );
382  for (int k=1;k<(nfft>>1)+1;++k)
383  m_tmpBuf1[nfft-k] = conj(m_tmpBuf1[k]);
384  inv(&m_tmpBuf2[0],&m_tmpBuf1[0],nfft);
385  for (int k=0;k<nfft;++k)
386  dst[k] = m_tmpBuf2[k].real();
387  }else{
388  // optimized version for multiple of 4
389  int ncfft = nfft>>1;
390  int ncfft2 = nfft>>2;
391  Complex * rtw = real_twiddles(ncfft2);
392  m_tmpBuf1.resize(ncfft);
393  m_tmpBuf1[0] = Complex( src[0].real() + src[ncfft].real(), src[0].real() - src[ncfft].real() );
394  for (int k = 1; k <= ncfft / 2; ++k) {
395  Complex fk = src[k];
396  Complex fnkc = conj(src[ncfft-k]);
397  Complex fek = fk + fnkc;
398  Complex tmp = fk - fnkc;
399  Complex fok = tmp * conj(rtw[k-1]);
400  m_tmpBuf1[k] = fek + fok;
401  m_tmpBuf1[ncfft-k] = conj(fek - fok);
402  }
403  get_plan(ncfft,true).work(0, reinterpret_cast<Complex*>(dst), &m_tmpBuf1[0], 1,1);
404  }
405  }
406 
407  protected:
409  typedef std::map<int,PlanData> PlanMap;
410 
412  std::map<int, std::vector<Complex> > m_realTwiddles;
413  std::vector<Complex> m_tmpBuf1;
414  std::vector<Complex> m_tmpBuf2;
415 
416  inline
417  int PlanKey(int nfft, bool isinverse) const { return (nfft<<1) | int(isinverse); }
418 
419  inline
420  PlanData & get_plan(int nfft, bool inverse)
421  {
422  // TODO look for PlanKey(nfft, ! inverse) and conjugate the twiddles
423  PlanData & pd = m_plans[ PlanKey(nfft,inverse) ];
424  if ( pd.m_twiddles.size() == 0 ) {
425  pd.make_twiddles(nfft,inverse);
426  pd.factorize(nfft);
427  }
428  return pd;
429  }
430 
431  inline
432  Complex * real_twiddles(int ncfft2)
433  {
434  using std::acos;
435  std::vector<Complex> & twidref = m_realTwiddles[ncfft2];// creates new if not there
436  if ( (int)twidref.size() != ncfft2 ) {
437  twidref.resize(ncfft2);
438  int ncfft= ncfft2<<1;
439  Scalar pi = acos( Scalar(-1) );
440  for (int k=1;k<=ncfft2;++k)
441  twidref[k-1] = exp( Complex(0,-pi * (Scalar(k) / ncfft + Scalar(.5)) ) );
442  }
443  return &twidref[0];
444  }
445 };
446 
447 } // end namespace internal
448 
449 } // end namespace Eigen
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
Eigen::conj
const AutoDiffScalar< DerType > & conj(const AutoDiffScalar< DerType > &x)
Definition: AutoDiffScalar.h:574
Eigen::internal::kissfft_impl::m_tmpBuf2
std::vector< Complex > m_tmpBuf2
Definition: ei_kissfft_impl.h:414
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::internal::kissfft_impl::PlanMap
std::map< int, PlanData > PlanMap
Definition: ei_kissfft_impl.h:409
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
EIGEN_PI
#define EIGEN_PI
Definition: Eigen/src/Core/MathFunctions.h:16
benchmark.n1
n1
Definition: benchmark.py:79
Eigen::internal::kiss_cpx_fft
Definition: ei_kissfft_impl.h:18
s
RealScalar s
Definition: level1_cplx_impl.h:126
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Eigen::internal::kissfft_impl::fwd
void fwd(Complex *dst, const Complex *src, int nfft)
Definition: ei_kissfft_impl.h:308
Eigen::internal::kiss_cpx_fft::m_scratchBuf
std::vector< Complex > m_scratchBuf
Definition: ei_kissfft_impl.h:25
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Eigen::internal::kissfft_impl::m_plans
PlanMap m_plans
Definition: ei_kissfft_impl.h:411
Eigen::internal::kissfft_impl::inv
void inv(Scalar *dst, const Complex *src, int nfft)
Definition: ei_kissfft_impl.h:376
Eigen::internal::kiss_cpx_fft::m_stageRemainder
std::vector< int > m_stageRemainder
Definition: ei_kissfft_impl.h:24
Eigen::internal::kiss_cpx_fft::factorize
void factorize(int nfft)
Definition: ei_kissfft_impl.h:70
Eigen::internal::kiss_cpx_fft::m_stageRadix
std::vector< int > m_stageRadix
Definition: ei_kissfft_impl.h:23
Eigen::internal::kiss_cpx_fft::Scalar
_Scalar Scalar
Definition: ei_kissfft_impl.h:20
Eigen::internal::kissfft_impl::fwd2
void fwd2(Complex *dst, const Complex *src, int n0, int n1)
Definition: ei_kissfft_impl.h:314
Eigen::internal::kiss_cpx_fft::bfly2
void bfly2(Complex *Fout, const size_t fstride, int m)
Definition: ei_kissfft_impl.h:130
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
Eigen::internal::kissfft_impl::Complex
std::complex< Scalar > Complex
Definition: ei_kissfft_impl.h:299
ceres::acos
Jet< T, N > acos(const Jet< T, N > &f)
Definition: jet.h:432
Eigen::internal::kissfft_impl::inv
void inv(Complex *dst, const Complex *src, int nfft)
Definition: ei_kissfft_impl.h:369
Eigen::internal::kissfft_impl::PlanData
kiss_cpx_fft< Scalar > PlanData
Definition: ei_kissfft_impl.h:408
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
n
int n
Definition: BiCGSTAB_simple.cpp:1
Eigen::internal::kissfft_impl::PlanKey
int PlanKey(int nfft, bool isinverse) const
Definition: ei_kissfft_impl.h:417
m2
MatrixType m2(n_dims)
Eigen::internal::kiss_cpx_fft::m_inverse
bool m_inverse
Definition: ei_kissfft_impl.h:26
EIGEN_UNUSED_VARIABLE
#define EIGEN_UNUSED_VARIABLE(var)
Definition: Macros.h:1076
Eigen::internal::kiss_cpx_fft::Complex
std::complex< Scalar > Complex
Definition: ei_kissfft_impl.h:21
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
Eigen::internal::kiss_cpx_fft::bfly_generic
void bfly_generic(Complex *Fout, const size_t fstride, int m, int p)
Definition: ei_kissfft_impl.h:259
Eigen::internal::kiss_cpx_fft::make_twiddles
void make_twiddles(int nfft, bool inverse)
Definition: ei_kissfft_impl.h:28
Eigen::internal::kiss_cpx_fft::bfly5
void bfly5(Complex *Fout, const size_t fstride, const size_t m)
Definition: ei_kissfft_impl.h:192
Eigen::internal::kissfft_impl::m_realTwiddles
std::map< int, std::vector< Complex > > m_realTwiddles
Definition: ei_kissfft_impl.h:412
Eigen::internal::kissfft_impl::real_twiddles
Complex * real_twiddles(int ncfft2)
Definition: ei_kissfft_impl.h:432
Eigen::internal::kiss_cpx_fft::bfly4
void bfly4(Complex *Fout, const size_t fstride, const size_t m)
Definition: ei_kissfft_impl.h:140
Eigen::internal::kiss_cpx_fft::bfly3
void bfly3(Complex *Fout, const size_t fstride, const size_t m)
Definition: ei_kissfft_impl.h:163
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
Eigen::internal::kissfft_impl
Definition: ei_kissfft_impl.h:296
Eigen::internal::kiss_cpx_fft::m_twiddles
std::vector< Complex > m_twiddles
Definition: ei_kissfft_impl.h:22
Eigen::internal::kissfft_impl::inv2
void inv2(Complex *dst, const Complex *src, int n0, int n1)
Definition: ei_kissfft_impl.h:323
Eigen::real
const AutoDiffScalar< DerType > & real(const AutoDiffScalar< DerType > &x)
Definition: AutoDiffScalar.h:576
Eigen::internal::kissfft_impl::Scalar
_Scalar Scalar
Definition: ei_kissfft_impl.h:298
Eigen::imag
DerType::Scalar imag(const AutoDiffScalar< DerType > &)
Definition: AutoDiffScalar.h:578
p
float * p
Definition: Tutorial_Map_using.cpp:9
Eigen::internal::kissfft_impl::clear
void clear()
Definition: ei_kissfft_impl.h:301
Eigen::numext::sin
EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T sin(const T &x)
Definition: Eigen/src/Core/MathFunctions.h:1621
internal
Definition: BandTriangularSolver.h:13
Eigen::internal::kissfft_impl::m_tmpBuf1
std::vector< Complex > m_tmpBuf1
Definition: ei_kissfft_impl.h:413
Eigen::internal::kissfft_impl::get_plan
PlanData & get_plan(int nfft, bool inverse)
Definition: ei_kissfft_impl.h:420
Eigen::internal::kissfft_impl::fwd
void fwd(Complex *dst, const Scalar *src, int nfft)
Definition: ei_kissfft_impl.h:336
align_3::t
Point2 t(10, 10)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Eigen::numext::cos
EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cos(const T &x)
Definition: Eigen/src/Core/MathFunctions.h:1602
Eigen::internal::kiss_cpx_fft::work
void work(int stage, Complex *xout, const _Src *xin, size_t fstride, size_t in_stride)
Definition: ei_kissfft_impl.h:95


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:28