Changeset 70 in pacpusframework for trunk/include/Pacpus/PacpusTools/math/ublas.hpp


Ignore:
Timestamp:
Jan 10, 2013, 11:24:23 AM (12 years ago)
Author:
Marek Kurdej
Message:

Added: more documentation.

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/include/Pacpus/PacpusTools/math/ublas.hpp

    r66 r70  
    2727
    2828namespace math {
    29 
    30    namespace ublas {
    31 
    32    /*!
    33     *\fn typedef boost::numeric::ublas::matrix<double> Matrix
    34     * \brief Definition of a matrix using double precision
    35     */
    36    typedef boost::numeric::ublas::matrix<double> Matrix;
    37    /*!
    38     *\fn typedef boost::numeric::ublas::vector<double> Vector
    39     * \brief Definition of a vector using double precision
    40     */
    41    typedef boost::numeric::ublas::vector<double> Vector;
    42    /*!
    43     *\fn typedef boost::numeric::ublas::zero_vector<double> ZeroVector;
    44     * \brief Definition of empty vector using double precision
    45     */
    46    typedef boost::numeric::ublas::zero_vector<double> ZeroVector;
    47    /*!
    48     *\fn   typedef boost::numeric::ublas::zero_matrix<double> ZeroMatrix;
    49     * \brief Definition of empty matrix using double precision
    50     */
    51    typedef boost::numeric::ublas::zero_matrix<double> ZeroMatrix;
    52 
    53    /*!
    54     * \fn inline boost::numeric::ublas::matrix<T> operator *(const boost::numeric::ublas::matrix<T> & m1, const boost::numeric::ublas::matrix<T> & m2)
    55     * \brief multiplication of two matrices
    56     * \param m1 : ublas matrix
    57     * \param m2 : ublas matrix
    58     * \return  ublas matrix
    59     */
    60    template<class T> inline boost::numeric::ublas::matrix<T> operator *(const boost::numeric::ublas::matrix<T> & m1, const boost::numeric::ublas::matrix<T> & m2)
    61    {
     29namespace ublas {
     30
     31/// Definition of a matrix using double precision
     32typedef boost::numeric::ublas::matrix<double> Matrix;
     33/// Definition of a vector using double precision
     34typedef boost::numeric::ublas::vector<double> Vector;
     35/// Definition of empty vector using double precision
     36typedef boost::numeric::ublas::zero_vector<double> ZeroVector;
     37/// Definition of empty matrix using double precision
     38typedef boost::numeric::ublas::zero_matrix<double> ZeroMatrix;
     39
     40/// Multiplication of two matrices.
     41///
     42/// @tparam T @todo Documentation
     43/// @param m1 ublas matrix
     44/// @param m2 ublas matrix
     45/// @returns ublas matrix
     46template<class T>
     47inline boost::numeric::ublas::matrix<T> operator *(const boost::numeric::ublas::matrix<T> & m1, const boost::numeric::ublas::matrix<T> & m2)
     48{
    6249    return prod(m1,m2);
    63    }
    64 
    65    /*!
    66     * \fn inline boost::numeric::ublas::vector<T> operator *(const boost::numeric::ublas::matrix<T> & m, const boost::numeric::ublas::vector<T> & v)
    67     * \brief product of a vector by a matrix
    68     * \param m : ublas matrix
    69     * \param v : ublas vector
    70     * \return ublas vector
    71     */
    72    template<class T> inline boost::numeric::ublas::vector<T> operator *(const boost::numeric::ublas::matrix<T> & m, const boost::numeric::ublas::vector<T> & v)
    73    {
     50}
     51
     52/// product of a vector by a matrix
     53///
     54/// @tparam T @todo Documentation
     55/// @param m ublas matrix
     56/// @param v ublas vector
     57/// @returns ublas vector
     58template<class T>
     59inline boost::numeric::ublas::vector<T> operator *(const boost::numeric::ublas::matrix<T> & m, const boost::numeric::ublas::vector<T> & v)
     60{
    7461    return prod(m,v);
    75    }
    76 
    77    /*!
    78     * \fn inline boost::numeric::ublas::vector<T> operator +(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
    79     * \brief addition of two vectors
    80     * \param v1 : ublas vector
    81     * \param v2 : ublas vector
    82     * \return  ublas vector
    83     */
    84    template<class T> inline boost::numeric::ublas::vector<T> operator +(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
    85    {
    86      boost::numeric::ublas::vector<T> tmp = v1;
    87      return tmp+=v2;
    88    }
    89 
    90    /*!
    91     * \fn  inline boost::numeric::ublas::vector<T> operator -(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
    92     * \brief subtraction of two vectors
    93     * \param v1 : ublas vector
    94     * \param v2 : ublas vector
    95     * \return  ublas vector
    96     */
    97    template<class T> inline boost::numeric::ublas::vector<T> operator -(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
    98    {
    99      boost::numeric::ublas::vector<T> tmp = v1;
    100      return tmp-=v2;
    101    }
    102 
    103    /*!
    104     * \fn inline boost::numeric::ublas::matrix<T>Trans(boost::numeric::ublas::matrix<T> &m)
    105     * \brief transpose a matrix
    106     * \param m : ublas matrix
    107     * \return  ublas matrix
    108     */
    109    template<class T> inline boost::numeric::ublas::matrix<T>Trans(boost::numeric::ublas::matrix<T> &m)
    110    {
    111      return trans(m);
    112    }
    113 
    114    /*!
    115     * \fn  inline boost::numeric::ublas::matrix<T> vector2matrix(const boost::numeric::ublas::vector<T> &v)
    116     * \brief convert a vector to a matrix
    117     * \param v : ublas vector
    118     * \return  ublas matrix
    119     */
    120    template <class T> inline boost::numeric::ublas::matrix<T> vector2matrix(const boost::numeric::ublas::vector<T> &v)
    121    {
     62}
     63
     64/// addition of two vectors
     65///
     66/// @tparam T @todo Documentation
     67/// @param v1 ublas vector
     68/// @param v2 ublas vector
     69/// @returns ublas vector
     70template<class T>
     71inline boost::numeric::ublas::vector<T> operator +(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
     72{
     73    boost::numeric::ublas::vector<T> tmp = v1;
     74    return tmp+=v2;
     75}
     76
     77/// subtraction of two vectors
     78///
     79/// @tparam T @todo Documentation
     80/// @param v1 ublas vector
     81/// @param v2 ublas vector
     82/// @returns ublas vector
     83template<class T>
     84inline boost::numeric::ublas::vector<T> operator -(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
     85{
     86    boost::numeric::ublas::vector<T> tmp = v1;
     87    return tmp-=v2;
     88}
     89
     90/// Transposes a matrix
     91///
     92/// @tparam T @todo Documentation
     93/// @param m ublas matrix
     94/// @returns ublas matrix
     95template<class T>
     96inline boost::numeric::ublas::matrix<T> Trans(boost::numeric::ublas::matrix<T> &m)
     97{
     98    return trans(m);
     99}
     100
     101/// Converts a vector to a matrix.
     102///
     103/// @param v ublas vector
     104/// @returns ublas matrix
     105template <class T>
     106inline boost::numeric::ublas::matrix<T> vector2matrix(const boost::numeric::ublas::vector<T> &v)
     107{
    122108    boost::numeric::ublas::matrix<T> tmp(v.size(),1);
    123     for(size_t i=0;i<v.size();i++) tmp(i,0)=v[i];
     109    for(size_t i=0;i<v.size();i++) {
     110        tmp(i,0)=v[i];
     111    }
    124112    return tmp;
    125    }
    126 
    127 
    128    /*!
    129     * \fn inline double Norm(const boost::numeric::ublas::vector<T> & v)
    130     * \brief compute the norm of a vector
    131     * \param v : ublas vector
    132     * \return  norm value
    133     */
    134    template <class T> inline double Norm(const boost::numeric::ublas::vector<T> & v){
     113}
     114
     115/// compute the norm of a vector
     116///
     117/// @param v ublas vector
     118/// @returns norm value
     119template <class T>
     120inline double Norm(const boost::numeric::ublas::vector<T> & v){
    135121    double norm =0;
    136     for(typename boost::numeric::ublas::vector<T>::const_iterator I=v.begin();I!=v.end();I++) norm+=(*I)*(*I);
     122    for(typename boost::numeric::ublas::vector<T>::const_iterator I=v.begin(); I != v.end(); ++I) {
     123        norm+=(*I)*(*I);
     124    }
    137125    return std::sqrt(norm);
    138    }
    139 
    140 
    141     /*!
    142     * \fn inline boost::numeric::ublas::vector<T> Mult(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
    143     * \brief term by term multiplication of two vectors
    144     * \param v1 : ublas vector
    145     * \param v2 : ublas vector
    146     * \return  ublas vector
    147     */
    148    template <class T> inline boost::numeric::ublas::vector<T> Mult(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2){
    149      if(v1.size()!=v2.size()) throw math_error("Dot(v1,v2) : vectors must have the same size");
    150      boost::numeric::ublas::vector<T> v(v1.size());
    151      for(size_t i=0;i<v1.size();i++) v[i]=v1[i]*v2[i];
    152      return v;
    153    }
    154 
    155     /*!
    156     * \fn inline double Dot(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
    157     * \brief dot product
    158     * \param v1 : ublas vector
    159     * \param v2 : ublas vector
    160     * \return  dot value
    161     */
    162    template <class T> inline double Dot(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2){
    163      if(v1.size()!=v2.size()) throw math_error("Dot(v1,v2) : vectors must have the same size");
    164      double dot=0;
    165      for(size_t i=0;i<v1.size();i++) dot+=v1[i]*v2[i];
    166      return dot;
    167    }
    168 
    169 
    170    /*!
    171     * \fn  inline Matrix Inv(const Matrix &m)
    172     * \brief matrix inversion using LU decomposition
    173     * \param m : ublas matrix
    174     * \return  ubls matrix
    175     */
    176    inline Matrix InvLU(const Matrix &m) throw(math_error){
    177         using namespace boost::numeric::ublas;
    178         typedef permutation_matrix<std::size_t> pmatrix;
    179 
    180         if(m.size1() != m.size2()) throw math_error("Inv(m): matrix must be square");
    181 
    182         // create a working copy of the input
    183         Matrix A(m);
    184         // create a permutation matrix for the LU-factorization
    185         pmatrix pm(A.size1());
    186         // perform LU-factorization
    187         int res = lu_factorize(A,pm);
    188         if( res != 0 )  throw math_error("Inv(m) : singular matrix");
    189         // create identity matrix of "inverse"
    190         Matrix inverse = identity_matrix<double>(A.size1());
    191         // backsubstitute to get the inverse
    192         lu_substitute(A, pm, inverse);
    193 
    194         return inverse;
    195   }
    196 
    197 
    198 
     126}
     127
     128/// term by term multiplication of two vectors
     129///
     130/// @param v1 : ublas vector
     131/// @param v2 : ublas vector
     132/// @returns ublas vector
     133template <class T>
     134inline boost::numeric::ublas::vector<T> Mult(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
     135{
     136    if(v1.size()!=v2.size()) {
     137        throw math_error("Dot(v1,v2) : vectors must have the same size");
     138    }
     139    boost::numeric::ublas::vector<T> v(v1.size());
     140    for(size_t i=0;i<v1.size();i++) {
     141        v[i]=v1[i]*v2[i];
     142    }
     143    return v;
     144}
     145
     146/// dot product
     147///
     148/// @param v1 ublas vector
     149/// @param v2 ublas vector
     150/// @returns dot product value
     151template <class T>
     152inline double Dot(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
     153{
     154    if(v1.size()!=v2.size()) {
     155        throw math_error("Dot(v1,v2) : vectors must have the same size");
     156    }
     157    double dot=0;
     158    for(size_t i=0;i<v1.size();i++) {
     159        dot+=v1[i]*v2[i];
     160    }
     161    return dot;
     162}
     163
     164/// matrix inversion using LU decomposition
     165/// @param m ublas matrix
     166/// @returns ublas matrix
     167inline Matrix InvLU(const Matrix &m)
     168    throw(math_error)
     169{
     170    using namespace boost::numeric::ublas;
     171    typedef permutation_matrix<std::size_t> pmatrix;
     172
     173    if(m.size1() != m.size2()) throw math_error("Inv(m): matrix must be square");
     174
     175    // create a working copy of the input
     176    Matrix A(m);
     177    // create a permutation matrix for the LU-factorization
     178    pmatrix pm(A.size1());
     179    // perform LU-factorization
     180    int res = lu_factorize(A,pm);
     181    if( res != 0 )  throw math_error("Inv(m) : singular matrix");
     182    // create identity matrix of "inverse"
     183    Matrix inverse = identity_matrix<double>(A.size1());
     184    // backsubstitute to get the inverse
     185    lu_substitute(A, pm, inverse);
     186
     187    return inverse;
     188}
    199189
    200190////////////////////////////////////////////////////////////////////////////////
    … …  
    202192///////////////////////////////////////////////////////////////////////////////
    203193
    204  template<class T>
    205  bool InvertMatrix (const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse) {
    206         using namespace boost::numeric::ublas;
    207         typedef permutation_matrix<std::size_t> pmatrix;
    208         // create a working copy of the input
    209         matrix<T> A(input);
    210         // create a permutation matrix for the LU-factorization
    211         pmatrix pm(A.size1());
    212 
    213         // perform LU-factorization
    214         int res = lu_factorize(A,pm);
    215         if( res != 0 ) return false;
    216 
    217         // create identity matrix of "inverse"
    218         inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));
    219 
    220         // backsubstitute to get the inverse
    221         lu_substitute(A, pm, inverse);
    222 
    223         return true;
    224  }
    225 
    226 template<class T>
    227  void TransposeMultiply (const boost::numeric::ublas::vector<T>& vector,
    228                          boost::numeric::ublas::matrix<T>& result,
    229                          size_t size)
    230  {
    231   result.resize (size,size);
    232   result.clear ();
    233   for(unsigned int row=0; row< vector.size(); ++row)
    234     {
    235       for(unsigned int col=0; col < vector.size(); ++col)
    236         result(row,col) = vector(col) * vector(row);
    237 
    238     }
    239  }
    240 
    241  template<class T>
    242  void HouseholderCornerSubstraction (boost::numeric::ublas::matrix<T>& LeftLarge,
    243                                    const boost::numeric::ublas::matrix<T>& RightSmall)
    244  {
    245   using namespace boost::numeric::ublas;
    246   using namespace std;
    247   if(
    248      !(
    249           (LeftLarge.size1() >= RightSmall.size1())
    250        && (LeftLarge.size2() >= RightSmall.size2())
    251        )
    252      )
    253     {
    254       cerr << "invalid matrix dimensions" << endl;
    255       return;
    256     }
    257 
    258   size_t row_offset = LeftLarge.size2() - RightSmall.size2();
    259   size_t col_offset = LeftLarge.size1() - RightSmall.size1();
    260 
    261   for(unsigned int row = 0; row < RightSmall.size2(); ++row )
    262     for(unsigned int col = 0; col < RightSmall.size1(); ++col )
    263       LeftLarge(col_offset+col,row_offset+row) -= RightSmall(col,row);
    264  }
    265 
    266  template<class T>
    267  void QR (const boost::numeric::ublas::matrix<T>& M,
    268                 boost::numeric::ublas::matrix<T>& Q,
    269                 boost::numeric::ublas::matrix<T>& R)
    270  {
    271   using namespace boost::numeric::ublas;
    272   using namespace std;
    273 
    274   if(
    275      !(
    276            (M.size1() == M.size2())
    277       )
    278     )
    279     {
    280       cerr << "invalid matrix dimensions" << endl;
    281       return;
    282     }
    283   size_t size = M.size1();
    284 
    285   // init Matrices
    286   matrix<T> H, HTemp;
    287   HTemp = identity_matrix<T>(size);
    288   Q = identity_matrix<T>(size);
    289   R = M;
    290 
    291   // find Householder reflection matrices
    292   for(unsigned int col = 0; col < size-1; ++col)
    293     {
    294       // create X vector
    295       boost::numeric::ublas::vector<T> RRowView = boost::numeric::ublas::column(R,col);
    296       vector_range< boost::numeric::ublas::vector<T> > X2 (RRowView, range (col, size));
    297       boost::numeric::ublas::vector<T> X = X2;
    298 
    299       // X -> U~
    300       if(X(0) >= 0)
    301         X(0) += norm_2(X);
    302       else
    303         X(0) += -1*norm_2(X);
    304 
    305       HTemp.resize(X.size(),X.size(),true);
    306 
    307       TransposeMultiply(X, HTemp, X.size());
    308 
    309       // HTemp = the 2UUt part of H
    310       HTemp *= ( 2 / inner_prod(X,X) );
    311 
    312       // H = I - 2UUt
    313       H = identity_matrix<T>(size);
    314       HouseholderCornerSubstraction(H,HTemp);
    315 
    316       // add H to Q and R
    317       Q = prod(Q,H);
    318       R = prod(H,R);
    319     }
    320  }
     194/// @todo Documentation
     195/// @tparam T @todo Documentation
     196template<class T>
     197bool InvertMatrix (const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse)
     198{
     199    using namespace boost::numeric::ublas;
     200    typedef permutation_matrix<std::size_t> pmatrix;
     201    // create a working copy of the input
     202    matrix<T> A(input);
     203    // create a permutation matrix for the LU-factorization
     204    pmatrix pm(A.size1());
     205
     206    // perform LU-factorization
     207    int res = lu_factorize(A,pm);
     208    if( res != 0 ) return false;
     209
     210    // create identity matrix of "inverse"
     211    inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));
     212
     213    // backsubstitute to get the inverse
     214    lu_substitute(A, pm, inverse);
     215
     216    return true;
     217}
     218
     219/// @todo Documentation
     220/// @tparam T @todo Documentation
     221template<class T>
     222void TransposeMultiply(const boost::numeric::ublas::vector<T>& vector,
     223    boost::numeric::ublas::matrix<T>& result,
     224    size_t size)
     225{
     226    result.resize (size,size);
     227    result.clear ();
     228    for(unsigned int row=0; row< vector.size(); ++row)
     229    {
     230        for(unsigned int col=0; col < vector.size(); ++col)
     231            result(row,col) = vector(col) * vector(row);
     232
     233    }
     234}
     235
     236/// @todo Documentation
     237/// @tparam T @todo Documentation
     238template<class T>
     239void HouseholderCornerSubstraction (boost::numeric::ublas::matrix<T>& LeftLarge,
     240    const boost::numeric::ublas::matrix<T>& RightSmall)
     241{
     242    using namespace boost::numeric::ublas;
     243    using namespace std;
     244    if(
     245        !(
     246        (LeftLarge.size1() >= RightSmall.size1())
     247        && (LeftLarge.size2() >= RightSmall.size2())
     248        )
     249        )
     250    {
     251        cerr << "invalid matrix dimensions" << endl;
     252        return;
     253    }
     254
     255    size_t row_offset = LeftLarge.size2() - RightSmall.size2();
     256    size_t col_offset = LeftLarge.size1() - RightSmall.size1();
     257
     258    for(unsigned int row = 0; row < RightSmall.size2(); ++row )
     259        for(unsigned int col = 0; col < RightSmall.size1(); ++col )
     260            LeftLarge(col_offset+col,row_offset+row) -= RightSmall(col,row);
     261}
     262
     263/// @todo Documentation
     264/// @tparam T @todo Documentation
     265template<class T>
     266void QR (const boost::numeric::ublas::matrix<T>& M,
     267    boost::numeric::ublas::matrix<T>& Q,
     268    boost::numeric::ublas::matrix<T>& R)
     269{
     270    using namespace boost::numeric::ublas;
     271    using namespace std;
     272
     273    if(
     274        !(
     275        (M.size1() == M.size2())
     276        )
     277        )
     278    {
     279        cerr << "invalid matrix dimensions" << endl;
     280        return;
     281    }
     282    size_t size = M.size1();
     283
     284    // init Matrices
     285    matrix<T> H, HTemp;
     286    HTemp = identity_matrix<T>(size);
     287    Q = identity_matrix<T>(size);
     288    R = M;
     289
     290    // find Householder reflection matrices
     291    for(unsigned int col = 0; col < size-1; ++col)
     292    {
     293        // create X vector
     294        boost::numeric::ublas::vector<T> RRowView = boost::numeric::ublas::column(R,col);
     295        vector_range< boost::numeric::ublas::vector<T> > X2 (RRowView, range (col, size));
     296        boost::numeric::ublas::vector<T> X = X2;
     297
     298        // X -> U~
     299        if(X(0) >= 0)
     300            X(0) += norm_2(X);
     301        else
     302            X(0) += -1*norm_2(X);
     303
     304        HTemp.resize(X.size(),X.size(),true);
     305
     306        TransposeMultiply(X, HTemp, X.size());
     307
     308        // HTemp = the 2UUt part of H
     309        HTemp *= ( 2 / inner_prod(X,X) );
     310
     311        // H = I - 2UUt
     312        H = identity_matrix<T>(size);
     313        HouseholderCornerSubstraction(H,HTemp);
     314
     315        // add H to Q and R
     316        Q = prod(Q,H);
     317        R = prod(H,R);
     318    }
     319}
    321320//////////////////////////////////////////////////////////////////////////////////////////::
    322321
    323 
    324 
    325 
    326 
    327 
    328    /*!
    329     * \fn  inline Matrix InvQR(const Matrix &m)
    330     * \brief matrix inversion using QR decomposition
    331     * \param m : ublas matrix
    332     * \return  ubls matrix
    333     */
    334    inline Matrix InvQR(const Matrix &m) throw(math_error){
    335         using namespace boost::numeric::ublas;
    336 
    337         if(m.size1() != m.size2()) throw math_error("Inv(m): matrix must be square");
    338         Matrix Q(m), R(m), Rinv(m);
    339         QR (m,Q,R);
    340         for( int i = 0 ; i < R.size1() ; i++ )
    341            for( int j = 0 ; j < R.size2() ; j++ )
    342               if( R(i,j) < 1e-10 )
    343                  R(i,j) = 0;
    344         InvertMatrix(R,Rinv);
    345         return Rinv*Trans(Q);
    346    }
    347 
    348 
    349     /*!
    350     * \fn  inline double DetLU(const Matrix & m)
    351     * \brief compute matrix determinant using LU decomposition
    352     * \param m : ublas matrix
    353     * \return  ublas matrix
    354     */
    355     inline double DetLU(const Matrix & m) throw(math_error){
    356         using namespace boost::numeric::ublas;
    357         typedef permutation_matrix<std::size_t> pmatrix;
    358 
    359 
    360         if(m.size1() != m.size2()) throw math_error("Determinant(m): matrix must be square");
    361 
    362         // create a working copy of the input
    363         Matrix A(m);
    364         // create a permutation matrix for the LU-factorization
    365         pmatrix pm(m.size1());
    366         // perform LU-factorization
    367         int res = lu_factorize(A, pm);
    368         if( res != 0 )  throw math_error("Determinant(m) : singular matrix");
    369         //compute determinant
    370         double det = 1.0;
    371         for (std::size_t i=0; i < pm.size(); ++i) {
    372           if (pm(i) != i)
     322/// Matrix inversion using QR decomposition
     323/// @param m ublas matrix
     324/// @returns ublas matrix
     325inline Matrix InvQR(const Matrix &m)
     326    throw(math_error)
     327{
     328    using namespace boost::numeric::ublas;
     329
     330    if(m.size1() != m.size2()) throw math_error("Inv(m): matrix must be square");
     331    Matrix Q(m), R(m), Rinv(m);
     332    QR (m,Q,R);
     333    for( int i = 0 ; i < R.size1() ; i++ )
     334        for( int j = 0 ; j < R.size2() ; j++ )
     335            if( R(i,j) < 1e-10 )
     336                R(i,j) = 0;
     337    InvertMatrix(R,Rinv);
     338    return Rinv*Trans(Q);
     339}
     340
     341
     342/// compute matrix determinant using LU decomposition
     343/// @param m ublas matrix
     344/// @returns ublas matrix
     345inline double DetLU(const Matrix & m)
     346    throw(math_error)
     347{
     348    using namespace boost::numeric::ublas;
     349    typedef permutation_matrix<std::size_t> pmatrix;
     350
     351
     352    if(m.size1() != m.size2()) throw math_error("Determinant(m): matrix must be square");
     353
     354    // create a working copy of the input
     355    Matrix A(m);
     356    // create a permutation matrix for the LU-factorization
     357    pmatrix pm(m.size1());
     358    // perform LU-factorization
     359    int res = lu_factorize(A, pm);
     360    if( res != 0 )  throw math_error("Determinant(m) : singular matrix");
     361    //compute determinant
     362    double det = 1.0;
     363    for (std::size_t i=0; i < pm.size(); ++i) {
     364        if (pm(i) != i)
    373365            det *= -1.0;
    374           det *= A(i,i);
     366        det *= A(i,i);
     367    }
     368    return det;
     369}
     370
     371
     372/// output stream function
     373inline std::ostream& operator << (std::ostream& ostrm, const Matrix & m)
     374{
     375    for (size_t i=0; i < m.size1(); i++)
     376    {
     377        ostrm << '['<<'\t';
     378        for (size_t j=0; j < m.size2(); j++)
     379        {
     380            double x = m(i,j);
     381            ostrm << x << '\t';
    375382        }
    376         return det;
    377       }
    378 
    379 
    380    // output stream function
    381    inline std::ostream& operator << (std::ostream& ostrm, const Matrix & m)
    382    {
    383     for (size_t i=0; i < m.size1(); i++)
    384      {
    385        ostrm << '['<<'\t';
    386        for (size_t j=0; j < m.size2(); j++)
    387        {
    388          double x = m(i,j);
    389          ostrm << x << '\t';
    390        }
    391       ostrm << ']'<< std::endl;
    392      }
    393      return ostrm;
    394    }
    395 
    396    // output stream function
    397    inline std::ostream& operator << (std::ostream& ostrm, const Vector & v)
    398    {
     383        ostrm << ']'<< std::endl;
     384    }
     385    return ostrm;
     386}
     387
     388/// output stream function
     389inline std::ostream& operator << (std::ostream& ostrm, const Vector & v)
     390{
    399391    for (size_t i=0; i < v.size(); i++)
    400      {
    401        ostrm << '['<<'\t';
    402          double x = v(i);
    403          ostrm << x << '\t';
    404       ostrm << ']'<< std::endl;
    405      }
    406      return ostrm;
    407    }
     392    {
     393        ostrm << '['<<'\t';
     394        double x = v(i);
     395        ostrm << x << '\t';
     396        ostrm << ']'<< std::endl;
     397    }
     398    return ostrm;
     399}
    408400
    409401} // namespace ublas
Note: See TracChangeset for help on using the changeset viewer.