00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef _QR_DECOMPOSITION_H
00020 #define _QR_DECOMPOSITION_H
00021
00022 #include <lsp/givens_rotation.h>
00023
00024 #include <limits>
00025
00026 #include <boost/numeric/ublas/matrix.hpp>
00027 #include <boost/numeric/ublas/matrix_proxy.hpp>
00028 #include <boost/numeric/ublas/storage.hpp>
00029
00030 using namespace boost::numeric::ublas;
00031
00032 namespace lsp{
00033
00034 namespace {
00035 template<class T> static T shift( T q2, T q1, T e2, T e1 ) {
00036 typedef T value_type;
00037 const value_type f = ( q2*q2 - q1*q1 + e2*e2 - e1*e1 ) / ( 2*e2*q1 );
00038 const value_type t = ( f < 0 ?
00039 - f + std::pow(( value_type(1)+f*f ),0.5) :
00040 - f - std::pow(( value_type(1)+f*f ),0.5) );
00041 return ( q2*q2 + e2*(e2 - q1/t) );
00042 };
00043 };
00044
00054 template<class T> class qr_decomposition {
00055 public:
00056 typedef T matrix_type;
00057 typedef typename matrix_type::value_type value_type;
00058 typedef typename matrix_type::size_type size_type;
00059 typedef matrix_vector_slice< matrix_type > diagonal_type;
00060 private:
00061 struct regular_tag {};
00062 struct left_tag {};
00063 struct right_tag {};
00064 private:
00065 matrix_type& m_matrix;
00066 mutable diagonal_type m_super;
00067 mutable diagonal_type m_leading;
00068 private:
00069
00070 template<class M1, class M2> void apply( M1& left, M2& right, const range& cell, const left_tag& ) const {
00071 typedef givens_rotation< value_type > givens_rotation_type;
00072 value_type z;
00073
00074 z = m_super( cell(0) );
00075 m_super( cell(0) ) = 0;
00076
00077 for( range::const_iterator it = cell.begin() + 1; it != cell.end() ; ++it ) {
00078 givens_rotation_type gr( m_leading(*it), z );
00079
00080 gr.apply( row(left, *it), row(left, cell(0)) );
00081 if( *it == cell( cell.size() - 1 ) )
00082 break;
00083 gr.apply( m_super(*it), z );
00084 }
00085 }
00086
00087 template<class M1, class M2> void apply( M1& left, M2& right, const range& cell, const right_tag& ) const {
00088 typedef givens_rotation< value_type > givens_rotation_type;
00089 value_type z;
00090
00091 z = m_super( cell( cell.size() - 2 ) );
00092 m_super( cell( cell.size() - 2 ) ) = 0;
00093
00094 for( range::const_reverse_iterator it = cell.rbegin() + 1; it != cell.rend(); ++it ) {
00095 givens_rotation_type gr( m_leading(*it), z );
00096
00097 gr.apply( column( right, *it ), column( right, cell( cell.size() - 1 ) ) );
00098 if( *it == cell.start() )
00099 break;
00100 gr.apply( m_super(*it-1), z );
00101 }
00102 }
00103
00104 template<class M1, class M2> void apply( M1& left, M2& right, const range& cell, const regular_tag& ) const {
00105 typedef givens_rotation< value_type > givens_rotation_type;
00106
00107 const value_type lim = std::numeric_limits< value_type >::epsilon() * norm_frobenius( m_matrix ) / m_matrix.size2();
00108
00109 for( range::const_reverse_iterator it = cell.rbegin() + 1; it != cell.rend(); ++it ) {
00110 while( std::abs( m_super( *it ) ) > lim ) {
00111
00112 value_type en1 = ( *it != cell(0) ? m_super(*it - 1) : value_type(0) );
00113 value_type e0 = m_leading( cell(0) ) - shift( m_leading(*it + 1), m_leading(*it), m_super(*it), en1 ) / m_leading( cell(0) );
00114 value_type z = m_super( cell(0) );
00115
00116 givens_rotation_type gr_left( e0, z );
00117 for( range::const_iterator it2 = cell.begin() + 1; *it2 != *it + 2; ++it2 ) {
00118
00119 gr_left.apply( m_leading(*it2-1), m_super(*it2-1) );
00120 gr_left.apply( column(right,*it2-1), column(right,*it2) );
00121
00122 z = gr_left.s() * m_leading(*it2);
00123 m_leading(*it2) = gr_left.c() * m_leading(*it2);
00124
00125 givens_rotation_type gr_right( m_leading(*it2-1), z );
00126 gr_right.apply( m_super(*it2-1), m_leading(*it2) );
00127 gr_right.apply( row(left,*it2-1), row(left,*it2) );
00128
00129 if( *it2 == *it + 1 ) break;
00130 z = gr_right.s() * m_super(*it2);
00131 m_super(*it2) = gr_right.c() * m_super(*it2);
00132
00133 gr_left = givens_rotation_type( m_super(*it2-1), z );
00134 }
00135 }
00136 m_super( *it ) = 0;
00137 }
00138 }
00139
00140 template<class M1, class M2> void apply( M1& left, M2& right, const range& cell ) const {
00141
00142 if( cell.size() == 1 )
00143 return ;
00144
00145 const value_type lim = 6 * std::numeric_limits< value_type >::epsilon() * norm_frobenius( m_matrix );
00146
00147
00148 for( range::const_reverse_iterator it = cell.rbegin() + 1; it != cell.rend() ; ++it ) {
00149 if( m_leading( *it ) == 0 ) {
00150 apply( left, right, range( *it, cell.start() + cell.size() ), left_tag() );
00151 for( range::const_reverse_iterator it2 = cell.rbegin(); it2 != it; ++it2 ){
00152 if( std::abs( m_leading( *it2 ) ) < lim ) m_leading( *it2 ) = 0;
00153 }
00154 for( range::const_reverse_iterator it2 = cell.rbegin() + 1; it2 != it; ++it2 ){
00155 if( std::abs( m_super( *it2 ) ) < lim ) m_super( *it2 ) = 0;
00156 }
00157 apply( left, right, range( cell.start(), *it+1 ) );
00158 apply( left, right, range( *it+1, cell.start() + cell.size() ) );
00159 return ;
00160 }
00161 }
00162
00163 if( m_leading( cell( cell.size() - 1 ) ) == 0 ) {
00164 apply( left, right, cell, right_tag() );
00165 for( range::const_reverse_iterator it2 = cell.rbegin(); it2 != cell.rend(); ++it2 ){
00166 if( std::abs( m_leading( *it2 ) ) < lim ) m_leading( *it2 ) = 0;
00167 }
00168 for( range::const_reverse_iterator it2 = cell.rbegin() + 1; it2 != cell.rend(); ++it2 ){
00169 if( std::abs( m_super( *it2 ) ) < lim ) m_super( *it2 ) = 0;
00170 }
00171 apply( left, right, range( cell.start(), cell(cell.size()-1) ) );
00172 return ;
00173 }
00174
00175
00176 for( range::const_reverse_iterator it = cell.rbegin() + 1; it != cell.rend(); ++it ) {
00177 if( m_super( *it ) == 0 ) {
00178 apply( left, right, range( cell.start(), *it + 1 ) );
00179 apply( left, right, range( *it + 1, cell.start() + cell.size() ) );
00180 return ;
00181 }
00182 }
00183
00184 apply( left, right, cell, regular_tag() );
00185
00186 }
00187
00188 public:
00194 qr_decomposition( matrix_type& matrix ):
00195 m_matrix( matrix ),
00196 m_super( matrix, slice(0, 1, matrix.size2() - 1), slice(1, 1, matrix.size2() - 1) ),
00197 m_leading( matrix, slice(0, 1, matrix.size2()), slice(0, 1, matrix.size2()) ) {
00198
00199 assert( matrix.upper() == 1 && matrix.lower() == 0 );
00200 }
00201
00217 template<class M1, class M2> void apply( M1& left, M2& right ) const {
00218 apply( left, right, range(0, std::min( m_matrix.size1(), m_matrix.size2() ) ) );
00219 }
00220
00221 };
00222
00223 };
00224
00225 #endif // _QR_DECOMPOSITION_H
00226
