]> Creatis software - FrontAlgorithms.git/blob - appli/CTArteries/FourierSeries.cxx
c7bf0799e8bbb82dd8f45472a16e8550752a8b03
[FrontAlgorithms.git] / appli / CTArteries / FourierSeries.cxx
1 #include <cmath>
2 #include <limits>
3 #include <map>
4
5 #include <vnl/vnl_math.h>
6 #include "FourierSeries.h"
7
8 // -------------------------------------------------------------------------
9 template< class S, unsigned int D >
10 FourierSeries< S, D >::
11 FourierSeries( unsigned int q )
12   : m_RealAxis( D - 2 ),
13     m_ImagAxis( D - 1 )
14 {
15   this->resize( ( q << 1 ) + 1, TComplex( S( 0 ), S( 0 ) ) );
16 }
17
18 // -------------------------------------------------------------------------
19 template< class S, unsigned int D >
20 FourierSeries< S, D >::
21 ~FourierSeries( )
22 {
23 }
24
25 // -------------------------------------------------------------------------
26 template< class S, unsigned int D >
27 const unsigned int& FourierSeries< S, D >::
28 GetRealAxis( ) const
29 {
30   return( this->m_RealAxis );
31 }
32
33 // -------------------------------------------------------------------------
34 template< class S, unsigned int D >
35 const unsigned int& FourierSeries< S, D >::
36 GetImagAxis( ) const
37 {
38   return( this->m_ImagAxis );
39 }
40
41 // -------------------------------------------------------------------------
42 template< class S, unsigned int D >
43 void FourierSeries< S, D >::
44 SetRealAxis( const unsigned int& a )
45 {
46   this->m_RealAxis = a;
47 }
48
49 // -------------------------------------------------------------------------
50 template< class S, unsigned int D >
51 void FourierSeries< S, D >::
52 SetImagAxis( const unsigned int& a )
53 {
54   this->m_ImagAxis = a;
55 }
56
57 // -------------------------------------------------------------------------
58 template< class S, unsigned int D >
59 S FourierSeries< S, D >::
60 GetArea( ) const
61 {
62   S a = S( 0 );
63   int q = this->GetNumberOfHarmonics( );
64   typename Self::const_iterator i = this->begin( );
65   for( int l = -q; i != this->end( ); ++i, ++l )
66     a += S( l ) * std::norm( *i );
67   return( S( vnl_math::pi ) * a );
68 }
69
70 // -------------------------------------------------------------------------
71 template< class S, unsigned int D >
72 S FourierSeries< S, D >::
73 GetCurvature( const S& w ) const
74 {
75   TComplex d1 = this->_Z( w, 1 );
76   TComplex d2 = this->_Z( w, 2 );
77   S d = std::abs( d1 );
78   if( d > S( 0 ) )
79     return( std::imag( std::conj( d1 ) * d2 ) / ( d * d * d ) );
80   else
81     return( S( 0 ) );
82 }
83
84 // -------------------------------------------------------------------------
85 template< class S, unsigned int D >
86 void FourierSeries< S, D >::
87 SetOrdering( bool cw )
88 {
89   // Check if the area sign is different from the desired orientation
90   bool ap = ( S( 0 ) < this->GetArea( ) );
91   if( ( !ap && cw ) || ( ap && !cw ) ) // XOR
92     this->InvertOrdering( );
93 }
94
95 // -------------------------------------------------------------------------
96 template< class S, unsigned int D >
97 void FourierSeries< S, D >::
98 InvertOrdering( )
99 {
100   int q = this->GetNumberOfHarmonics( );
101   for( int l = 1; l <= q; l++ )
102   {
103     TComplex z = ( *this )[ l ];
104     ( *this )[  l ] = ( *this )[ -l ];
105     ( *this )[ -l ] = z;
106
107   } // rof
108 }
109
110 // -------------------------------------------------------------------------
111 template< class S, unsigned int D >
112 void FourierSeries< S, D >::
113 SetOrderingToClockWise( )
114 {
115   this->SetOrdering( false );
116 }
117
118 // -------------------------------------------------------------------------
119 template< class S, unsigned int D >
120 void FourierSeries< S, D >::
121 SetOrderingToCounterClockWise( )
122 {
123   this->SetOrdering( true );
124 }
125
126 // -------------------------------------------------------------------------
127 template< class S, unsigned int D >
128 int FourierSeries< S, D >::
129 GetNumberOfHarmonics( ) const
130 {
131   return( ( this->size( ) - 1 ) >> 1 );
132 }
133
134 // -------------------------------------------------------------------------
135 template< class S, unsigned int D >
136 void FourierSeries< S, D >::
137 SetNumberOfHarmonics( const int& q )
138 {
139   int diff_q = this->GetNumberOfHarmonics( ) - q;
140
141   while( diff_q != 0 )
142   {
143     if( diff_q > 0 )
144     {
145       this->pop_front( );
146       this->pop_back( );
147       diff_q--;
148     }
149     else
150     {
151       this->push_front( TComplex( S( 0 ), S( 0 ) ) );
152       this->push_back( TComplex( S( 0 ), S( 0 ) ) );
153       diff_q++;
154
155     } // fi
156
157   } // elihw
158 }
159
160 // -------------------------------------------------------------------------
161 template< class S, unsigned int D >
162 typename FourierSeries< S, D >::
163 TComplex& FourierSeries< S, D >::
164 operator[]( const int& l )
165 {
166   static TComplex _zero;
167   int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
168   if( idx < 0 || idx >= this->size( ) )
169   {
170     _zero = TComplex( S( 0 ) );
171     return( _zero );
172   }
173   else
174     return( this->Superclass::operator[]( idx ) );
175 }
176
177 // -------------------------------------------------------------------------
178 template< class S, unsigned int D >
179 const typename FourierSeries< S, D >::
180 TComplex& FourierSeries< S, D >::
181 operator[]( const int& l ) const
182 {
183   static const TComplex _zero( S( 0 ) );
184   int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
185   if( idx < 0 || idx >= this->size( ) )
186     return( _zero );
187   else
188     return( this->Superclass::operator[]( idx ) );
189 }
190
191 // -------------------------------------------------------------------------
192 template< class S, unsigned int D >
193 typename FourierSeries< S, D >::
194 TPoint FourierSeries< S, D >::
195 operator( )( const S& w ) const
196 {
197   TComplex z = this->_Z( w, 0 );
198   TPoint p;
199   p.Fill( S( 0 ) );
200   p[ this->m_RealAxis ] = z.real( );
201   p[ this->m_ImagAxis ] = z.imag( );
202   return( p );
203 }
204
205 // -------------------------------------------------------------------------
206 template< class S, unsigned int D >
207 typename FourierSeries< S, D >::
208 TVector FourierSeries< S, D >::
209 operator( )( const S& w, const unsigned int& n ) const
210 {
211   TComplex z = this->_Z( w, n );
212   TVector v( S( 0 ) );
213   v[ this->m_RealAxis ] = z.real( );
214   v[ this->m_ImagAxis ] = z.imag( );
215   return( v );
216 }
217
218 // -------------------------------------------------------------------------
219 template< class S, unsigned int D >
220 typename FourierSeries< S, D >::
221 Self FourierSeries< S, D >::
222 operator+( const Self& o ) const
223 {
224   int q1 = o.GetNumberOfHarmonics( );
225   int q2 = this->GetNumberOfHarmonics( );
226   int q = ( q2 < q1 )? q1: q2;
227
228   Self res;
229   res.SetNumberOfHarmonics( q );
230   for( int l = -q; l <= q; ++l )
231     res[ l ] = ( *this )[ l ] + o[ l ];
232
233   return( res );
234 }
235
236 // -------------------------------------------------------------------------
237 template< class S, unsigned int D >
238 typename FourierSeries< S, D >::
239 Self& FourierSeries< S, D >::
240 operator+=( const Self& o )
241 {
242   int q1 = o.GetNumberOfHarmonics( );
243   int q2 = this->GetNumberOfHarmonics( );
244   int q = ( q2 < q1 )? q1: q2;
245
246   this->SetNumberOfHarmonics( q );
247   for( int l = -q; l <= q; ++l )
248     ( *this )[ l ] += o[ l ];
249
250   return( *this );
251 }
252
253 // -------------------------------------------------------------------------
254 template< class S, unsigned int D >
255 typename FourierSeries< S, D >::
256 Self FourierSeries< S, D >::
257 operator-( const Self& o ) const
258 {
259   int q1 = o.GetNumberOfHarmonics( );
260   int q2 = this->GetNumberOfHarmonics( );
261   int q = ( q2 < q1 )? q1: q2;
262
263   Self res;
264   res.SetNumberOfHarmonics( q );
265   for( int l = -q; l <= q; ++l )
266     res[ l ] = ( *this )[ l ] - o[ l ];
267
268   return( res );
269 }
270
271 // -------------------------------------------------------------------------
272 template< class S, unsigned int D >
273 typename FourierSeries< S, D >::
274 Self& FourierSeries< S, D >::
275 operator-=( const Self& o )
276 {
277   int q1 = o.GetNumberOfHarmonics( );
278   int q2 = this->GetNumberOfHarmonics( );
279   int q = ( q2 < q1 )? q1: q2;
280
281   this->SetNumberOfHarmonics( q );
282   for( int l = -q; l <= q; ++l )
283     ( *this )[ l ] -= o[ l ];
284
285   return( *this );
286 }
287
288 // -------------------------------------------------------------------------
289 template< class S, unsigned int D >
290 typename FourierSeries< S, D >::
291 Self FourierSeries< S, D >::
292 operator+( const TComplex& translation ) const
293 {
294   Self res = *this;
295   res[ 0 ] += translation;
296   return( res );
297 }
298
299 // -------------------------------------------------------------------------
300 template< class S, unsigned int D >
301 typename FourierSeries< S, D >::
302 Self& FourierSeries< S, D >::
303 operator+=( const TComplex& translation )
304 {
305   ( *this )[ 0 ] += translation;
306   return( *this );
307 }
308
309 // -------------------------------------------------------------------------
310 template< class S, unsigned int D >
311 typename FourierSeries< S, D >::
312 Self FourierSeries< S, D >::
313 operator-( const TComplex& translation ) const
314 {
315   Self res = *this;
316   res[ 0 ] -= translation;
317   return( res );
318 }
319
320 // -------------------------------------------------------------------------
321 template< class S, unsigned int D >
322 typename FourierSeries< S, D >::
323 Self& FourierSeries< S, D >::
324 operator-=( const TComplex& translation )
325 {
326   ( *this )[ 0 ] -= translation;
327   return( *this );
328 }
329
330 // -------------------------------------------------------------------------
331 template< class S, unsigned int D >
332 typename FourierSeries< S, D >::
333 Self FourierSeries< S, D >::
334 operator+( const TVector& translation ) const
335 {
336   return(
337     ( *this ) +
338     TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] )
339     );
340 }
341
342 // -------------------------------------------------------------------------
343 template< class S, unsigned int D >
344 typename FourierSeries< S, D >::
345 Self& FourierSeries< S, D >::
346 operator+=( const TVector& translation )
347 {
348   ( *this ) +=
349     TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] );
350   return( *this );
351 }
352
353 // -------------------------------------------------------------------------
354 template< class S, unsigned int D >
355 typename FourierSeries< S, D >::
356 Self FourierSeries< S, D >::
357 operator-( const TVector& translation ) const
358 {
359   return(
360     ( *this ) -
361     TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] )
362     );
363 }
364
365 // -------------------------------------------------------------------------
366 template< class S, unsigned int D >
367 typename FourierSeries< S, D >::
368 Self& FourierSeries< S, D >::
369 operator-=( const TVector& translation )
370 {
371   ( *this ) -=
372     TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] );
373   return( *this );
374 }
375
376 // -------------------------------------------------------------------------
377 template< class S, unsigned int D >
378 typename FourierSeries< S, D >::
379 TPoint FourierSeries< S, D >::
380 GetCenter( ) const
381 {
382   TComplex z = ( *this )[ 0 ];
383   TPoint p;
384   p.Fill( S( 0 ) );
385   p[ this->m_RealAxis ] = z.real( );
386   p[ this->m_ImagAxis ] = z.imag( );
387   return( p );
388 }
389
390 // -------------------------------------------------------------------------
391 template< class S, unsigned int D >
392 typename FourierSeries< S, D >::
393 Self FourierSeries< S, D >::
394 operator*( const TComplex& scale_or_rotation ) const
395 {
396   Self res;
397   res.clear( );
398   for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
399     res.push_back( *i * scale_or_rotation );
400   return( res );
401 }
402
403 // -------------------------------------------------------------------------
404 template< class S, unsigned int D >
405 typename FourierSeries< S, D >::
406 Self& FourierSeries< S, D >::
407 operator*=( const TComplex& scale_or_rotation )
408 {
409   for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
410     *i *= scale_or_rotation;
411   return( *this );
412 }
413
414 // -------------------------------------------------------------------------
415 template< class S, unsigned int D >
416 typename FourierSeries< S, D >::
417 Self FourierSeries< S, D >::
418 operator/( const S& scale ) const
419 {
420   Self res;
421   res.clear( );
422   for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
423     res.push_back( *i / scale );
424   return( res );
425 }
426
427 // -------------------------------------------------------------------------
428 template< class S, unsigned int D >
429 typename FourierSeries< S, D >::
430 Self& FourierSeries< S, D >::
431 operator/=( const S& scale )
432 {
433   for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
434     *i /= scale;
435   return( *this );
436 }
437
438 // -------------------------------------------------------------------------
439 template< class S, unsigned int D >
440 typename FourierSeries< S, D >::
441 Self FourierSeries< S, D >::
442 operator&( const S& phase ) const
443 {
444   Self res;
445   res.clear( );
446   int q = this->GetNumberOfHarmonics( );
447   typename Self::const_iterator i = this->begin( );
448   for( int l = -q; i != this->end( ); ++i, ++l )
449     res.push_back( *i * std::polar( S( 1 ), S( l ) * phase ) );
450   return( res );
451 }
452
453 // -------------------------------------------------------------------------
454 template< class S, unsigned int D >
455 typename FourierSeries< S, D >::
456 Self& FourierSeries< S, D >::
457 operator&=( const S& phase )
458 {
459   int q = this->GetNumberOfHarmonics( );
460   typename Self::iterator i = this->begin( );
461   for( int l = -q; i != this->end( ); ++i, ++l )
462     *i *= std::polar( S( 1 ), S( l ) * phase );
463   return( *this );
464 }
465
466 // -------------------------------------------------------------------------
467 template< class S, unsigned int D >
468 S FourierSeries< S, D >::
469 GetPhase( const S& w ) const
470 {
471   // Some constant values
472   static const S _0 = S( 0 );
473   static const S _1 = S( 1 );
474   static const S _2 = S( 2 );
475   static const S _2pi = _2 * S( vnl_math::pi );
476   static const S _epsilon = S( 1e-14 );
477   static const S _tolerance = S( 1e-10 );
478   static const unsigned int _samples = 100;
479   static const unsigned int _maxIterations = 1000;
480   static const S _angleOff = _2pi / S( _samples );
481
482   // Some other values
483   int q = int( this->GetNumberOfHarmonics( ) );
484   if( q == 0 )
485     return( _0 );
486   
487   // Get a centered copy
488   Self contour = *this;
489   contour[ 0 ] = TComplex( _0, _0 );
490
491   // Compute maximum coefficient norm
492   S max_norm = _0;
493   for( int l = 1; l <= q; ++l )
494   {
495     S np = std::abs( contour[  l ] );
496     S nn = std::abs( contour[ -l ] );
497     S n = ( np < nn )? nn: np;
498     if( max_norm < n )
499       max_norm = n;
500
501   } // rof
502
503   // Rotate to desired phase
504   contour *= std::polar( _1, -w );
505
506   // Try to normalize contour: if not, malformed contour!
507   if( max_norm > _0 )
508   {
509     contour /= max_norm;
510
511     // 1. Approximate initial guess by a simple dichotomy
512     std::vector< std::pair< S, S > > function;
513     S minA = std::numeric_limits< S >::max( );
514     unsigned int minIdx = -1;
515
516     // 1.1. Roughly sample phases
517     for( unsigned int s = 0; s < _samples; ++s )
518     {
519       S w = S( s ) * _angleOff;
520       S a = std::arg( contour._Z( w, 0 ) );
521       function.push_back( std::pair< S, S >( w, a ) );
522       if( a < minA )
523       {
524         minA = a;
525         minIdx = s;
526
527       } // fi
528
529     } // rof
530
531     // 1.2. Get zero cuts by zero crossing analysis and keep the farthest
532     //      point in the real axis (ie. the last data in "cuts")
533     S prevA = _1;
534     std::map< S, unsigned int > cuts;
535     for( unsigned int s = 0; s < _samples; ++s )
536     {
537       unsigned int i = ( s + minIdx ) % _samples;
538       if( function[ i ].second * prevA < _0 )
539         cuts[ std::real( contour._Z( function[ i ].first, 0 ) ) ] = i;
540       prevA = function[ i ].second;
541
542     } // rof
543
544     // 1.3. Get initial guess
545     S w0 = _0;
546     if( cuts.size( ) > 0 )
547     {
548       unsigned int i = cuts.rbegin( )->second;
549       if( i == 0 )
550         i = function.size( );
551       w0 = function[ i - 1 ].first;
552
553     } // fi
554
555     // 2. Refine by Newthon-Raphson
556     bool stop = false;
557     unsigned int i = 0;
558     while( i < _maxIterations && !stop )
559     {
560       TComplex c = contour._Z( w0, 0 );
561       S d = std::imag( c * std::conj( contour._Z( w0, 1 ) ) );
562
563       // Emergency stop!!!
564       if( !( std::fabs( d ) < _epsilon ) )
565       {
566         S w1 = w0 + ( std::arg( c ) * ( std::norm( c ) / d ) );
567         if( ( std::fabs( w1 - w0 ) / std::fabs( w1 ) ) < _tolerance )
568           stop = true;
569         else
570           w0 = w1;
571         i++;
572       }
573       else
574         stop = true;
575
576     } // elihw
577     return( w0 );
578   }
579   else
580     return( _0 );
581 }
582
583 // -------------------------------------------------------------------------
584 template< class S, unsigned int D >
585 void FourierSeries< S, D >::
586 Sample( std::vector< TPoint >& p, const unsigned long& s ) const
587 {
588   static const S _2pi = S( 2 ) * S( vnl_math::pi );
589   S off = _2pi / S( s - 1 );
590   for( unsigned int w = 0; w < s; ++w )
591     p.push_back( ( *this )( S( w ) * off ) );
592 }
593
594 // -------------------------------------------------------------------------
595 template< class S, unsigned int D >
596 void FourierSeries< S, D >::
597 GetEllipse( int l, S& a, S& b, S& t, S& p ) const
598 {
599   a = b = t = p = S( 0 );
600   if( l == 0 || l > this->GetNumberOfHarmonics( ) )
601     return;
602
603   TComplex zp = ( *this )[  l ];
604   TComplex zn = ( *this )[ -l ];
605   S np = std::abs( zp );
606   S nn = std::abs( zn );
607
608   // Radii
609   a = np + nn;
610   b = np - nn;
611
612   // Rotation and phase
613   if( np > S( 0 ) && nn > S( 0 ) )
614   {
615     zp /= np;
616     zn /= nn;
617     t = std::real( std::log( zp * zn ) / TComplex( S( 0 ), S( 2 ) ) );
618     p = std::real( std::log( zp / zn ) / TComplex( S( 0 ), S( 2 * l ) ) );
619   }
620   else
621   {
622     t = S( 0 );
623     p = ( np > S( 0 ) )? std::arg( zp ): std::arg( zn );
624
625   } // fi
626 }
627
628 // -------------------------------------------------------------------------
629 template< class S, unsigned int D >
630 void FourierSeries< S, D >::
631 SetEllipse( int l, S& a, S& b, S& t, S& p )
632 {
633   S nzp = ( a + b ) / S( 2 );
634   S nzn = ( a - b ) / S( 2 );
635   TComplex et = std::polar( S( 1 ), t );
636   TComplex ep = std::polar( S( 1 ), S( l ) * p );
637   ( *this )[  l ] = et * ep * nzp;
638   ( *this )[ -l ] = et * std::conj( ep ) * nzn;
639 }
640
641 // -------------------------------------------------------------------------
642 template< class S, unsigned int D >
643 typename FourierSeries< S, D >::
644 TComplex FourierSeries< S, D >::
645 _Z( const S& w, const unsigned int& n ) const
646 {
647   static const S _0 = S( 0 );
648   static const S _1 = S( 1 );
649   S _n = S( n );
650
651   TComplex z( _0, _0 );
652   int q = this->GetNumberOfHarmonics( );
653   for( int l = -q; l <= q; ++l )
654   {
655     TComplex v = ( *this )[ l ] * std::polar( _1, w * S( l ) );
656     if( n > 0 )
657       v *= std::pow( TComplex( _0, S( l ) ), _n );
658     z += v;
659
660   } // rof
661   return( z );
662 }
663
664 // -------------------------------------------------------------------------
665 template< class S, unsigned int D >
666 void FourierSeries< S, D >::
667 _DFT( const std::vector< TComplex >& p, unsigned int q )
668 {
669   static const S _2pi = S( 2 ) * S( vnl_math::pi );
670
671   this->SetNumberOfHarmonics( q );
672   *this *= S( 0 );
673   if( p.size( ) == 0 )
674     return;
675
676   std::vector< TComplex > dft;
677   for( long m = 0; m < p.size( ); ++m )
678   {
679     TComplex z( S( 0 ), S( 0 ) );
680     for( long k = 0; k < p.size( ); ++k )
681       z +=
682         p[ k ] *
683         std::polar( S( 1 ), -( _2pi * S( m * k ) ) / S( p.size( ) ) );
684     z /= S( p.size( ) );
685     dft.push_back( z );
686
687   } // rof
688   
689   ( *this )[ 0 ] = dft[ 0 ];
690   for( int l = 1; l <= q; ++l )
691   {
692     ( *this )[  l ] = dft[ l ];
693     ( *this )[ -l ] = dft[ p.size( ) - l ];
694
695   } // rof
696
697   // Reset contour
698   /* TODO:
699   this->SetNumberOfHarmonics( q );
700
701   // Compute number of calculations K: cut harmonics to q
702   long N = long( p.size( ) );
703   if( N == 0 )
704     return;
705   long K = ( long( q ) << 1 ) + 1;
706   if( K < N - 1 )
707     K = N - 1;
708
709   // Compute harmonics
710   for( long l = -K; l <= K; ++l )
711   {
712     S k = ( ( S( l ) + S( ( l < 0 )? N: 0 ) ) * _2pi ) / S( N );
713     ( *this )[ l ] = TComplex( S( 0 ), S( 0 ) );
714     for( long n = 0; n < N; ++n )
715       ( *this )[ l ] += p[ n ] * std::polar( S( 1 ), k * S( -n ) );
716     ( *this )[ l ] /= S( N );
717
718   } // rof
719   */
720 }
721
722 // -------------------------------------------------------------------------
723 template class FourierSeries< float, 2 >;
724 template class FourierSeries< float, 3 >;
725 template class FourierSeries< double, 2 >;
726 template class FourierSeries< double, 3 >;
727
728 // eof - $RCSfile$