linear_algebra_qr.hpp

Go to the documentation of this file.

/*
 * Copyright(c):
 * Signal Image and Communications (SIC) Department 
 * http://www.sic.sp2mi.univ-poitiers.fr/ 
 *  - University of Poitiers, France http://www.univ-poitiers.fr 
 *  - XLIM  Institute UMR CNRS 7252  http://www.xlim.fr/ 
 * 
 * and 
 * 
 * D2 Fluid, Thermic and Combustion 
 *  - University of Poitiers, France http://www.univ-poitiers.fr 
 *  - PPRIME Institute -  UPR CNRS 3346  http://www.pprime.fr
 *  - ISAE-ENSMA http://www.ensma.fr
 *
 * Contributor(s):
 * The SLIP team,
 * Benoit Tremblais <tremblais_AT_sic.univ-poitiers.fr>,
 * Laurent David <laurent.david_AT_lea.univ-poitiers.fr>, 
 * Ludovic Chatellier <ludovic.chatellier_AT_univ-poitiers.fr>, 
 * Lionel Thomas <lionel.thomas_AT_univ-poitiers.fr>, 
 * Denis Arrivault <arrivault_AT_sic.univ-poitiers.fr>, 
 * Julien Dombre <julien.dombre_AT_univ-poitiers.fr>.
 *
 * Description:
 * The Simple Library of Image Processing (SLIP) is a new image processing 
 * library. It is written in the C++ language following as much as possible 
 * the ISO/ANSI C++ standard. It is consequently compatible with any system  
 * satisfying the ANSI C++ complience. It works on different Unix , Linux ,  
 * Mircrosoft Windows and Mac OS X plateforms. SLIP is a research library that  
 * was created by the Signal, Image and Communications (SIC) departement of  
 * the XLIM, UMR 7252 CNRS Institute in collaboration with the Fluids, Thermic  
 * and Combustion departement of the P', UPR 3346 CNRS Institute of the  
 * University of Poitiers.
 * 
 * The SLIP Library source code has been registered to the APP (French Agency 
 * for the Protection of Programs) by the University of Poitiers and CNRS, 
 * under  registration number IDDN.FR.001.300034.000.S.P.2010.000.21000.

 * http://www.sic.sp2mi.univ-poitiers.fr/slip/
 *
 * This software is governed by the CeCILL-C license under French law and
 * abiding by the rules of distribution of free software.  You can  use, 
 * modify and/ or redistribute the software under the terms of the CeCILL-C 
 * license as circulated by CEA, CNRS and INRIA at the following URL 
 * http://www.cecill.info.
 * As a counterpart to the access to the source code and  rights to copy,
 * modify and redistribute granted by the license, users are provided only
 * with a limited warranty  and the software's author,  the holder of the
 * economic rights,  and the successive licensors  have only  limited
 * liability.
 * 
 * In this respect, the user's attention is drawn to the risks associated
 * with loading,  using,  modifying and/or developing or reproducing the
 * software by the user in light of its specific status of free software,
 * that may mean  that it is complicated to manipulate,  and  that  also
 * therefore means  that it is reserved for developers  and  experienced
 * professionals having in-depth computer knowledge. Users are therefore
 * encouraged to load and test the software's suitability as regards their
 * requirements in conditions enabling the security of their systems and/or
 * data to be ensured and,  more generally, to use and operate it in the 
 * same conditions as regards security. 
 *
 * The fact that you are presently reading this means that you have had
 * knowledge of the CeCILL-C license and that you accept its terms.
 */



#ifndef SLIP_LINEAR_ALGEBRA_QR_HPP
#define SLIP_LINEAR_ALGEBRA_QR_HPP

#include "linear_algebra.hpp"
#include "linear_algebra_traits.hpp"
#include "linear_algebra_svd.hpp"
#include "macros.hpp"
#include "gram_schmidt.hpp"
#include "compare.hpp"
//#include "Matrix.hpp"
#include <limits>
#include <vector>



namespace slip
{
 
  template <class Matrix1, class Matrix2>
  inline
  void balance(Matrix1 & A, Matrix2 & D)
  {
    assert(A.rows() == A.cols());
    assert(D.rows() == D.cols());
    assert(A.rows() == D.cols());
    typedef typename Matrix1::value_type _T;
    std::size_t dim = A.rows();

    _T row_norm, col_norm;
    bool stop = 0;

    // initialize D to the identity matrix
    slip::identity(D);

    while(!stop)
      {
                double g, f, s;
                stop = 1;
                for (std::size_t i = 0; i < dim; ++i)
                  {
                    row_norm = 0.0;
                    col_norm = 0.0;
                    for (std::size_t j = 0; j < dim; ++j)
                      {
                                if (j != i)
                                  {
                                    col_norm += std::abs(A[j][i]);
                                    row_norm += std::abs(A[i][j]);
                                  }
                      }
                    
                    if ((col_norm == 0.0) || (row_norm == 0.0))
                {
                  continue;
                }
                    g = row_norm / 2.0;
                    f = 1.0;
                    s = col_norm + row_norm;
                      
                    // find the integer power of two which
                    // comes closest to balancing the matrix
                   
                    while (col_norm < g)
                      {
                                f *= 2.0;
                                col_norm *= 4.0;
                      }

                    g = row_norm * 2.0;

                    while (col_norm > g)
                      {
                                f /= 2.0;
                                col_norm /= 4.0;
                      }
                    
                    if ((row_norm + col_norm) < 0.95 * s * f)
                      {
                                stop = 0;
                                
                                g = 1.0 / f;
                
                                // apply similarity transformation D, where
                                // D[i][i] = f
                                D[i][i] *= f;   
                                //              std::cout << "g : " << g << " - f : " << f << std::endl;
                                // multiply by D on the right and D^{-1} on the left
                                std::transform(A.row_begin(i),A.row_end(i),A.row_begin(i),std::bind2nd(std::multiplies<_T>(),g));
                                std::transform(A.col_begin(i),A.col_end(i),A.col_begin(i),std::bind2nd(std::multiplies<_T>(),f));
                      }
                  }
      }
    
  }

  template <typename MatrixIterator1,
                    typename MatrixIterator2,
                    typename MatrixIterator3>
  inline
  void
  gram_schmidt_qr(MatrixIterator1 A_up, MatrixIterator1 A_bot, 
                                  MatrixIterator2 Q_up, MatrixIterator2 Q_bot, 
                                  MatrixIterator3 R_up, MatrixIterator3 R_bot,
                                  const typename slip::lin_alg_traits<typename MatrixIterator1::value_type>::value_type tol)
  {
    assert((A_up - A_bot)[0] == (Q_up - Q_bot)[0]);
    assert((A_up - A_bot)[1] == (Q_up - Q_bot)[1]);
    assert((A_up - A_bot)[1] == (R_up - R_bot)[0]);
    assert((A_up - A_bot)[1] == (R_up - R_bot)[1]);
    assert((A_up - A_bot)[0] == (A_up - A_bot)[1]); 
    typedef typename MatrixIterator1::value_type value_type;
    typedef  typename MatrixIterator1::size_type size_type;
    typename MatrixIterator1::difference_type sizeA = A_bot - A_up;
    typename MatrixIterator2::difference_type sizeQ = Q_bot - Q_up;
    typename MatrixIterator3::difference_type sizeR = R_bot - R_up;
    size_type A_rows = sizeA[0];
    size_type A_cols = sizeA[1];

    //init T with A
    slip::Array2d<value_type> T(A_rows,A_cols,A_up,A_bot);
     
    while(!is_upper_triangular(T.upper_left(),T.bottom_right(),tol))
      {
                //orthogonalization of T = QR
                slip::modified_gram_schmidt(T.upper_left(),T.bottom_right(),
                                                                    Q_up,Q_bot,
                                                                    R_up,R_bot);
                //T = RQ
                slip::matrix_matrix_multiplies(R_up,R_bot,
                                                                       Q_up,Q_bot,
                                                                       T.upper_left(),T.bottom_right());

      }
  }


  template <typename Matrix1,
                    typename Matrix2,
                    typename Matrix3>
  inline
  void
  gram_schmidt_qr(const Matrix1& A,
                                       Matrix2& Q,
                                       Matrix3& R,
                                       const typename slip::lin_alg_traits<typename Matrix1::value_type>::value_type tol)
  {
    slip::gram_schmidt_qr(A.upper_left(),A.bottom_right(),
                                                       Q.upper_left(),Q.bottom_right(),
                                                       R.upper_left(),R.bottom_right(),
                                                       tol);
  }


  template <typename Matrix1, typename Vector>
  inline
  void
  householder_qr(Matrix1& M, Vector& V0)
                                   
  {
    assert(M.rows() == M.cols());
    assert(V0.size() == M.rows());
    typedef typename Matrix1::value_type value_type;

    std::size_t M_rows = M.rows();
    std::size_t M_cols = M.cols();
   
    slip::Vector<value_type> V(M_rows);
    slip::Box2d<int> box;

    for(std::size_t j = 0; j < M_cols; ++j)
      {
                //computes the householder vector from jth column of M
                box.set_coord(int(j),int(j),int(M_rows-1),int(M_cols-1));
                value_type nu = value_type(0);
                
                slip::housegen(M.upper_left(box).col_begin(0),
                                       M.upper_left(box).col_end(0),
                                       V.begin()+j,V.end(),
                                       nu);
                V0[j] = *(V.begin()+j);
                //computes M = (I-betaVV^*)M(j:M_rows-1,j:M_cols-1)
                slip::left_householder_update(V.begin()+j,V.end(),
                                                                      M.upper_left(box),M.bottom_right(box));
                if(j < M.rows())
                  {
                    //copy householder vector in the jth column of the 
                    //lower-triangular part of M
                    std::copy(V.begin()+(j+1),V.end(),
                                      M.upper_left(box).col_begin(0)+1);
                  }

      }
  }
   
  template <typename Matrix1,typename Matrix2, typename Matrix3>
  inline
  void
   householder_qr(const Matrix1& M, Matrix2& Q, Matrix3& R)
                                   
  {
    assert(Q.rows() == Q.cols());
    assert(M.rows() == Q.rows());
    assert(R.rows() == M.rows());
    assert(R.cols() == M.cols());
   
    typedef typename Matrix1::value_type value_type;
  
    std::size_t M_rows = M.rows();
    
    slip::Vector<value_type> V0(M_rows);
    
    //copy R = M
    std::copy(M.begin(),M.end(),R.begin());
    slip::householder_qr(R,V0);
    slip::left_householder_accumulate(R,V0,Q);
    //fill the lower part of R with zero
    for(std::size_t i = 0; i < M_rows; ++i)
      {
                std::fill(R.row_begin(i),R.row_begin(i)+i,value_type(0.0));
      }
  }


  template <typename MatrixIterator>
  inline
  std::size_t subdiag_smaller_elem(MatrixIterator A_up, MatrixIterator A_bot, 
                                                                   typename slip::lin_alg_traits<typename std::iterator_traits<MatrixIterator>::value_type>::value_type precision)
  {
    slip::DPoint2d<int> d(0,1);
    typedef typename std::iterator_traits<MatrixIterator>::value_type _T;
    MatrixIterator A_diag = A_bot;
    MatrixIterator A_sub = A_bot;
    A_sub -= d;
    d.set_coord(1,1);
    A_diag-=d;
    A_sub -= d;
    std::size_t pos = 1;
    while(A_diag != A_up)
      {
                if(((*A_sub) == _T(0)) ||
                   (std::abs(*A_sub) < precision*(std::abs(*A_diag) + std::abs(*(A_diag-d)))))
                  {
                    *A_sub = _T(0);
                    return pos;
                  }
                pos++;
                A_sub-=d;
                A_diag-=d;
      }
    return 0;
  }


  template <typename MatrixIterator1, typename MatrixIterator2, typename MatrixIterator3>
  void Schur_factorization_2by2(MatrixIterator1 A_up, MatrixIterator1 A_bot, 
                                                                MatrixIterator2 Z_up, MatrixIterator2 Z_bot, 
                                                                MatrixIterator3 D_up, MatrixIterator3 D_bot)
  {
    typedef typename std::iterator_traits<MatrixIterator1>::value_type _T;
    typename MatrixIterator1::difference_type sizeA= A_bot - A_up;
    typename MatrixIterator2::difference_type sizeZ= Z_bot - Z_up;
    typename MatrixIterator3::difference_type sizeD= D_bot - D_up;
    
    assert(sizeA[0] == sizeA[1]);
    assert(sizeZ[0] == sizeZ[1]);
    assert(sizeD[0] == sizeD[1]);
    assert(sizeA[1] == 2);
    assert(sizeZ[1] == 2);
    assert(sizeD[1] == 2);
    
    slip::DPoint2d<int> d10(1,0);
    slip::DPoint2d<int> d01(0,1);
    slip::DPoint2d<int> d11(1,1);
    _T a = *A_up;
    _T b = *(A_up+d01);
    _T c = *(A_up+d10);
    _T d = *(A_up+d11);
    _T cs,sn;
    _T tmp;
    _T p, z;
    _T bcmax, bcmis, scale;
    _T tau, sigma;
    _T cs1, sn1;
    _T aa, bb, cc, dd;
    _T sab, sac;
    
    if (std::abs(c) <= std::abs(std::numeric_limits<_T>::epsilon()))
      {
                // A is already upper triangular
                // rotation matrix is the identity
                cs = _T(1.0);
                sn = _T(0.0);
      }
    else if (std::abs(b) <= std::abs(std::numeric_limits<_T>::epsilon()))
      {
                // A is lower triangular
                // swap rows and columns to make it upper triangular
                
                cs = _T(0.0);
                sn = _T(1.0);
                
                tmp = d;
                d = a;
                a = tmp;
                b = -c;
                c = _T(0.0);
      }
    else if ((std::abs(a - d) <= std::abs(std::numeric_limits<_T>::epsilon())) && (slip::sign(b) != slip::sign(c)))
      {
                // A has complex eigenvalues with a and d as real part (a=d) 
                cs = _T(1.0);
                sn = _T(0.0);
      }
    else
      {
                // General case
                tmp = a - d;
                p = 0.5 * tmp;
                bcmax = (std::abs(b) > std::abs(c) ? std::abs(b) : std::abs(c));
                bcmis = (std::abs(b) < std::abs(c) ? std::abs(b) : std::abs(c)) * slip::sign(b) * slip::sign(c);
                scale = (std::abs(p) > bcmax ? std::abs(p) : bcmax);
                z = (p / scale) * p + (bcmax / scale) * bcmis;
                
                //if (z >= 4.0 * std::abs(std::numeric_limits< _T>::epsilon()))
                if (z >= 4.0 * std::abs(slip::epsilon<_T>()))
                  {
                    // real eigenvalues
                    z = p + slip::sign(p) * std::abs(std::sqrt(scale) * std::sqrt(z));
                    a = d + z;
                    d -= (bcmax / z) * bcmis;
                    tau = slip::pythagore(c, z);
                    cs = z / tau;
                    sn = c / tau;
                    b -= c;
                    c = 0.0;
                  }
                else
                  {
                    
                    // complex eigenvalues, or one double real eigenvalue
                    
                    sigma = b + c;
                    tau = slip::pythagore(sigma, tmp);
                    cs = std::sqrt(0.5 * (1.0 + std::abs(sigma) / tau));
                    sn = -(p / (tau * cs)) * slip::sign(sigma);
                    // [ aa bb ] = [ a b ] [ cs -sn ]
                    // [ cc dd ]   [ c d ] [ sn  cs ]
                    
                    aa = a * cs + b * sn;
                    bb = -a * sn + b * cs;
                    cc = c * cs + d * sn;
                    dd = -c * sn + d * cs;
                    
                    // [ a b ] = [  cs  sn ] [ aa bb ]
                    // [ c d ]   [ -sn  cs ] [ cc dd ]
                    
                    a = aa * cs + cc * sn;
                    b = bb * cs + dd * sn;
                    c = -aa * sn + cc * cs;
                    d = -bb * sn + dd * cs;
                    
                    tmp = 0.5 * (a + d);
                    a = d = tmp;
                    
                    if (c != 0.0)
                      {
                                if (b != 0.0)
                                  {
                                    if (slip::sign(b) == slip::sign(c))
                                      {
                                                // one double real eigenvalue
                                                
                                                sab = std::sqrt(std::abs(b));
                                                sac = std::sqrt(std::abs(c));
                                                p = slip::sign(c) * std::abs(sab * sac);
                                                tau = 1.0 / std::sqrt(std::abs(b + c));
                                                a = tmp + p;
                                                d = tmp - p;
                                                b -= c;
                                                c = 0.0;
                                                
                                                cs1 = sab * tau;
                                                sn1 = sac * tau;
                                                tmp = cs * cs1 - sn * sn1;
                                                sn = cs * sn1 + sn * cs1;
                                                cs = tmp;
                                      }
                                  }
                                else
                                  {
                                    b = -c;
                                    c = 0.0;
                                    tmp = cs;
                                    cs = -sn;
                                    sn = tmp;
                                  }
                      }
                  }
      }
    
    *D_up = a;
    *(D_up+d11) = d;
    *(D_up+d01) = b;
    *(D_up+d10) = c;
    
    *Z_up = cs;
    *(Z_up+d01) = -sn;
    *(Z_up+d10) = sn;
    *(Z_up+d11)= cs;
  }
  
  template <class Matrix, typename VectorIterator>
  inline
  void MatrixHouseholder(Matrix & H, VectorIterator V_begin, VectorIterator V_end)
  {
    assert(H.rows() == H.cols());
    assert(H.rows() == std::size_t(V_end-V_begin));
    typedef typename Matrix::value_type _T;
    std::size_t d = H.rows();
    for(std::size_t i=0; i< d; i++)
      {
                H[i][i] = _T(1);
      }
    _T norm = slip::inner_product(V_begin,V_end,V_begin);
    if(norm != _T(0))
      {

                slip::rank1_update(H.upper_left(),H.bottom_right(),
                                                   -_T(2)/norm,
                                                   V_begin,V_end,
                                                   V_begin,V_end);
      }
   
  }
  
  template <class Vector, typename MatrixIterator>
  inline
  void VectorHouseholder(Vector & V, MatrixIterator M_up, MatrixIterator M_bot)
  {
    typedef typename Vector::value_type _T;
    typename MatrixIterator::difference_type sizeM = M_bot - M_up;
    assert(V.size() == std::size_t(sizeM[0]));
    
    _T alpha = - _T(slip::sign(*M_up.col_begin(0)) * std::sqrt(slip::inner_product(M_up.col_begin(0),M_up.col_end(0),M_up.col_begin(0))));
    
    V[0] = (*M_up.col_begin(0)) - alpha;
    for(std::size_t i=1; i<V.size(); ++i)
      V[i] = *(M_up.col_begin(0)+i);
  }
  

 
  
  template <typename HessenbergMatrix, typename Matrix>
  inline
  bool Francis_QR_Step(HessenbergMatrix & H, Matrix & Z, slip::Box2d<int> box, 
                                       bool compute_z)
  {
    typedef typename HessenbergMatrix::value_type _T;
    std::size_t Dim = H.rows();
    //Francis QR step
    std::size_t n = (box.bottom_right())[0]+1;
    std::size_t beg = (box.upper_left())[0];
    
    _T s  = H[n-1][n-1] + H[n-2][n-2];
    _T t  = (H[n-1][n-1] * H[n-2][n-2]) - (H[n-1][n-2] * H[n-2][n-1]);
    
    _T x = (H[beg][beg] * H[beg][beg]) + (H[beg][beg+1] * H[beg+1][beg]) - (s * H[beg][beg]) + t;
    _T y = H[beg+1][beg] * (H[beg][beg] + H[beg+1][beg+1] - s);
    _T z = H[beg+1][beg] * H[beg+2][beg+1];
    
    for(std::size_t k = beg; k < n-2; k++)
      {
                Matrix P3(3,3,_T(0));
                slip::Vector<_T> V3(3);
                V3[0] = x; V3[1] = y, V3[2] = z;

                _T alpha = - _T(slip::sign(V3[0]) * std::sqrt(slip::inner_product(V3.begin(),V3.end(),V3.begin())));
                slip::Vector<_T> Vh(V3);
                Vh[0] -= alpha;
                MatrixHouseholder(P3,Vh.begin(),Vh.end());
                
                Matrix Pk(slip::identity<Matrix>(Dim,Dim));
                Matrix T(Dim,Dim,_T(0));
                slip::Box2d<int> bk(k,k,k+2,k+2);
                std::copy(P3.upper_left(),P3.bottom_right(),Pk.upper_left(bk));
                
                Matrix Pktr(Pk);
                slip::transpose(Pk,Pktr);
                //H = Pktr.H.Pk
                slip::matrix_matrix_multiplies(Pktr,H,T);
                slip::matrix_matrix_multiplies(T,Pk,H);
                if(compute_z)
                  {
                    //we accumulate P
                    slip::matrix_matrix_multiplies(Z,Pk,T);
                    std::copy(T.upper_left(),T.bottom_right(),Z.upper_left());
                  }
                
                x = H[k+1][k];
                y = H[k+2][k];
                if(k < (n - 3))
                  z = H[k+3][k];
      }
    
    Matrix P2(2,2,_T(0));
    slip::Vector<_T> V2(2);
    V2[0] = x; V2[1] = y;

    _T alpha = - _T(slip::sign(V2[0]) * std::sqrt(slip::inner_product(V2.begin(),V2.end(),V2.begin())));
    slip::Vector<_T> Vh(V2);
    Vh[0] -= alpha;
    slip::MatrixHouseholder(P2,Vh.begin(),Vh.end());
    
    Matrix Pk(slip::identity<Matrix>(Dim,Dim));
    Matrix T(Dim,Dim,_T(0));
    slip::Box2d<int> bk(n-2,n-2,n-1,n-1);
    std::copy(P2.upper_left(),P2.bottom_right(),Pk.upper_left(bk));
    
    Matrix Pktr(Pk);
    slip::transpose(Pk,Pktr);
    //H = Pktr.H.Pk
    slip::matrix_matrix_multiplies(Pktr,H,T);
    slip::matrix_matrix_multiplies(T,Pk,H);
    if(compute_z)
      {
                //we accumulate P
                slip::matrix_matrix_multiplies(Z,Pk,T);
                std::copy(T.upper_left(),T.bottom_right(),Z.upper_left());
      }
    
    return 1;
  }
  
  
  template <class HessenbergMatrix, class Vector, class Matrix>
  inline
  void Francis_Schur_standardize(HessenbergMatrix & H, Vector & Eig, 
                                                                 Matrix & Z, slip::Box2d<int> b22, 
                                                                 bool compute_z)
  {
    assert(b22.width() == b22.height());
    assert(b22.width() == 2);
    typedef typename HessenbergMatrix::value_type _T;
    typedef typename Vector::value_type Complex;
    Complex J(0,1);
    std::size_t n = H.rows();
    std::size_t pos = (b22.upper_left())[0];
    Matrix U(2,2,_T(0));
    Matrix D(2,2,_T(0));
    //Schur decomposition of teh 2_by_2 block
    slip::Schur_factorization_2by2(H.upper_left(b22),H.bottom_right(b22),U.upper_left(),U.bottom_right(),D.upper_left(),D.bottom_right());
    
    // set eigenvalues
    Eig[pos] = Complex(D[0][0]);
    Eig[pos + 1] = Complex(D[1][1]);
    if (std::abs(D[1][0]) > std::abs(std::numeric_limits< _T>::epsilon()))
      {
                //double tmp = std::sqrt(std::abs(D[0][1]) * std::abs(D[1][0]));
                typename Complex::value_type tmp = std::sqrt(std::abs(D[0][1]) * std::abs(D[1][0]));
                Eig[pos] += J*tmp;
                Eig[pos + 1] -=  J*tmp;
      }
    
    
    //  with
    //     U = [ CS -SN ]
    //         [ SN  CS ]
    //
    //  If H was
    //
    //     H = [ H11 | H12 | H13 ]
    //         [ 0*  | H22 | H23 ]
    //         [ 0   | 0*  | H33 ]
    //  
    //  we must compute :
    // 
    //     H = P' H P  = [ H11  | H12U   |  H13   ]
    //                   [ U'0* | U'H22U |  U'H23 ]
    //                   [ 0    | 0* U   |  H33   ]
    //
    //     Z = Z * P   = [ Z11 | Z12 U | Z13 ]
    //                   [ Z21 | Z22 U | Z23 ]
    //                   [ Z31 | Z32 U | Z33 ]
    //  with
    //
    //     P = [ I 0 0 ]
    //         [ 0 U 0 ]
    //         [ 0 0 I ]
    
    
    // calculation of H22
    std::copy(D.upper_left(),D.bottom_right(),H.upper_left(b22));
    Matrix Ut(U);
    Ut[0][1] = U[1][0];
    Ut[1][0] = U[0][1];
    Matrix R(n,n,_T(0));
    if (compute_z && (pos < (n - 2)))
      {
                slip::Box2d<int> m23 (pos,pos+2,pos+1,n-1);
                slip::Box2d<int> m32 (pos+2,pos,n-1,pos+1);
                
                // calculation of H23
                slip::matrix_matrix_multiplies(Ut.upper_left(),Ut.bottom_right(),H.upper_left(m23),H.bottom_right(m23),R.upper_left(m23),R.bottom_right(m23));
                std::copy(R.upper_left(m23),R.bottom_right(m23),H.upper_left(m23));
                
                // calculation of Z32
                slip::matrix_matrix_multiplies(Z.upper_left(m32),Z.bottom_right(m32),U.upper_left(),U.bottom_right(),R.upper_left(m32),R.bottom_right(m32));
                std::copy(R.upper_left(m32),R.bottom_right(m32),Z.upper_left(m32));
      }
    if (pos > 0)
      {
                // calculation of H12
                slip::Box2d<int> m12 (0,pos,pos-1,pos+1);

                slip::matrix_matrix_multiplies(H.upper_left(m12),H.bottom_right(m12),U.upper_left(),U.bottom_right(),R.upper_left(m12),R.bottom_right(m12));
                std::copy(R.upper_left(m12),R.bottom_right(m12),H.upper_left(m12));
                if(compute_z)
                  {
                    // calculation of Z12
                    slip::matrix_matrix_multiplies(Z.upper_left(m12),Z.bottom_right(m12),U.upper_left(),U.bottom_right(),R.upper_left(m12),R.bottom_right(m12));
                    std::copy(R.upper_left(m12),R.bottom_right(m12),Z.upper_left(m12));
                  }
      }
    
    // calculation of Z22
    slip::matrix_matrix_multiplies(Z.upper_left(b22),Z.bottom_right(b22),U.upper_left(),U.bottom_right(),R.upper_left(b22),R.bottom_right(b22));
    std::copy(R.upper_left(b22),R.bottom_right(b22),Z.upper_left(b22));
    
  } 
  
  template <class HessenbergMatrix, class Vector, class Matrix>
  inline
  bool Francis_Schur_decomp(HessenbergMatrix & H, Vector & Eig, Matrix & Z, 
                                                    slip::Box2d<int> & box, 
                                                    typename slip::lin_alg_traits<typename HessenbergMatrix::value_type>::value_type precision, 
                                                    bool compute_Z)
  {
    assert(H.rows() == H.cols());
    assert((Z.rows() == Z.cols()));
    assert(Eig.size() == H.cols());
    assert((Z.rows() == H.rows()));
    assert(box.width() == box.height());
    assert(box.width() <= (int)H.rows());
    
    typedef typename HessenbergMatrix::value_type _T;
    typedef typename Vector::value_type Complex;
    
    std::size_t n = box.width();
    std::complex<_T> lambda1,lambda2;
    std::size_t endbox = (box.bottom_right())[0], begbox = (box.upper_left())[0], it=0, ittot = -1;
    
    while((n > 2) && (it < 10))
      {
                ittot++;
                std::size_t q = slip::subdiag_smaller_elem(H.upper_left(box),H.bottom_right(box),precision);

                if(q==0)
                  {
                    // no small subdiagonal element found - perform a QR
                    // sweep on the active reduced hessenberg matrix
                    slip::Francis_QR_Step(H,Z,box,compute_Z);
                    continue;
                  }
                if (q == 1)
                  {
                    // H[endbox][endbox] is a real eigenvalue
                    //Eig[endbox] = std::complex<_T>(H[endbox][endbox]);
                    Eig[endbox] = H[endbox][endbox];
                    
                    it = 0;
                    endbox--;
                    box.set_coord(begbox,begbox,endbox,endbox);
                    n = box.width();
                  }
                else if (q == 2)
                  {
                    // The last 2*2 diagonal matrix is an eigenvalue system
                    slip::Box2d<int> b22 (endbox-1,endbox-1,endbox,endbox);
                    slip::Francis_Schur_standardize(H,Eig,Z,b22,compute_Z);
                    it = 0;
                    endbox-=2;
                    box.set_coord(begbox,begbox,endbox,endbox);
                    n = box.width();
                  }
                else if (q == n-1)
                  {
                    // H[begbox][begbox] and  H[begbox+1][begbox+1] are real eigenvalues
                    //  Eig[begbox] = std::complex<_T>(H[begbox][begbox]);
                    //            begbox++;
                    //Eig[begbox] = std::complex<_T>(H[begbox][begbox]);
                    Eig[begbox] = H[begbox][begbox];
                    begbox++;
                    it = 0;
                    box.set_coord(begbox,begbox,endbox,endbox);
                    n = box.width();
                  }
                else if (q == n-2)
                  {
                    // The first 2*2 diagonal matrix is an eigenvalue system
                    slip::Box2d<int> b22 (begbox,begbox,begbox+1,begbox+1);
                    slip::Francis_Schur_standardize(H,Eig,Z,b22,compute_Z);
                    it = 0;
                    begbox+=2;
                    box.set_coord(begbox,begbox,endbox,endbox);
                    n = box.width();
                  }
                else
                  {
                    //  There is a zero element on the subdiagonal somewhere
                    //  in the middle of the matrix - we can now operate
                    //  separately on the two submatrices split by this
                    //  element. q is the row index of the zero element.
                    
                    // operate on lower right block first
                    slip::Box2d<int> blr(endbox-q+1,endbox-q+1,endbox,endbox);
                    slip::Francis_Schur_decomp(H,Eig,Z,blr,precision,compute_Z);
                    // operate on upper left block
                    slip::Box2d<int> bul(begbox,begbox,endbox-q,endbox-q);
                    slip::Francis_Schur_decomp(H,Eig,Z,bul,precision,compute_Z);
                    n = 0;
                  }
      }
    
    // special cases of n = 1 or 2
    
    if (n == 1)
      {
                Eig[begbox] = H[begbox][begbox];
                begbox ++;
                box.set_coord(begbox,begbox,endbox,endbox);
                n = 0;
      }
    else if (n == 2)
      {
                slip::Francis_Schur_standardize(H,Eig,Z,box,compute_Z);
                begbox+=2;
                box.set_coord(begbox,begbox,endbox,endbox);
                n = 0;
      }
    return 0;
  }

  template <class Matrix1, class Matrix2,class Vector, class Matrix3>
  inline
  bool schur(const Matrix1& M, Matrix2& H, Matrix3 & Z, Vector & Eig,
                     bool compute_Z,typename slip::lin_alg_traits<typename Matrix1::value_type>::value_type precision  = typename slip::lin_alg_traits<typename Matrix1::value_type>::value_type(1.0E-12))
  {
    slip::householder_hessenberg(M,H,Z);
    slip::Box2d<int> box(0,0,(M.rows()-1),(M.cols()-1));
    return slip::Francis_Schur_decomp(H,Eig,Z,box,precision,compute_Z);
    
  }

 
  template <class Matrix1,class Matrix2>
  inline
  bool Sylvester_solve(const Matrix1 & S, slip::Box2d<int> & b11, slip::Box2d<int> & b22, double & g, Matrix2 & X)
  {
    typedef typename Matrix1::value_type _T;
    slip::Point2d<std::size_t> b11_up(std::size_t(b11.upper_left()[0]),std::size_t(b11.upper_left()[1]));
    slip::Point2d<std::size_t> b11_bot(std::size_t(b11.bottom_right()[0]),std::size_t(b11.bottom_right()[1]));
    slip::Point2d<std::size_t> b22_up(std::size_t(b22.upper_left()[0]),std::size_t(b22.upper_left()[1]));
    slip::Point2d<std::size_t> b22_bot(std::size_t(b22.bottom_right()[0]),std::size_t(b22.bottom_right()[1]));
    int p = int((b11_bot - b11_up)[0]) + 1;
    int q = int((b22_bot - b22_up)[0]) + 1;
    assert(p == int((b11_bot - b11_up)[1])+1);
    assert((p == 1)||(p == 2));
    assert(q == int((b22_bot - b22_up)[1])+1);
    assert((q == 1) ||(q == 2));
    slip::DPoint2d<std::size_t> u(1,1);
    assert((b22_up - b11_bot) == u);
    assert(b11_up[0] == b11_up[1]);
   
    
    //first case p==q==1
    if((p==1)&&(q==1))
      {
                assert(X.dim1() == 1);
                assert(X.dim2() == 1);

                if(std::abs(S(b11_up) - S(b22_up)) < std::abs(std::numeric_limits<_T>::epsilon()))
                  {
                    g = _T(0);
                    std::cout << "Possible dependance between blocs" << std::endl;
                    return false;
                  }
                X[0][0] = g * S[b11_up.x1()][b11_up.x2()+1] / (S(b11_up) - S(b22_up));
      }

    //second case p==2 && q==1
    else if((p==2)&&(q==1))
      {
                assert(X.dim1() == 1);
                assert(X.dim2() == 2);
                std::vector<_T> xt(2,_T(0));
                std::vector<_T> B;
                B.push_back(g * S[b11_up.x1()][b11_up.x2()+2]);
                B.push_back(g * S[b11_bot.x1()][b11_bot.x2()+1]);
                Matrix1 G(2,2,_T(0));
                G[0][0] = S(b11_up) - S(b22_up);
                G[0][1] = S[b11_up.x1()][b11_up.x2()+1];
                G[1][0] = S[b11_up.x1()+1][b11_up.x2()];
                G[1][1] = S(b11_bot) - S(b22_bot); 
                if(!slip::gauss_solve_with_filling(G,xt,B,slip::Euclidean_norm<_T>(G.begin(),G.end()) * std::abs(std::numeric_limits<_T>::epsilon())))
                  {
                    g = _T(0);
                    std::cout << "Possible dependance between blocs" << std::endl;
                  } 
                std::copy(xt.begin(),xt.end(),X.begin());
      }
 
    //third case p==1 && q==2
    else if((p==1)&&(q==2))
      {
                assert(X.dim1() == 2);
                assert(X.dim2() == 1);
                std::vector<_T> xt(2,_T(0));
                std::vector<_T> B;
                B.push_back(g * S[b11_up.x1()][b11_up.x2()+1]);
                B.push_back(g * S[b11_bot.x1()][b11_bot.x2()+2]);
                Matrix1 G(2,2,_T(0));
                G[0][0] = S(b11_up) - S(b22_up);
                G[0][1] = -S[b22_up.x1()+1][b22_up.x2()];
                G[1][0] = -S[b22_up.x1()][b22_up.x2()+1];
                G[1][1] = S(b11_bot) - S(b22_bot); 
                if(!slip::gauss_solve_with_filling(G,xt,B,slip::Euclidean_norm<_T>(G.begin(),G.end()) * std::abs(std::numeric_limits<_T>::epsilon())))
                  {
                     g = _T(0);
                    std::cout << "Possible dependance between blocs" << std::endl;
                  } 
                std::copy(xt.begin(),xt.end(),X.begin());
      }
  
    //fourth case p==2 && q==2
    else if((p==2)&&(q==2))
      {
                assert(X.dim1() == 2);
                assert(X.dim2() == 2);
                std::vector<_T> xt(4,_T(0));
                std::vector<_T> B;
                B.push_back(g * S[b11_up.x1()][b11_up.x2()+2]);
                B.push_back(g * S[b11_up.x1()][b11_up.x2()+3]);
                B.push_back(g * S[b11_bot.x1()][b11_bot.x2()+1]);
                B.push_back(g * S[b11_bot.x1()][b11_bot.x2()+2]);
                Matrix1 G(4,4,_T(0));
                G[0][0] = S(b11_up) - S(b22_up);
                G[0][1] = -S[b22_up.x1()+1][b22_up.x2()];
                G[0][2] = -S[b11_up.x1()][b11_up.x2()+1];

                G[1][0] = -S[b22_up.x1()][b22_up.x2()+1];
                G[1][1] = S(b11_up) - S(b22_bot);
                G[1][3] = S[b11_up.x1()][b11_up.x2()+1];
 
                G[2][0] = S[b11_up.x1()+1][b11_up.x2()];
                G[2][2] = S(b11_bot) - S(b22_up);
                G[2][3] = -S[b22_up.x1()+1][b22_up.x2()];

                G[3][1] = S[b11_up.x1()+1][b11_up.x2()];
                G[3][2] = -S[b22_up.x1()][b22_up.x2()+1];
                G[3][3] = S(b11_bot) - S(b22_bot);

                if(!slip::gauss_solve_with_filling(G,xt,B,slip::Euclidean_norm<_T>(G.begin(),G.end()) * std::abs(std::numeric_limits<_T>::epsilon())))
                  {
                    g = _T(0);
                    std::cout << "Possible dependance between blocs" << std::endl;
                  } 
                std::copy(xt.begin(),xt.end(),X.begin());
      }
    return true;
  } 

 
}// end slip



#endif //SLIP_LINEAR_ALGEBRA_QR_HPP