6 #include <vnl/vnl_math.h>
7 #include <cpExtensions/DataStructures/FourierSeriesContour.h>
9 // -------------------------------------------------------------------------
10 template< class _TScalar >
11 cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
12 FourierSeriesContour( unsigned int q )
14 this->resize( ( q << 1 ) + 1, TComplex( TScalar( 0 ), TScalar( 0 ) ) );
17 // -------------------------------------------------------------------------
18 template< class _TScalar >
19 cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
20 ~FourierSeriesContour( )
24 // -------------------------------------------------------------------------
25 template< class _TScalar >
26 _TScalar cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
29 TScalar a = TScalar( 0 );
30 int q = this->GetNumberOfHarmonics( );
31 typename Self::const_iterator i = this->begin( );
32 for( int l = -q; i != this->end( ); ++i, ++l )
33 a += TScalar( l ) * std::norm( *i );
34 return( TScalar( vnl_math::pi ) * a );
37 // -------------------------------------------------------------------------
38 template< class _TScalar >
39 _TScalar cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
40 GetCurvature( const _TScalar& w ) const
42 TComplex d1 = this->_Z( w, 1 );
43 TComplex d2 = this->_Z( w, 2 );
44 TScalar d = std::abs( d1 );
45 if( d > TScalar( 0 ) )
46 return( std::imag( std::conj( d1 ) * d2 ) / ( d * d * d ) );
48 return( TScalar( 0 ) );
51 // -------------------------------------------------------------------------
52 template< class _TScalar >
53 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
54 SetOrdering( bool cw )
56 // Check if the area sign is different from the desired orientation
57 bool ap = ( TScalar( 0 ) < this->GetArea( ) );
58 if( ( !ap && cw ) || ( ap && !cw ) ) // XOR
59 this->InvertOrdering( );
62 // -------------------------------------------------------------------------
63 template< class _TScalar >
64 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
67 int q = this->GetNumberOfHarmonics( );
68 for( int l = 1; l <= q; l++ )
70 TComplex z = ( *this )[ l ];
71 ( *this )[ l ] = ( *this )[ -l ];
77 // -------------------------------------------------------------------------
78 template< class _TScalar >
79 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
80 SetOrderingToClockWise( )
82 this->SetOrdering( false );
85 // -------------------------------------------------------------------------
86 template< class _TScalar >
87 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
88 SetOrderingToCounterClockWise( )
90 this->SetOrdering( true );
93 // -------------------------------------------------------------------------
94 template< class _TScalar >
95 int cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
96 GetNumberOfHarmonics( ) const
98 return( ( this->size( ) - 1 ) >> 1 );
101 // -------------------------------------------------------------------------
102 template< class _TScalar >
103 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
104 SetNumberOfHarmonics( const int& q )
106 int diff_q = this->GetNumberOfHarmonics( ) - q;
118 this->push_front( TComplex( TScalar( 0 ), TScalar( 0 ) ) );
119 this->push_back( TComplex( TScalar( 0 ), TScalar( 0 ) ) );
127 // -------------------------------------------------------------------------
128 template< class _TScalar >
129 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
130 TComplex& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
131 operator[]( const int& l )
133 static TComplex _zero;
134 int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
135 if( idx < 0 || idx >= this->size( ) )
137 _zero = TComplex( TScalar( 0 ) );
141 return( this->Superclass::operator[]( idx ) );
144 // -------------------------------------------------------------------------
145 template< class _TScalar >
146 const typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
147 TComplex& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
148 operator[]( const int& l ) const
150 static const TComplex _zero( TScalar( 0 ) );
151 int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
152 if( idx < 0 || idx >= this->size( ) )
155 return( this->Superclass::operator[]( idx ) );
158 // -------------------------------------------------------------------------
159 template< class _TScalar >
160 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
161 TPoint cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
162 operator( )( const _TScalar& w ) const
164 TComplex z = this->_Z( w, 0 );
171 // -------------------------------------------------------------------------
172 template< class _TScalar >
173 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
174 TVector cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
175 operator( )( const _TScalar& w, const unsigned int& n ) const
177 TComplex z = this->_Z( w, n );
184 // -------------------------------------------------------------------------
185 template< class _TScalar >
186 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
187 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
188 operator+( const Self& o ) const
190 int q1 = o.GetNumberOfHarmonics( );
191 int q2 = this->GetNumberOfHarmonics( );
192 int q = ( q2 < q1 )? q1: q2;
195 res.SetNumberOfHarmonics( q );
196 for( int l = -q; l <= q; ++l )
197 res[ l ] = ( *this )[ l ] + o[ l ];
202 // -------------------------------------------------------------------------
203 template< class _TScalar >
204 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
205 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
206 operator+=( const Self& o )
208 int q1 = o.GetNumberOfHarmonics( );
209 int q2 = this->GetNumberOfHarmonics( );
210 int q = ( q2 < q1 )? q1: q2;
212 this->SetNumberOfHarmonics( q );
213 for( int l = -q; l <= q; ++l )
214 ( *this )[ l ] += o[ l ];
219 // -------------------------------------------------------------------------
220 template< class _TScalar >
221 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
222 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
223 operator-( const Self& o ) const
225 int q1 = o.GetNumberOfHarmonics( );
226 int q2 = this->GetNumberOfHarmonics( );
227 int q = ( q2 < q1 )? q1: q2;
230 res.SetNumberOfHarmonics( q );
231 for( int l = -q; l <= q; ++l )
232 res[ l ] = ( *this )[ l ] - o[ l ];
237 // -------------------------------------------------------------------------
238 template< class _TScalar >
239 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
240 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
241 operator-=( const Self& o )
243 int q1 = o.GetNumberOfHarmonics( );
244 int q2 = this->GetNumberOfHarmonics( );
245 int q = ( q2 < q1 )? q1: q2;
247 this->SetNumberOfHarmonics( q );
248 for( int l = -q; l <= q; ++l )
249 ( *this )[ l ] -= o[ l ];
254 // -------------------------------------------------------------------------
255 template< class _TScalar >
256 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
257 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
258 operator+( const TComplex& translation ) const
261 res[ 0 ] += translation;
265 // -------------------------------------------------------------------------
266 template< class _TScalar >
267 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
268 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
269 operator+=( const TComplex& translation )
271 ( *this )[ 0 ] += translation;
275 // -------------------------------------------------------------------------
276 template< class _TScalar >
277 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
278 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
279 operator-( const TComplex& translation ) const
282 res[ 0 ] -= translation;
286 // -------------------------------------------------------------------------
287 template< class _TScalar >
288 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
289 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
290 operator-=( const TComplex& translation )
292 ( *this )[ 0 ] -= translation;
296 // -------------------------------------------------------------------------
297 template< class _TScalar >
298 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
299 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
300 operator+( const TVector& translation ) const
302 return( ( *this ) + TComplex( translation[ 0 ], translation[ 1 ] ) );
305 // -------------------------------------------------------------------------
306 template< class _TScalar >
307 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
308 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
309 operator+=( const TVector& translation )
311 ( *this ) += TComplex( translation[ 0 ], translation[ 1 ] );
315 // -------------------------------------------------------------------------
316 template< class _TScalar >
317 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
318 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
319 operator-( const TVector& translation ) const
321 return( ( *this ) - TComplex( translation[ 0 ], translation[ 1 ] ) );
324 // -------------------------------------------------------------------------
325 template< class _TScalar >
326 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
327 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
328 operator-=( const TVector& translation )
330 ( *this ) -= TComplex( translation[ 0 ], translation[ 1 ] );
334 // -------------------------------------------------------------------------
335 template< class _TScalar >
336 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
337 TPoint cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
340 TComplex z = ( *this )[ 0 ];
347 // -------------------------------------------------------------------------
348 template< class _TScalar >
349 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
350 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
351 operator*( const TComplex& scale_or_rotation ) const
355 for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
356 res.push_back( *i * scale_or_rotation );
360 // -------------------------------------------------------------------------
361 template< class _TScalar >
362 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
363 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
364 operator*=( const TComplex& scale_or_rotation )
366 for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
367 *i *= scale_or_rotation;
371 // -------------------------------------------------------------------------
372 template< class _TScalar >
373 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
374 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
375 operator/( const _TScalar& scale ) const
379 for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
380 res.push_back( *i / scale );
384 // -------------------------------------------------------------------------
385 template< class _TScalar >
386 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
387 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
388 operator/=( const _TScalar& scale )
390 for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
395 // -------------------------------------------------------------------------
396 template< class _TScalar >
397 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
398 Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
399 operator&( const _TScalar& phase ) const
403 int q = this->GetNumberOfHarmonics( );
404 typename Self::const_iterator i = this->begin( );
405 for( int l = -q; i != this->end( ); ++i, ++l )
406 res.push_back( *i * std::polar( TScalar( 1 ), TScalar( l ) * phase ) );
410 // -------------------------------------------------------------------------
411 template< class _TScalar >
412 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
413 Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
414 operator&=( const _TScalar& phase )
416 int q = this->GetNumberOfHarmonics( );
417 typename Self::iterator i = this->begin( );
418 for( int l = -q; i != this->end( ); ++i, ++l )
419 *i *= std::polar( TScalar( 1 ), TScalar( l ) * phase );
423 // -------------------------------------------------------------------------
424 template< class _TScalar >
425 _TScalar cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
426 GetPhase( const _TScalar& w ) const
428 // Some constant values
429 static const TScalar _0 = TScalar( 0 );
430 static const TScalar _1 = TScalar( 1 );
431 static const TScalar _2 = TScalar( 2 );
432 static const TScalar _2pi = _2 * TScalar( vnl_math::pi );
433 static const TScalar _epsilon = TScalar( 1e-14 );
434 static const TScalar _tolerance = TScalar( 1e-10 );
435 static const unsigned int _samples = 100;
436 static const unsigned int _maxIterations = 1000;
437 static const TScalar _angleOff = _2pi / TScalar( _samples );
440 int q = int( this->GetNumberOfHarmonics( ) );
444 // Get a centered copy
445 Self contour = *this;
446 contour[ 0 ] = TComplex( _0, _0 );
448 // Compute maximum coefficient norm
449 TScalar max_norm = _0;
450 for( int l = 1; l <= q; ++l )
452 TScalar np = std::abs( contour[ l ] );
453 TScalar nn = std::abs( contour[ -l ] );
454 TScalar n = ( np < nn )? nn: np;
460 // Rotate to desired phase
461 contour *= std::polar( _1, -w );
463 // Try to normalize contour: if not, malformed contour!
468 // 1. Approximate initial guess by a simple dichotomy
469 std::vector< std::pair< TScalar, TScalar > > function;
470 TScalar minA = std::numeric_limits< TScalar >::max( );
471 unsigned int minIdx = -1;
473 // 1.1. Roughly sample phases
474 for( unsigned int s = 0; s < _samples; ++s )
476 TScalar w = TScalar( s ) * _angleOff;
477 TScalar a = std::arg( contour._Z( w, 0 ) );
478 function.push_back( std::pair< TScalar, TScalar >( w, a ) );
488 // 1.2. Get zero cuts by zero crossing analysis and keep the farthest
489 // point in the real axis (ie. the last data in "cuts")
491 std::map< TScalar, unsigned int > cuts;
492 for( unsigned int s = 0; s < _samples; ++s )
494 unsigned int i = ( s + minIdx ) % _samples;
495 if( function[ i ].second * prevA < _0 )
496 cuts[ std::real( contour._Z( function[ i ].first, 0 ) ) ] = i;
497 prevA = function[ i ].second;
501 // 1.3. Get initial guess
503 if( cuts.size( ) > 0 )
505 unsigned int i = cuts.rbegin( )->second;
507 i = function.size( );
508 w0 = function[ i - 1 ].first;
512 // 2. Refine by Newthon-Raphson
515 while( i < _maxIterations && !stop )
517 TComplex c = contour._Z( w0, 0 );
518 TScalar d = std::imag( c * std::conj( contour._Z( w0, 1 ) ) );
521 if( !( std::fabs( d ) < _epsilon ) )
523 TScalar w1 = w0 + ( std::arg( c ) * ( std::norm( c ) / d ) );
524 if( ( std::fabs( w1 - w0 ) / std::fabs( w1 ) ) < _tolerance )
540 // -------------------------------------------------------------------------
541 template< class _TScalar >
542 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
543 Sample( std::vector< TPoint >& p, const unsigned long& s ) const
545 static const TScalar _2pi = TScalar( 2 ) * TScalar( vnl_math::pi );
546 TScalar off = _2pi / TScalar( s - 1 );
547 for( unsigned int w = 0; w < s; ++w )
548 p.push_back( ( *this )( TScalar( w ) * off ) );
551 // -------------------------------------------------------------------------
552 template< class _TScalar >
553 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
554 GetEllipse( int l, _TScalar& a, _TScalar& b, _TScalar& t, _TScalar& p ) const
556 a = b = t = p = TScalar( 0 );
557 if( l == 0 || l > this->GetNumberOfHarmonics( ) )
560 TComplex zp = ( *this )[ l ];
561 TComplex zn = ( *this )[ -l ];
562 TScalar np = std::abs( zp );
563 TScalar nn = std::abs( zn );
569 // Rotation and phase
570 if( np > TScalar( 0 ) && nn > TScalar( 0 ) )
575 std::log( zp * zn ) / TComplex( TScalar( 0 ), TScalar( 2 ) )
578 std::log( zp / zn ) / TComplex( TScalar( 0 ), TScalar( 2 * l ) )
584 p = ( np > TScalar( 0 ) )? std::arg( zp ): std::arg( zn );
589 // -------------------------------------------------------------------------
590 template< class _TScalar >
591 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
592 SetEllipse( int l, _TScalar& a, _TScalar& b, _TScalar& t, _TScalar& p )
594 TScalar nzp = ( a + b ) / TScalar( 2 );
595 TScalar nzn = ( a - b ) / TScalar( 2 );
596 TComplex et = std::polar( TScalar( 1 ), t );
597 TComplex ep = std::polar( TScalar( 1 ), TScalar( l ) * p );
598 ( *this )[ l ] = et * ep * nzp;
599 ( *this )[ -l ] = et * std::conj( ep ) * nzn;
602 // -------------------------------------------------------------------------
603 template< class _TScalar >
604 typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
605 TComplex cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
606 _Z( const _TScalar& w, const unsigned int& n ) const
608 static const TScalar _0 = TScalar( 0 );
609 static const TScalar _1 = TScalar( 1 );
610 TScalar _n = TScalar( n );
612 TComplex z( _0, _0 );
613 int q = this->GetNumberOfHarmonics( );
614 for( int l = -q; l <= q; ++l )
616 TComplex v = ( *this )[ l ] * std::polar( _1, w * TScalar( l ) );
618 v *= std::pow( TComplex( _0, TScalar( l ) ), _n );
625 // -------------------------------------------------------------------------
626 template< class _TScalar >
627 void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
628 _DFT( const std::vector< TComplex >& p, unsigned int q )
630 static const TScalar _2pi = TScalar( 2 ) * TScalar( vnl_math::pi );
632 this->SetNumberOfHarmonics( q );
633 *this *= TScalar( 0 );
637 std::vector< TComplex > dft;
638 for( long m = 0; m < p.size( ); ++m )
640 TComplex z( TScalar( 0 ), TScalar( 0 ) );
641 for( long k = 0; k < p.size( ); ++k )
646 -( _2pi * TScalar( m * k ) ) / TScalar( p.size( ) )
648 z /= TScalar( p.size( ) );
653 ( *this )[ 0 ] = dft[ 0 ];
654 for( int l = 1; l <= q; ++l )
656 ( *this )[ l ] = dft[ l ];
657 ( *this )[ -l ] = dft[ p.size( ) - l ];
662 // -------------------------------------------------------------------------
663 template class cpExtensions::DataStructures::FourierSeriesContour< float >;
664 template class cpExtensions::DataStructures::FourierSeriesContour< double >;