Add QR decomposition to HAL

2026-07-29 15:23:05 +04:00 · 2016-08-23 16:35:03 +03:00
parent d102ea96c0
commit dfe4519c07
10 changed files with 348 additions and 5 deletions
@@ -225,6 +225,129 @@ bool Cholesky64f(double* A, size_t astep, int m, double* b, size_t bstep, int n)
    return CholImpl(A, astep, m, b, bstep, n);
 }

+template<typename _Tp> inline static int
+sign(_Tp x)
+{
+    if (x >= (_Tp)0)
+        return 1;
+    else
+        return -1;
+}
+
+template<typename _Tp> static inline int
+QRImpl(_Tp* A, size_t astep, int m, int n, int k, _Tp* b, size_t bstep, _Tp* hFactors, _Tp eps)
+{
+    astep /= sizeof(_Tp);
+    bstep /= sizeof(_Tp);
+
+    cv::AutoBuffer<_Tp> buffer;
+    size_t buf_size = m ? m + n : hFactors != NULL;
+    buffer.allocate(buf_size);
+    _Tp* vl = buffer;
+    if (hFactors == NULL)
+        hFactors = vl + m;
+
+    for (int l = 0; l < n; l++)
+    {
+        //generate vl
+        int vlSize = m - l;
+        _Tp vlNorm = (_Tp)0;
+        for (int i = 0; i < vlSize; i++)
+        {
+            vl[i] = A[(l + i)*astep + l];
+            vlNorm += vl[i] * vl[i];
+        }
+        _Tp tmpV = vl[0];
+        vl[0] = vl[0] + sign(vl[0])*std::sqrt(vlNorm);
+        vlNorm = std::sqrt(vlNorm + vl[0] * vl[0] - tmpV*tmpV);
+        for (int i = 0; i < vlSize; i++)
+        {
+            vl[i] /= vlNorm;
+        }
+        //multiply A_l*vl
+        for (int j = l; j < n; j++)
+        {
+            _Tp v_lA = (_Tp)0;
+            for (int i = l; i < m; i++)
+            {
+                v_lA += vl[i - l] * A[i*astep + j];
+            }
+
+            for (int i = l; i < m; i++)
+            {
+                A[i*astep + j] -= 2 * vl[i - l] * v_lA;
+            }
+        }
+
+        //save vl and factors
+        hFactors[l] = vl[0] * vl[0];
+        for (int i = 1; i < vlSize; i++)
+        {
+            A[(l + i)*astep + l] = vl[i] / vl[0];
+        }
+    }
+
+    if (b)
+    {
+        //generate new rhs
+        for (int l = 0; l < n; l++)
+        {
+            //unpack vl
+            vl[0] = (_Tp)1;
+            for (int j = 1; j < m - l; j++)
+            {
+              vl[j] = A[(j + l)*astep + l];
+            }
+
+            //h_l*x
+            for (int j = 0; j < k; j++)
+            {
+                _Tp v_lB = (_Tp)0;
+                for (int i = l; i < m; i++)
+                  v_lB += vl[i - l] * b[i*bstep + j];
+
+                for (int i = l; i < m; i++)
+                    b[i*bstep + j] -= 2 * vl[i - l] * v_lB * hFactors[l];
+            }
+        }
+        //do back substitution
+        for (int i = n - 1; i >= 0; i--)
+        {
+            for (int j = n - 1; j > i; j--)
+            {
+                for (int p = 0; p < k; p++)
+                    b[i*bstep + p] -= b[j*bstep + p] * A[i*astep + j];
+            }
+            if (std::abs(A[i*astep + i]) < eps)
+                return 0;
+            for (int p = 0; p < k; p++)
+                b[i*bstep + p] /= A[i*astep + i];
+        }
+    }
+
+    return 1;
+}
+
+int QR32f(float* A, size_t astep, int m, int n, int k, float* b, size_t bstep, float* hFactors)
+{
+    CV_INSTRUMENT_REGION()
+
+    int output;
+    CALL_HAL_RET(QR32f, cv_hal_QR32f, output, A, astep, m, n, k, b, bstep, hFactors);
+    output = QRImpl(A, astep, m, n, k, b, bstep, hFactors, FLT_EPSILON * 10);
+    return output;
+}
+
+int QR64f(double* A, size_t astep, int m, int n, int k, double* b, size_t bstep, double* hFactors)
+{
+    CV_INSTRUMENT_REGION()
+
+    int output;
+    CALL_HAL_RET(QR64f, cv_hal_QR64f, output, A, astep, m, n, k, b, bstep, hFactors)
+    output = QRImpl(A, astep, m, n, k, b, bstep, hFactors, DBL_EPSILON * 100);
+    return output;
+}
+
 //=============================================================================
 // for compatibility with 3.0