OpenCV QR分解

#include<cxcore.h>
#include<cv.h>
#include<highgui.h>
#include<stdio.h>
cv::Ptr<CvMat> householder(cv::Ptr<CvMat>& x, cv::Ptr<CvMat>& y);
cv::Ptr<CvMat> blkdiag(const cv::Ptr<CvMat>& Ht, const CvSize& rect);
cv::Ptr<CvMat> blkdiag(const cv::Ptr<CvMat>& Ht, const CvSize& rect);
int ypQR_householder(const cv::Ptr<CvMat>& A, cv::Ptr<CvMat>& Q, cv::Ptr<CvMat>& R)
{
    int width = A->cols;
    int height = A->rows;
    R = cvCloneMat(A);
    Q = cvCreateMat(height, height, A->type);
    cvSetIdentity(Q);
    int end = 0;
    if(width > height)
        end = height;
    else
    end = width;
    for(int i = 0 ; i < end; i++)
    {
        cv::Ptr<CvMat> x = cvCreateMatHeader( height - i, 1, CV_32F);//此处一定要注意
        cvGetSubRect(R, x, cvRect(i, i, 1, height - i));
        cv::Ptr<CvMat> y = cvCreateMat( height - i, 1, CV_32F);
        cvZero(y);
        cvmSet(y, 0, 0, 1);
        cv::Ptr<CvMat> Ht = householder(x, y);
        if(NULL == Ht) return -1;
        cv::Ptr<CvMat> H = blkdiag(Ht, cvSize(Q->cols, Q->rows));
        if(NULL == H) return -1;
        cvmMul(Q, H, Q);
        cvmMul(H, R, R);
    }
    return 0;
}

cv::Ptr<CvMat> householder(cv::Ptr<CvMat>& x, cv::Ptr<CvMat>& y)
{
    int sign = 1;
    if(cvmGet(x, 0, 0) < 0)
    sign = -1;
    float nx = cvNorm(x);
    float ny = cvNorm(y);
    float rho = -1 * sign * nx / ny;
    cvConvertScale(y, y, rho);
    cv::Ptr<CvMat> diff_xy = cvCreateMat(x->rows, x->cols, x->type);
    cvSub(x , y, diff_xy);

    float n_diff_xy = cvNorm(diff_xy);
    cvConvertScale(diff_xy, diff_xy, 1/n_diff_xy);

    cv::Ptr<CvMat> I = cvCreateMat( x->rows, x->rows, x->type);
    cvSetIdentity(I);

    cv::Ptr<CvMat> diff_xy_trans = cvCreateMat(diff_xy->cols, diff_xy->rows, diff_xy->type);
    cvTranspose(diff_xy, diff_xy_trans);

    cv::Ptr<CvMat> diff_xy_mul = cvCreateMat(diff_xy->rows, diff_xy_trans->cols, diff_xy->type);
    cvmMul(diff_xy, diff_xy_trans, diff_xy_mul);
    cvConvertScale( diff_xy_mul, diff_xy_mul, -2);
    cvAdd(I, diff_xy_mul, diff_xy_mul);

    return diff_xy_mul;
}

cv::Ptr<CvMat> blkdiag(const cv::Ptr<CvMat>& Ht, const CvSize& rect)
{
    int H_width = rect.width;
    int H_height = rect.height;

    int Ht_width = Ht->cols;
    int Ht_height = Ht->rows;
    if(H_width < Ht_width || H_height < Ht_height)
        return NULL;
    else if(H_width == Ht_width && H_height == Ht_height)
        return Ht;
    else
    {
        cv::Ptr<CvMat> H = cvCreateMat(rect.height, rect.width, CV_32F);
        cvSetIdentity(H);
        int start_i = H_height - Ht_height;
        int start_j = H_width - Ht_width;

        for(int i = start_i; i < H_height; i++)
        {
            for(int j = start_j; j < H_width; j++)
            {
                cvmSet(H, i, j, cvmGet(Ht, i- start_i, j - start_j));
            }
        }

        return H;
    }
}

/***********************************test*****************************************/
void test_qr()
{
    float a[] = {-5,7, 6,8,-20,26,5,-5,34,16,9,65,10,13,15};
    cv::Ptr<CvMat> A = cvCreateMatHeader(3, 5, CV_32FC1);
    cvInitMatHeader( A, 3, 5, CV_32FC1, a);
    cv::Ptr<CvMat> Q = NULL;
    cv::Ptr<CvMat> R = NULL;
    int re = ypQR_householder(A, Q, R);
    if(-1 == re || NULL == Q || NULL == R) return ;

    cv::Ptr<CvMat> Aq = cvCreateMat( 3, 5,CV_32FC1);
    cvmMul(Q, R, Aq);
    printf("A:\n");
    for(int i=0;i< A->rows;i++)
    {
        for(int j=0;j<A->cols;j++)
            printf("%f ",cvGetReal2D( A, i, j ));
        printf("\n");
    }
    printf("Q:\n");
    for(int i=0;i< Q->rows;i++)
    {
        for(int j=0;j<Q->cols;j++)
            printf("%f ",cvGetReal2D( Q, i, j ));
        printf("\n");
    }
    printf("R:\n");
    for(int i=0;i< R->rows;i++)
    {
        for(int j=0;j<R->cols;j++)
            printf("%f ",cvGetReal2D( R, i, j ));
        printf("\n");
    }
    printf("At:\n");
    for(int i=0;i<Aq->rows;i++)
    {
        printf("\n");
        for(int j=0;j<Aq->cols;j++)
            printf("%f ",cvGetReal2D( Aq, i, j ));
    }
    printf("\n");
    return ;
}
int main(){
    test_qr();
}