Matrix QR Decomposition using OpenCV

Matrix QR decomposition is very useful in least square fitting model. But there is no function available to do it in OpenCV directly. So i write a function to do it myself.

It is based on the House Holder Algorithm. The defailed algorithm can be found in https://en.wikipedia.org/wiki/QR_decomposition.

The function and test code is in below.

void HouseHolderQR(const cv::Mat &A, cv::Mat &Q, cv::Mat &R)
{
    assert ( A.channels() ==  );
    assert ( A.rows >= A.cols );
    auto sign = [](float value) { return value >=  ? : -; };
    const auto totalRows = A.rows;
    const auto totalCols = A.cols;
    R = A.clone();
    Q = cv::Mat::eye ( totalRows, totalRows, A.type() );
    for ( int col = ; col < A.cols; ++ col )
    {
        cv::Mat matAROI = cv::Mat ( R, cv::Range ( col, totalRows ), cv::Range ( col, totalCols ) );
        cv::Mat y = matAROI.col (  );
        auto yNorm = norm ( y );
        cv::Mat e1 = cv::Mat::eye ( y.rows, , A.type() );
        cv::Mat w = y + sign(y.at<float>(,)) *  yNorm * e1;
        cv::Mat v = w / norm( w );
        cv::Mat vT; cv::transpose(v, vT );
        cv::Mat I = cv::Mat::eye( matAROI.rows, matAROI.rows, A.type() );
        cv::Mat I_2VVT = I -  * v * vT;
        cv::Mat matH = cv::Mat::eye ( totalRows, totalRows, A.type() );
        cv::Mat matHROI = cv::Mat(matH, cv::Range ( col, totalRows ), cv::Range ( col, totalRows ) );
        I_2VVT.copyTo ( matHROI );
        R = matH * R;
        Q = Q * matH;
    }
}

void TestQRDecomposition()
{
    cv::Mat A, Q, R;

    //Test case 1
    {
    //A = cv::Mat ( 4, 3, CV_32FC1 );
    //A.at<float>(0,0) = -1.f;
    //A.at<float>(0,1) = -1.f;
    //A.at<float>(0,2) =  1.f;

    //A.at<float>(1,0) =  1.f;
    //A.at<float>(1,1) =  3.f;
    //A.at<float>(1,2) =  3.f;

    //A.at<float>(2,0) = -1.f;
    //A.at<float>(2,1) = -1.f;
    //A.at<float>(2,2) =  5.f;

    //A.at<float>(3,0) =  1.f;
    //A.at<float>(3,1) =  3.f;
    //A.at<float>(3,2) =  7.f;
    }

    {
        A = cv::Mat(, , CV_32FC1);
        A.at<float>(, ) = .f;
        A.at<float>(, ) = -.f;
        A.at<float>(, ) = .f;

        A.at<float>(, ) = .f;
        A.at<float>(, ) = .f;
        A.at<float>(, ) = -.f;

        A.at<float>(, ) = -.f;
        A.at<float>(, ) = .f;
        A.at<float>(, ) = -.f;

        A.at<float>(, ) = -.f;
        A.at<float>(, ) = .f;
        A.at<float>(, ) = .f;

        A.at<float>(, ) = .f;
        A.at<float>(, ) = .f;
        A.at<float>(, ) = .f;
    }

    std::cout << "A: " << std::endl;
    printfMat<float>(A);

    HouseHolderQR(A, Q, R);

    std::cout << "Q: " << std::endl;
    printfMat<float>(Q);

    std::cout << "R: " << std::endl;
    printfMat<float>(R);

    cv::Mat AVerify = Q * R;
    std::cout << "AVerify: " << std::endl;
    printfMat<float>(AVerify);
}