1 // =========================================================================
2 // @author: Leonardo Florez-Valencia
3 // @email: florez-l@javeriana.edu.co
4 // =========================================================================
9 #include <vnl/vnl_math.h>
10 #include <ivq/ITK/FourierSeries.h>
12 // -------------------------------------------------------------------------
13 template< class _TScalar, unsigned int _VDim >
14 ivq::ITK::FourierSeries< _TScalar, _VDim >::
15 FourierSeries( unsigned int q )
16 : m_RealAxis( _VDim - 2 ),
17 m_ImagAxis( _VDim - 1 )
19 this->resize( ( q << 1 ) + 1, TComplex( TScalar( 0 ), TScalar( 0 ) ) );
22 // -------------------------------------------------------------------------
23 template< class _TScalar, unsigned int _VDim >
24 ivq::ITK::FourierSeries< _TScalar, _VDim >::
29 // -------------------------------------------------------------------------
30 template< class _TScalar, unsigned int _VDim >
31 const unsigned int& ivq::ITK::FourierSeries< _TScalar, _VDim >::
34 return( this->m_RealAxis );
37 // -------------------------------------------------------------------------
38 template< class _TScalar, unsigned int _VDim >
39 const unsigned int& ivq::ITK::FourierSeries< _TScalar, _VDim >::
42 return( this->m_ImagAxis );
45 // -------------------------------------------------------------------------
46 template< class _TScalar, unsigned int _VDim >
47 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
48 SetRealAxis( const unsigned int& a )
53 // -------------------------------------------------------------------------
54 template< class _TScalar, unsigned int _VDim >
55 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
56 SetImagAxis( const unsigned int& a )
61 // -------------------------------------------------------------------------
62 template< class _TScalar, unsigned int _VDim >
63 _TScalar ivq::ITK::FourierSeries< _TScalar, _VDim >::
66 TScalar a = TScalar( 0 );
67 int q = this->GetNumberOfHarmonics( );
68 typename Self::const_iterator i = this->begin( );
69 for( int l = -q; i != this->end( ); ++i, ++l )
70 a += TScalar( l ) * std::norm( *i );
71 return( TScalar( vnl_math::pi ) * a );
74 // -------------------------------------------------------------------------
75 template< class _TScalar, unsigned int _VDim >
76 _TScalar ivq::ITK::FourierSeries< _TScalar, _VDim >::
77 GetCurvature( const TScalar& w ) const
79 TComplex d1 = this->_Z( w, 1 );
80 TComplex d2 = this->_Z( w, 2 );
81 TScalar d = std::abs( d1 );
82 if( d > TScalar( 0 ) )
83 return( std::imag( std::conj( d1 ) * d2 ) / ( d * d * d ) );
85 return( TScalar( 0 ) );
88 // -------------------------------------------------------------------------
89 template< class _TScalar, unsigned int _VDim >
90 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
91 SetOrdering( bool cw )
93 // Check if the area sign is different from the desired orientation
94 bool ap = ( TScalar( 0 ) < this->GetArea( ) );
95 if( ( !ap && cw ) || ( ap && !cw ) ) // XOR
96 this->InvertOrdering( );
99 // -------------------------------------------------------------------------
100 template< class _TScalar, unsigned int _VDim >
101 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
104 int q = this->GetNumberOfHarmonics( );
105 for( int l = 1; l <= q; l++ )
107 TComplex z = ( *this )[ l ];
108 ( *this )[ l ] = ( *this )[ -l ];
114 // -------------------------------------------------------------------------
115 template< class _TScalar, unsigned int _VDim >
116 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
117 SetOrderingToClockWise( )
119 this->SetOrdering( false );
122 // -------------------------------------------------------------------------
123 template< class _TScalar, unsigned int _VDim >
124 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
125 SetOrderingToCounterClockWise( )
127 this->SetOrdering( true );
130 // -------------------------------------------------------------------------
131 template< class _TScalar, unsigned int _VDim >
132 int ivq::ITK::FourierSeries< _TScalar, _VDim >::
133 GetNumberOfHarmonics( ) const
135 return( ( this->size( ) - 1 ) >> 1 );
138 // -------------------------------------------------------------------------
139 template< class _TScalar, unsigned int _VDim >
140 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
141 SetNumberOfHarmonics( const int& q )
143 int diff_q = this->GetNumberOfHarmonics( ) - q;
155 this->push_front( TComplex( TScalar( 0 ), TScalar( 0 ) ) );
156 this->push_back( TComplex( TScalar( 0 ), TScalar( 0 ) ) );
164 // -------------------------------------------------------------------------
165 template< class _TScalar, unsigned int _VDim >
166 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
167 TComplex& ivq::ITK::FourierSeries< _TScalar, _VDim >::
168 operator[]( const int& l )
170 static TComplex _zero;
171 int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
172 if( idx < 0 || idx >= this->size( ) )
174 _zero = TComplex( TScalar( 0 ) );
178 return( this->Superclass::operator[]( idx ) );
181 // -------------------------------------------------------------------------
182 template< class _TScalar, unsigned int _VDim >
183 const typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
184 TComplex& ivq::ITK::FourierSeries< _TScalar, _VDim >::
185 operator[]( const int& l ) const
187 static const TComplex _zero( TScalar( 0 ) );
188 int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
189 if( idx < 0 || idx >= this->size( ) )
192 return( this->Superclass::operator[]( idx ) );
195 // -------------------------------------------------------------------------
196 template< class _TScalar, unsigned int _VDim >
197 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
198 TPoint ivq::ITK::FourierSeries< _TScalar, _VDim >::
199 operator( )( const TScalar& w ) const
201 TComplex z = this->_Z( w, 0 );
203 p.Fill( TScalar( 0 ) );
204 p[ this->m_RealAxis ] = z.real( );
205 p[ this->m_ImagAxis ] = z.imag( );
209 // -------------------------------------------------------------------------
210 template< class _TScalar, unsigned int _VDim >
211 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
212 TVector ivq::ITK::FourierSeries< _TScalar, _VDim >::
213 operator( )( const TScalar& w, const unsigned int& n ) const
215 TComplex z = this->_Z( w, n );
216 TVector v( TScalar( 0 ) );
217 v[ this->m_RealAxis ] = z.real( );
218 v[ this->m_ImagAxis ] = z.imag( );
222 // -------------------------------------------------------------------------
223 template< class _TScalar, unsigned int _VDim >
224 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
225 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
226 operator+( const Self& o ) const
228 int q1 = o.GetNumberOfHarmonics( );
229 int q2 = this->GetNumberOfHarmonics( );
230 int q = ( q2 < q1 )? q1: q2;
233 res.SetNumberOfHarmonics( q );
234 for( int l = -q; l <= q; ++l )
235 res[ l ] = ( *this )[ l ] + o[ l ];
240 // -------------------------------------------------------------------------
241 template< class _TScalar, unsigned int _VDim >
242 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
243 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
244 operator+=( const Self& o )
246 int q1 = o.GetNumberOfHarmonics( );
247 int q2 = this->GetNumberOfHarmonics( );
248 int q = ( q2 < q1 )? q1: q2;
250 this->SetNumberOfHarmonics( q );
251 for( int l = -q; l <= q; ++l )
252 ( *this )[ l ] += o[ l ];
257 // -------------------------------------------------------------------------
258 template< class _TScalar, unsigned int _VDim >
259 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
260 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
261 operator-( const Self& o ) const
263 int q1 = o.GetNumberOfHarmonics( );
264 int q2 = this->GetNumberOfHarmonics( );
265 int q = ( q2 < q1 )? q1: q2;
268 res.SetNumberOfHarmonics( q );
269 for( int l = -q; l <= q; ++l )
270 res[ l ] = ( *this )[ l ] - o[ l ];
275 // -------------------------------------------------------------------------
276 template< class _TScalar, unsigned int _VDim >
277 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
278 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
279 operator-=( const Self& o )
281 int q1 = o.GetNumberOfHarmonics( );
282 int q2 = this->GetNumberOfHarmonics( );
283 int q = ( q2 < q1 )? q1: q2;
285 this->SetNumberOfHarmonics( q );
286 for( int l = -q; l <= q; ++l )
287 ( *this )[ l ] -= o[ l ];
292 // -------------------------------------------------------------------------
293 template< class _TScalar, unsigned int _VDim >
294 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
295 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
296 operator+( const TComplex& translation ) const
299 res[ 0 ] += translation;
303 // -------------------------------------------------------------------------
304 template< class _TScalar, unsigned int _VDim >
305 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
306 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
307 operator+=( const TComplex& translation )
309 ( *this )[ 0 ] += translation;
313 // -------------------------------------------------------------------------
314 template< class _TScalar, unsigned int _VDim >
315 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
316 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
317 operator-( const TComplex& translation ) const
320 res[ 0 ] -= translation;
324 // -------------------------------------------------------------------------
325 template< class _TScalar, unsigned int _VDim >
326 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
327 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
328 operator-=( const TComplex& translation )
330 ( *this )[ 0 ] -= translation;
334 // -------------------------------------------------------------------------
335 template< class _TScalar, unsigned int _VDim >
336 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
337 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
338 operator+( const TVector& translation ) const
342 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] )
346 // -------------------------------------------------------------------------
347 template< class _TScalar, unsigned int _VDim >
348 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
349 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
350 operator+=( const TVector& translation )
353 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] );
357 // -------------------------------------------------------------------------
358 template< class _TScalar, unsigned int _VDim >
359 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
360 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
361 operator-( const TVector& translation ) const
365 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] )
369 // -------------------------------------------------------------------------
370 template< class _TScalar, unsigned int _VDim >
371 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
372 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
373 operator-=( const TVector& translation )
376 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] );
380 // -------------------------------------------------------------------------
381 template< class _TScalar, unsigned int _VDim >
382 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
383 TPoint ivq::ITK::FourierSeries< _TScalar, _VDim >::
386 TComplex z = ( *this )[ 0 ];
388 p.Fill( TScalar( 0 ) );
389 p[ this->m_RealAxis ] = z.real( );
390 p[ this->m_ImagAxis ] = z.imag( );
394 // -------------------------------------------------------------------------
395 template< class _TScalar, unsigned int _VDim >
396 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
397 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
398 operator*( const TComplex& scale_or_rotation ) const
402 for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
403 res.push_back( *i * scale_or_rotation );
407 // -------------------------------------------------------------------------
408 template< class _TScalar, unsigned int _VDim >
409 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
410 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
411 operator*=( const TComplex& scale_or_rotation )
413 for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
414 *i *= scale_or_rotation;
418 // -------------------------------------------------------------------------
419 template< class _TScalar, unsigned int _VDim >
420 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
421 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
422 operator/( const TScalar& scale ) const
426 for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
427 res.push_back( *i / scale );
431 // -------------------------------------------------------------------------
432 template< class _TScalar, unsigned int _VDim >
433 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
434 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
435 operator/=( const TScalar& scale )
437 for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
442 // -------------------------------------------------------------------------
443 template< class _TScalar, unsigned int _VDim >
444 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
445 Self ivq::ITK::FourierSeries< _TScalar, _VDim >::
446 operator&( const TScalar& phase ) const
450 int q = this->GetNumberOfHarmonics( );
451 typename Self::const_iterator i = this->begin( );
452 for( int l = -q; i != this->end( ); ++i, ++l )
453 res.push_back( *i * std::polar( TScalar( 1 ), TScalar( l ) * phase ) );
457 // -------------------------------------------------------------------------
458 template< class _TScalar, unsigned int _VDim >
459 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
460 Self& ivq::ITK::FourierSeries< _TScalar, _VDim >::
461 operator&=( const TScalar& phase )
463 int q = this->GetNumberOfHarmonics( );
464 typename Self::iterator i = this->begin( );
465 for( int l = -q; i != this->end( ); ++i, ++l )
466 *i *= std::polar( TScalar( 1 ), TScalar( l ) * phase );
470 // -------------------------------------------------------------------------
471 template< class _TScalar, unsigned int _VDim >
472 _TScalar ivq::ITK::FourierSeries< _TScalar, _VDim >::
473 GetPhase( const TScalar& w ) const
475 // Some constant values
476 static const TScalar _0 = TScalar( 0 );
477 static const TScalar _1 = TScalar( 1 );
478 static const TScalar _2 = TScalar( 2 );
479 static const TScalar _2pi = _2 * TScalar( vnl_math::pi );
480 static const TScalar _epsilon = TScalar( 1e-14 );
481 static const TScalar _tolerance = TScalar( 1e-10 );
482 static const unsigned int _samples = 100;
483 static const unsigned int _maxIterations = 1000;
484 static const TScalar _angleOff = _2pi / TScalar( _samples );
487 int q = int( this->GetNumberOfHarmonics( ) );
491 // Get a centered copy
492 Self contour = *this;
493 contour[ 0 ] = TComplex( _0, _0 );
495 // Compute maximum coefficient norm
496 TScalar max_norm = _0;
497 for( int l = 1; l <= q; ++l )
499 TScalar np = std::abs( contour[ l ] );
500 TScalar nn = std::abs( contour[ -l ] );
501 TScalar n = ( np < nn )? nn: np;
507 // Rotate to desired phase
508 contour *= std::polar( _1, -w );
510 // Try to normalize contour: if not, malformed contour!
515 // 1. Approximate initial guess by a simple dichotomy
516 std::vector< std::pair< TScalar, TScalar > > function;
517 TScalar minA = std::numeric_limits< TScalar >::max( );
518 unsigned int minIdx = -1;
520 // 1.1. Roughly sample phases
521 for( unsigned int s = 0; s < _samples; ++s )
523 TScalar w = TScalar( s ) * _angleOff;
524 TScalar a = std::arg( contour._Z( w, 0 ) );
525 function.push_back( std::pair< TScalar, TScalar >( w, a ) );
535 // 1.2. Get zero cuts by zero crossing analysis and keep the farthest
536 // point in the real axis (ie. the last data in "cuts")
538 std::map< TScalar, unsigned int > cuts;
539 for( unsigned int s = 0; s < _samples; ++s )
541 unsigned int i = ( s + minIdx ) % _samples;
542 if( function[ i ].second * prevA < _0 )
543 cuts[ std::real( contour._Z( function[ i ].first, 0 ) ) ] = i;
544 prevA = function[ i ].second;
548 // 1.3. Get initial guess
550 if( cuts.size( ) > 0 )
552 unsigned int i = cuts.rbegin( )->second;
554 i = function.size( );
555 w0 = function[ i - 1 ].first;
559 // 2. Refine by Newthon-Raphson
562 while( i < _maxIterations && !stop )
564 TComplex c = contour._Z( w0, 0 );
565 TScalar d = std::imag( c * std::conj( contour._Z( w0, 1 ) ) );
568 if( !( std::fabs( d ) < _epsilon ) )
570 TScalar w1 = w0 + ( std::arg( c ) * ( std::norm( c ) / d ) );
571 if( ( std::fabs( w1 - w0 ) / std::fabs( w1 ) ) < _tolerance )
587 // -------------------------------------------------------------------------
588 template< class _TScalar, unsigned int _VDim >
589 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
590 Sample( std::vector< TPoint >& p, const unsigned long& s ) const
592 static const TScalar _2pi = TScalar( 2 ) * TScalar( vnl_math::pi );
593 TScalar off = _2pi / TScalar( s - 1 );
594 for( unsigned int w = 0; w < s; ++w )
595 p.push_back( ( *this )( TScalar( w ) * off ) );
598 // -------------------------------------------------------------------------
599 template< class _TScalar, unsigned int _VDim >
600 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
601 GetEllipse( int l, TScalar& a, TScalar& b, TScalar& t, TScalar& p ) const
603 a = b = t = p = TScalar( 0 );
604 if( l == 0 || l > this->GetNumberOfHarmonics( ) )
607 TComplex zp = ( *this )[ l ];
608 TComplex zn = ( *this )[ -l ];
609 TScalar np = std::abs( zp );
610 TScalar nn = std::abs( zn );
616 // Rotation and phase
617 if( np > TScalar( 0 ) && nn > TScalar( 0 ) )
621 t = std::real( std::log( zp * zn ) / TComplex( TScalar( 0 ), TScalar( 2 ) ) );
622 p = std::real( std::log( zp / zn ) / TComplex( TScalar( 0 ), TScalar( 2 * l ) ) );
627 p = ( np > TScalar( 0 ) )? std::arg( zp ): std::arg( zn );
632 // -------------------------------------------------------------------------
633 template< class _TScalar, unsigned int _VDim >
634 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
635 SetEllipse( int l, TScalar& a, TScalar& b, TScalar& t, TScalar& p )
637 TScalar nzp = ( a + b ) / TScalar( 2 );
638 TScalar nzn = ( a - b ) / TScalar( 2 );
639 TComplex et = std::polar( TScalar( 1 ), t );
640 TComplex ep = std::polar( TScalar( 1 ), TScalar( l ) * p );
641 ( *this )[ l ] = et * ep * nzp;
642 ( *this )[ -l ] = et * std::conj( ep ) * nzn;
645 // -------------------------------------------------------------------------
646 template< class _TScalar, unsigned int _VDim >
647 typename ivq::ITK::FourierSeries< _TScalar, _VDim >::
648 TComplex ivq::ITK::FourierSeries< _TScalar, _VDim >::
649 _Z( const TScalar& w, const unsigned int& n ) const
651 static const TScalar _0 = TScalar( 0 );
652 static const TScalar _1 = TScalar( 1 );
653 TScalar _n = TScalar( n );
655 TComplex z( _0, _0 );
656 int q = this->GetNumberOfHarmonics( );
657 for( int l = -q; l <= q; ++l )
659 TComplex v = ( *this )[ l ] * std::polar( _1, w * TScalar( l ) );
661 v *= std::pow( TComplex( _0, TScalar( l ) ), _n );
668 // -------------------------------------------------------------------------
669 template< class _TScalar, unsigned int _VDim >
670 void ivq::ITK::FourierSeries< _TScalar, _VDim >::
671 _DFT( const std::vector< TComplex >& p, unsigned int q )
673 // Basic values and configuration
674 static const TScalar _0 = TScalar( 0 );
675 static const TScalar _1 = TScalar( 1 );
676 static const TScalar _2pi = TScalar( 2 ) * TScalar( vnl_math::pi );
677 this->SetNumberOfHarmonics( q );
678 *this *= TScalar( 0 );
681 TScalar N = TScalar( p.size( ) );
684 TComplex c( _0, _0 );
685 for( long m = 0; m < p.size( ); ++m )
692 bool operator()( const TComplex& a, const TComplex& b )
695 ( std::real( b ) < std::real( a ) ) &&
696 ( std::fabs( std::imag( a ) ) < std::fabs( std::imag( b ) ) )
700 std::map< TComplex, long, _TComplexCmp > ordered;
701 for( long m = 0; m < p.size( ); ++m )
702 ordered[ p[ m ] - c ] = m;
703 long si = ordered.begin( )->second;
705 // Real DFT computation
706 std::vector< TComplex > dft;
708 TScalar _2piN = -_2pi / N;
709 for( long m = 1; m < p.size( ); ++m )
711 TComplex z( _0, _0 );
712 for( long i = 0; i < p.size( ); ++i )
714 p[ ( i + si ) % p.size( ) ] *
715 std::polar( _1, _2piN * TScalar( m * i ) );
721 // Assign clamped coefficients
722 ( *this )[ 0 ] = dft[ 0 ];
723 for( int x = 1; x <= q; ++x )
725 ( *this )[ x ] = dft[ x ];
726 ( *this )[ -x ] = dft[ p.size( ) - x ];
731 // -------------------------------------------------------------------------
732 template class IVQ_EXPORT ivq::ITK::FourierSeries< float, 2 >;
733 template class IVQ_EXPORT ivq::ITK::FourierSeries< float, 3 >;
734 template class IVQ_EXPORT ivq::ITK::FourierSeries< double, 2 >;
735 template class IVQ_EXPORT ivq::ITK::FourierSeries< double, 3 >;