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() == 1 );
    assert ( A.rows >= A.cols );
    auto sign = [](float value) { return value >= 0 ? 1: -1; };
    const auto totalRows = A.rows;
    const auto totalCols = A.cols;
    R = A.clone();
    Q = cv::Mat::eye ( totalRows, totalRows, A.type() );
    for ( int col = 0; col < A.cols; ++ col )
    {
        cv::Mat matAROI = cv::Mat ( R, cv::Range ( col, totalRows ), cv::Range ( col, totalCols ) );
        cv::Mat y = matAROI.col ( 0 );
        auto yNorm = norm ( y );
        cv::Mat e1 = cv::Mat::eye ( y.rows, 1, A.type() );
        cv::Mat w = y + sign(y.at<float>(0,0)) *  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 - 2 * 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(5, 3, CV_32FC1);
        A.at<float>(0, 0) = 12.f;
        A.at<float>(0, 1) = -51.f;
        A.at<float>(0, 2) = 4.f;

        A.at<float>(1, 0) = 6.f;
        A.at<float>(1, 1) = 167.f;
        A.at<float>(1, 2) = -68.f;

        A.at<float>(2, 0) = -4.f;
        A.at<float>(2, 1) = 24.f;
        A.at<float>(2, 2) = -41.f;

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

        A.at<float>(4, 0) = 2.f;
        A.at<float>(4, 1) = 0.f;
        A.at<float>(4, 2) = 3.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);
}