QR_Lapack.h

Go to the documentation of this file.

#ifndef TOON_INCLUDE_QR_LAPACK_H
#define TOON_INCLUDE_QR_LAPACK_H


#include "TooN.h"
#include "lapack.h"
#include <utility>

namespace TooN{

template<int Rows=Dynamic, int Cols=Rows, class Precision=double>
class QR_Lapack{

    private:
        static const int square_Size = (Rows>=0 && Cols>=0)?(Rows<Cols?Rows:Cols):Dynamic;

    public: 
        template<int R, int C, class P, class B> 
        QR_Lapack(const Matrix<R,C,P,B>& m, bool p=0)
        :copy(m),tau(square_size()), Q(square_size(), square_size()), do_pivoting(p), pivot(Zeros(square_size()))
        {
            //pivot is set to all zeros, which means all columns are free columns
            //and can take part in column pivoting.

            compute();
        }
        
        const Matrix<Rows, Cols, Precision, ColMajor>& get_R()
        {
            return copy;
        }
        
        const Matrix<square_Size, square_Size, Precision, ColMajor>& get_Q()
        {
            return Q;
        }   

        const Vector<Cols, int>& get_P()
        {
            return pivot;
        }

    private:

        void compute()
        {   
            int M = copy.num_rows();
            int N = copy.num_cols();
            
            int LWORK=-1;
            int INFO;
            int lda = M;

            Precision size;
            
            //Set up the pivot vector
            if(do_pivoting)
                pivot = Zeros;
            else
                for(int i=0; i < pivot.size(); i++)
                    pivot[i] = i+1;

            
            //Compute the working space
            geqp3_(&M, &N, copy.get_data_ptr(), &lda, pivot.get_data_ptr(), tau.get_data_ptr(), &size, &LWORK, &INFO);

            LWORK = (int) size;

            Precision* work = new Precision[LWORK];
            
            geqp3_(&M, &N, copy.get_data_ptr(), &lda, pivot.get_data_ptr(), tau.get_data_ptr(), work, &LWORK, &INFO);


            if(INFO < 0)
                std::cerr << "error in QR, INFO was " << INFO << std::endl;

            //The upper "triangle+" of copy is R
            //The lower right and tau contain enough information to reconstruct Q
            
            //LAPACK provides a handy function to do the reconstruction
            Q = copy.template slice<0,0,square_Size, square_Size>(0,0,square_size(), square_size());
            
            int K = square_size();
            M=K;
            N=K;
            lda = K;
            orgqr_(&M, &N, &K, Q.get_data_ptr(), &lda, tau.get_data_ptr(), work, &LWORK, &INFO);

            if(INFO < 0)
                std::cerr << "error in QR, INFO was " << INFO << std::endl;

            delete [] work;
            
            //Now zero out the lower triangle
            for(int r=1; r < square_size(); r++)
                for(int c=0; c<r; c++)
                    copy[r][c] = 0;

            //Now fix the pivot matrix.
            //We need to go from FORTRAN to C numbering. 
            for(int i=0; i < pivot.size(); i++)
                pivot[i]--;
        }

        Matrix<Rows, Cols, Precision, ColMajor> copy;
        Vector<square_Size, Precision> tau;
        Matrix<square_Size, square_Size, Precision, ColMajor> Q;
        bool do_pivoting;
        Vector<Cols, int> pivot;
        

        int square_size()
        {
            return std::min(copy.num_rows(), copy.num_cols());  
        }
};

}


#endif