From 2f6a2a85f37e301cf5b680a63904b6468caa6b12 Mon Sep 17 00:00:00 2001 From: Leonardo Florez-Valencia Date: Mon, 24 Aug 2015 19:21:22 -0500 Subject: [PATCH] Kalman completely ported --- .../Extensions/Algorithms/IsoImageSlicer.h | 17 ++-- .../Algorithms/KalmanConstantFilter.cxx | 51 ++++++++++ .../Algorithms/KalmanConstantFilter.h | 60 ++++++++++++ .../Extensions/Algorithms/KalmanFilter.cxx | 31 ++++--- .../Extensions/Algorithms/KalmanFilter.h | 45 +++++---- .../Algorithms/KalmanVelocityFilter.cxx | 92 +++++++++++++++++++ .../Algorithms/KalmanVelocityFilter.h | 70 ++++++++++++++ 7 files changed, 320 insertions(+), 46 deletions(-) create mode 100644 lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.cxx create mode 100644 lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.h create mode 100644 lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.cxx create mode 100644 lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.h diff --git a/lib/cpPlugins/Extensions/Algorithms/IsoImageSlicer.h b/lib/cpPlugins/Extensions/Algorithms/IsoImageSlicer.h index cda4130..e623670 100644 --- a/lib/cpPlugins/Extensions/Algorithms/IsoImageSlicer.h +++ b/lib/cpPlugins/Extensions/Algorithms/IsoImageSlicer.h @@ -167,15 +167,16 @@ namespace cpPlugins namespace Algorithms { CPPLUGINS_DEFINE_ISOIMAGESLICER( - IsoImageSlicer, - itk::ResampleImageFilter, - itk::InterpolateImageFunction - ); + IsoImageSlicer, + itk::ResampleImageFilter, + itk::InterpolateImageFunction + ); CPPLUGINS_DEFINE_ISOIMAGESLICER( - VectorIsoImageSlicer, - itk::VectorResampleImageFilter, - itk::VectorInterpolateImageFunction - ); + VectorIsoImageSlicer, + itk::VectorResampleImageFilter, + itk::VectorInterpolateImageFunction + ); + } // ecapseman } // ecapseman diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.cxx b/lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.cxx new file mode 100644 index 0000000..219aff4 --- /dev/null +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.cxx @@ -0,0 +1,51 @@ +// ------------------------------------------------------------------------- +// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co) +// ------------------------------------------------------------------------- + +#include + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanConstantFilter< T >:: +Configure( unsigned int m ) +{ + this->Superclass::Configure( m, 1, m ); +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanConstantFilter< T >:: +Initialize( ) +{ + this->Superclass::Initialize( ); + + this->m_A.set_identity( ); + this->m_B.fill( TScalar( 0 ) ); + this->m_H.set_identity( ); +} + +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanConstantFilter< T >:: +KalmanConstantFilter( ) + : Superclass( ) +{ + this->Configure( 1 ); +} + +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanConstantFilter< T >:: +~KalmanConstantFilter( ) +{ +} + +// ------------------------------------------------------------------------- +// Explicit instantiations + +using namespace cpPlugins::Extensions::Algorithms; + +template class KalmanConstantFilter< float >; +template class KalmanConstantFilter< double >; + +// eof - $RCSfile$ diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.h b/lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.h new file mode 100644 index 0000000..b8c61bc --- /dev/null +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanConstantFilter.h @@ -0,0 +1,60 @@ +// ------------------------------------------------------------------------- +// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co) +// ------------------------------------------------------------------------- + +#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANCONSTANTFILTER__H__ +#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANCONSTANTFILTER__H__ + +#include + +namespace cpPlugins +{ + namespace Extensions + { + namespace Algorithms + { + /** + */ + template< typename T > + class KalmanConstantFilter + : public KalmanFilter< T > + { + public: + typedef KalmanConstantFilter Self; + typedef KalmanFilter< T > Superclass; + typedef itk::SmartPointer< Self > Pointer; + typedef itk::SmartPointer< const Self > ConstPointer; + + typedef typename Superclass::TScalar TScalar; + typedef typename Superclass::TMatrix TMatrix; + typedef typename Superclass::TVector TVector; + + public: + itkNewMacro( Self ); + itkTypeMacro( KalmanConstantFilter, KalmanFilter ); + + public: + void Configure( unsigned int m ); + + /// Iteration methods + virtual void Initialize( ); + + protected: + KalmanConstantFilter( ); + virtual ~KalmanConstantFilter( ); + + private: + // Purposely not implemented. + KalmanConstantFilter( const Self& ); + void operator=( const Self& ); + }; + + } // ecapseman + + } // ecapseman + +} // ecapseman + +#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANCONSTANTFILTER__H__ + +// eof - $RCSfile$ diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx index 9542026..c7f650d 100644 --- a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx @@ -3,21 +3,6 @@ #include #include -// ------------------------------------------------------------------------- -template< typename T > -cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: -KalmanFilter( unsigned int s, unsigned int i, unsigned int m ) -{ - this->Configure( s, i, m ); -} - -// ------------------------------------------------------------------------- -template< typename T > -cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: -~KalmanFilter( ) -{ -} - // ------------------------------------------------------------------------- template< typename T > void cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: @@ -127,6 +112,22 @@ Filtrate( ) } // fi } +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +KalmanFilter( ) + : Superclass( ) +{ + this->Configure( 1, 1, 1 ); +} + +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +~KalmanFilter( ) +{ +} + // ------------------------------------------------------------------------- // Explicit instantiations diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h index ac5d9e2..74b0f3a 100644 --- a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h @@ -5,23 +5,15 @@ #ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__ #define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__ +#include +#include #include #include // ------------------------------------------------------------------------- -#define kalmanGetMacro( type, name ) \ - virtual type Get##name( ) const \ - { return( this->m_##name ); } - -// ------------------------------------------------------------------------- -#define kalmanSetMacro( type, name ) \ - virtual void Set##name( const type& m ) \ - { this->m_##name = m; } - -// ------------------------------------------------------------------------- -#define kalmanGetSetMacro( type, name ) \ - kalmanGetMacro( type, name ); \ - kalmanSetMacro( type, name ); +#define kalmanGetSetMacro( type, name ) \ + itkGetConstMacro( name, type ); \ + itkSetMacro( name, type ); // ------------------------------------------------------------------------- #define kalmanGetMatrixMacro( var, name ) \ @@ -31,7 +23,7 @@ // ------------------------------------------------------------------------- #define kalmanSetMatrixMacro( var, name ) \ virtual void Set##name( const TMatrix& m ) \ - { this->m_##var = m; } + { this->Set##var( m ); } // ------------------------------------------------------------------------- #define kalmanGetSetMatrixMacro( var, name ) \ @@ -46,7 +38,7 @@ // ------------------------------------------------------------------------- #define kalmanSetVectorMacro( var, name ) \ virtual void Set##name( const TVector& v ) \ - { this->m_##var = v; } + { this->Set##var( v ); } // ------------------------------------------------------------------------- #define kalmanGetSetVectorMacro( var, name ) \ @@ -66,9 +58,13 @@ namespace cpPlugins */ template< typename T > class KalmanFilter + : public itk::Object { public: - typedef KalmanFilter Self; + typedef KalmanFilter Self; + typedef itk::Object Superclass; + typedef itk::SmartPointer< Self > Pointer; + typedef itk::SmartPointer< const Self > ConstPointer; // Template parameters types typedef T TScalar; @@ -86,10 +82,13 @@ namespace cpPlugins }; public: + itkNewMacro( Self ); + itkTypeMacro( KalmanFilter, itkObject ); + // Values - kalmanGetMacro( unsigned int, StateSize ); - kalmanGetMacro( unsigned int, InputSize ); - kalmanGetMacro( unsigned int, MeasureSize ); + itkGetConstMacro( StateSize, unsigned int ); + itkGetConstMacro( InputSize, unsigned int ); + itkGetConstMacro( MeasureSize, unsigned int ); // Matrices kalmanGetSetMacro( TMatrix, A ); @@ -128,10 +127,6 @@ namespace cpPlugins kalmanGetSetVectorMacro( xp, APosterioriState ); public: - KalmanFilter( - unsigned int s = 1, unsigned int i = 1, unsigned int m = 1 - ); - virtual ~KalmanFilter( ); void Configure( unsigned int s, unsigned int i, unsigned int m ); @@ -156,6 +151,10 @@ namespace cpPlugins unsigned char CurrentStep( ) const { return( this->m_Step ); } + protected: + KalmanFilter( ); + virtual ~KalmanFilter( ); + private: // Purposely not implemented KalmanFilter( const Self& ); diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.cxx b/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.cxx new file mode 100644 index 0000000..6ec3295 --- /dev/null +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.cxx @@ -0,0 +1,92 @@ +// ------------------------------------------------------------------------- +// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co) +// ------------------------------------------------------------------------- + +#include +#include + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >:: +Configure( unsigned int m ) +{ + this->Superclass::Configure( m << 1, 1, m ); + + this->m_TimeOffset.set_size( 2, 2 ); + this->SetTimeOffset( TScalar( 1 ) ); + + this->m_Sigma.set_size( this->m_MeasureSize, this->m_MeasureSize ); + this->m_Sigma.set_identity( ); +} + +// ------------------------------------------------------------------------- +template< typename T > +typename cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >:: +TScalar cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >:: +GetTimeOffset( ) const +{ + return( TScalar( std::sqrt( double( this->m_TimeOffset[ 1 ][ 1 ] ) ) ) ); +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >:: +SetTimeOffset( TScalar t ) +{ + TScalar t2 = t * t; + TScalar t3 = t2 * t; + TScalar t4 = t3 * t; + this->m_TimeOffset[ 0 ][ 0 ] = t4 / TScalar( 4 ); + this->m_TimeOffset[ 1 ][ 1 ] = t2; + + this->m_TimeOffset[ 0 ][ 1 ] = t3 / TScalar( 2 ); + this->m_TimeOffset[ 1 ][ 0 ] = this->m_TimeOffset[ 0 ][ 1 ]; +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >:: +Initialize( ) +{ + this->Superclass::Initialize( ); + + Self::Kronecker( this->m_Q, this->m_Sigma, this->m_TimeOffset ); + + this->m_A.set_identity( ); + this->m_B.fill( TScalar( 0 ) ); + this->m_H.fill( TScalar( 0 ) ); + + TScalar tOff = this->GetTimeOffset( ); + for( unsigned int i = 0; i < this->m_MeasureSize; i++ ) + { + this->m_A[ i ][ this->m_MeasureSize + i ] = tOff; + this->m_H[ i ][ i ] = TScalar( 1 ); + + } // rof +} + +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >:: +KalmanVelocityFilter( ) + : Superclass( ) +{ + this->Configure( 1 ); +} + +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >:: +~KalmanVelocityFilter( ) +{ +} + +// ------------------------------------------------------------------------- +// Explicit instantiations + +using namespace cpPlugins::Extensions::Algorithms; + +template class KalmanVelocityFilter< float >; +template class KalmanVelocityFilter< double >; + +// eof - $RCSfile$ diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.h b/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.h new file mode 100644 index 0000000..0454ec3 --- /dev/null +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.h @@ -0,0 +1,70 @@ +// ------------------------------------------------------------------------- +// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co) +// ------------------------------------------------------------------------- + +#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANVELOCITYFILTER__H__ +#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANVELOCITYFILTER__H__ + +#include + +namespace cpPlugins +{ + namespace Extensions + { + namespace Algorithms + { + /** + */ + template< typename T > + class KalmanVelocityFilter + : public KalmanFilter< T > + { + public: + typedef KalmanVelocityFilter Self; + typedef KalmanFilter< T > Superclass; + typedef itk::SmartPointer< Self > Pointer; + typedef itk::SmartPointer< const Self > ConstPointer; + + typedef typename Superclass::TScalar TScalar; + typedef typename Superclass::TMatrix TMatrix; + typedef typename Superclass::TVector TVector; + + public: + itkNewMacro( Self ); + itkTypeMacro( KalmanVelocityFilter, KalmanFilter ); + + kalmanGetSetMacro( TMatrix, Sigma ); + kalmanGetSetMatrixMacro( Sigma, AccelerationNoise ); + + public: + void Configure( unsigned int m ); + + TScalar GetTimeOffset( ) const; + void SetTimeOffset( TScalar t ); + + /// Iteration methods + virtual void Initialize( ); + + protected: + KalmanVelocityFilter( ); + virtual ~KalmanVelocityFilter( ); + + private: + // Purposely not implemented. + KalmanVelocityFilter( const Self& ); + void operator=( const Self& ); + + protected: + TMatrix m_TimeOffset; + TMatrix m_Sigma; + }; + + } // ecapseman + + } // ecapseman + +} // ecapseman + +#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANVELOCITYFILTER__H__ + +// eof - $RCSfile$ -- 2.47.1