decomposition.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2012. Philipp Wagner <bytefish[at]gmx[dot]de>.
 * Released to public domain under terms of the BSD Simplified license.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the organization nor the names of its contributors
 *     may be used to endorse or promote products derived from this software
 *     without specific prior written permission.
 *
 *   See <http://www.opensource.org/licenses/bsd-license>
 */
#ifndef __DECOMPOSITION_HPP__
#define __DECOMPOSITION_HPP__

#include "opencv2/core/core.hpp"

using namespace cv;
using namespace std;
namespace cv
{

class EigenvalueDecomposition
{
private:

        // Holds the data dimension.
        int n;

        // Stores real/imag part of a complex division.
        double cdivr, cdivi;

        // Pointer to internal memory.
        double *d, *e, *ort;
        double **V, **H;

        // Holds the computed eigenvalues.
        Mat _eigenvalues;

        // Holds the computed eigenvectors.
        Mat _eigenvectors;

        // Allocates memory.
        template<typename _Tp>
        _Tp *alloc_1d(int m)
        {
                return new _Tp[m];
        }

        // Allocates memory.
        template<typename _Tp>
        _Tp *alloc_1d(int m, _Tp val)
        {
                _Tp *arr = alloc_1d<_Tp>(m);
                for (int i = 0; i < m; i++)
                        arr[i] = val;
                return arr;
        }

        // Allocates memory.
        template<typename _Tp>
        _Tp **alloc_2d(int m, int n)
        {
                _Tp **arr = new _Tp*[m];
                for (int i = 0; i < m; i++)
                        arr[i] = new _Tp[n];
                return arr;
        }

        // Allocates memory.
        template<typename _Tp>
        _Tp **alloc_2d(int m, int n, _Tp val)
        {
                _Tp **arr = alloc_2d<_Tp>(m, n);
                for (int i = 0; i < m; i++)
                {
                        for (int j = 0; j < n; j++)
                        {
                                arr[i][j] = val;
                        }
                }
                return arr;
        }

        void cdiv(double xr, double xi, double yr, double yi)
        {
                double r, d;
                if (std::abs(yr) > std::abs(yi))
                {
                        r = yi / yr;
                        d = yr + r * yi;
                        cdivr = (xr + r * xi) / d;
                        cdivi = (xi - r * xr) / d;
                }
                else
                {
                        r = yr / yi;
                        d = yi + r * yr;
                        cdivr = (r * xr + xi) / d;
                        cdivi = (r * xi - xr) / d;
                }
        }

        // Nonsymmetric reduction from Hessenberg to real Schur form.

        void hqr2()
        {

                //  This is derived from the Algol procedure hqr2,
                //  by Martin and Wilkinson, Handbook for Auto. Comp.,
                //  Vol.ii-Linear Algebra, and the corresponding
                //  Fortran subroutine in EISPACK.

                // Initialize
                int nn = this->n;
                int n = nn - 1;
                int low = 0;
                int high = nn - 1;
                double eps = pow(2.0, -52.0);
                double exshift = 0.0;
                double p = 0, q = 0, r = 0, s = 0, z = 0, t, w, x, y;

                // Store roots isolated by balanc and compute matrix norm

                double norm = 0.0;
                for (int i = 0; i < nn; i++)
                {
                        if (i < low | i > high)
                        {
                                d[i] = H[i][i];
                                e[i] = 0.0;
                        }
                        for (int j = max(i - 1, 0); j < nn; j++)
                        {
                                norm = norm + std::abs(H[i][j]);
                        }
                }

                // Outer loop over eigenvalue index
                int iter = 0;
                while (n >= low)
                {

                        // Look for single small sub-diagonal element
                        int l = n;
                        while (l > low)
                        {
                                s = std::abs(H[l - 1][l - 1]) + std::abs(H[l][l]);
                                if (s == 0.0)
                                {
                                        s = norm;
                                }
                                if (std::abs(H[l][l - 1]) < eps * s)
                                {
                                        break;
                                }
                                l--;
                        }

                        // Check for convergence
                        // One root found

                        if (l == n)
                        {
                                H[n][n] = H[n][n] + exshift;
                                d[n] = H[n][n];
                                e[n] = 0.0;
                                n--;
                                iter = 0;

                                // Two roots found

                        }
                        else if (l == n - 1)
                        {
                                w = H[n][n - 1] * H[n - 1][n];
                                p = (H[n - 1][n - 1] - H[n][n]) / 2.0;
                                q = p * p + w;
                                z = sqrt(std::abs(q));
                                H[n][n] = H[n][n] + exshift;
                                H[n - 1][n - 1] = H[n - 1][n - 1] + exshift;
                                x = H[n][n];

                                // Real pair

                                if (q >= 0)
                                {
                                        if (p >= 0)
                                        {
                                                z = p + z;
                                        }
                                        else
                                        {
                                                z = p - z;
                                        }
                                        d[n - 1] = x + z;
                                        d[n] = d[n - 1];
                                        if (z != 0.0)
                                        {
                                                d[n] = x - w / z;
                                        }
                                        e[n - 1] = 0.0;
                                        e[n] = 0.0;
                                        x = H[n][n - 1];
                                        s = std::abs(x) + std::abs(z);
                                        p = x / s;
                                        q = z / s;
                                        r = sqrt(p * p + q * q);
                                        p = p / r;
                                        q = q / r;

                                        // Row modification

                                        for (int j = n - 1; j < nn; j++)
                                        {
                                                z = H[n - 1][j];
                                                H[n - 1][j] = q * z + p * H[n][j];
                                                H[n][j] = q * H[n][j] - p * z;
                                        }

                                        // Column modification

                                        for (int i = 0; i <= n; i++)
                                        {
                                                z = H[i][n - 1];
                                                H[i][n - 1] = q * z + p * H[i][n];
                                                H[i][n] = q * H[i][n] - p * z;
                                        }

                                        // Accumulate transformations

                                        for (int i = low; i <= high; i++)
                                        {
                                                z = V[i][n - 1];
                                                V[i][n - 1] = q * z + p * V[i][n];
                                                V[i][n] = q * V[i][n] - p * z;
                                        }

                                        // Complex pair

                                }
                                else
                                {
                                        d[n - 1] = x + p;
                                        d[n] = x + p;
                                        e[n - 1] = z;
                                        e[n] = -z;
                                }
                                n = n - 2;
                                iter = 0;

                                // No convergence yet

                        }
                        else
                        {

                                // Form shift

                                x = H[n][n];
                                y = 0.0;
                                w = 0.0;
                                if (l < n)
                                {
                                        y = H[n - 1][n - 1];
                                        w = H[n][n - 1] * H[n - 1][n];
                                }

                                // Wilkinson's original ad hoc shift

                                if (iter == 10)
                                {
                                        exshift += x;
                                        for (int i = low; i <= n; i++)
                                        {
                                                H[i][i] -= x;
                                        }
                                        s = std::abs(H[n][n - 1]) + std::abs(H[n - 1][n - 2]);
                                        x = y = 0.75 * s;
                                        w = -0.4375 * s * s;
                                }

                                // MATLAB's new ad hoc shift

                                if (iter == 30)
                                {
                                        s = (y - x) / 2.0;
                                        s = s * s + w;
                                        if (s > 0)
                                        {
                                                s = sqrt(s);
                                                if (y < x)
                                                {
                                                        s = -s;
                                                }
                                                s = x - w / ((y - x) / 2.0 + s);
                                                for (int i = low; i <= n; i++)
                                                {
                                                        H[i][i] -= s;
                                                }
                                                exshift += s;
                                                x = y = w = 0.964;
                                        }
                                }

                                iter = iter + 1; // (Could check iteration count here.)

                                // Look for two consecutive small sub-diagonal elements
                                int m = n - 2;
                                while (m >= l)
                                {
                                        z = H[m][m];
                                        r = x - z;
                                        s = y - z;
                                        p = (r * s - w) / H[m + 1][m] + H[m][m + 1];
                                        q = H[m + 1][m + 1] - z - r - s;
                                        r = H[m + 2][m + 1];
                                        s = std::abs(p) + std::abs(q) + std::abs(r);
                                        p = p / s;
                                        q = q / s;
                                        r = r / s;
                                        if (m == l)
                                        {
                                                break;
                                        }
                                        if (std::abs(H[m][m - 1]) * (std::abs(q) + std::abs(r)) < eps * (std::abs(p) * (std::abs(H[m - 1][m - 1]) + std::abs(z) + std::abs(H[m + 1][m + 1]))))
                                        {
                                                break;
                                        }
                                        m--;
                                }

                                for (int i = m + 2; i <= n; i++)
                                {
                                        H[i][i - 2] = 0.0;
                                        if (i > m + 2)
                                        {
                                                H[i][i - 3] = 0.0;
                                        }
                                }

                                // Double QR step involving rows l:n and columns m:n

                                for (int k = m; k <= n - 1; k++)
                                {
                                        bool notlast = (k != n - 1);
                                        if (k != m)
                                        {
                                                p = H[k][k - 1];
                                                q = H[k + 1][k - 1];
                                                r = (notlast ? H[k + 2][k - 1] : 0.0);
                                                x = std::abs(p) + std::abs(q) + std::abs(r);
                                                if (x != 0.0)
                                                {
                                                        p = p / x;
                                                        q = q / x;
                                                        r = r / x;
                                                }
                                        }
                                        if (x == 0.0)
                                        {
                                                break;
                                        }
                                        s = sqrt(p * p + q * q + r * r);
                                        if (p < 0)
                                        {
                                                s = -s;
                                        }
                                        if (s != 0)
                                        {
                                                if (k != m)
                                                {
                                                        H[k][k - 1] = -s * x;
                                                }
                                                else if (l != m)
                                                {
                                                        H[k][k - 1] = -H[k][k - 1];
                                                }
                                                p = p + s;
                                                x = p / s;
                                                y = q / s;
                                                z = r / s;
                                                q = q / p;
                                                r = r / p;

                                                // Row modification

                                                for (int j = k; j < nn; j++)
                                                {
                                                        p = H[k][j] + q * H[k + 1][j];
                                                        if (notlast)
                                                        {
                                                                p = p + r * H[k + 2][j];
                                                                H[k + 2][j] = H[k + 2][j] - p * z;
                                                        }
                                                        H[k][j] = H[k][j] - p * x;
                                                        H[k + 1][j] = H[k + 1][j] - p * y;
                                                }

                                                // Column modification

                                                for (int i = 0; i <= min(n, k + 3); i++)
                                                {
                                                        p = x * H[i][k] + y * H[i][k + 1];
                                                        if (notlast)
                                                        {
                                                                p = p + z * H[i][k + 2];
                                                                H[i][k + 2] = H[i][k + 2] - p * r;
                                                        }
                                                        H[i][k] = H[i][k] - p;
                                                        H[i][k + 1] = H[i][k + 1] - p * q;
                                                }

                                                // Accumulate transformations

                                                for (int i = low; i <= high; i++)
                                                {
                                                        p = x * V[i][k] + y * V[i][k + 1];
                                                        if (notlast)
                                                        {
                                                                p = p + z * V[i][k + 2];
                                                                V[i][k + 2] = V[i][k + 2] - p * r;
                                                        }
                                                        V[i][k] = V[i][k] - p;
                                                        V[i][k + 1] = V[i][k + 1] - p * q;
                                                }
                                        } // (s != 0)
                                } // k loop
                        } // check convergence
                } // while (n >= low)

                // Backsubstitute to find vectors of upper triangular form

                if (norm == 0.0)
                {
                        return;
                }

                for (n = nn - 1; n >= 0; n--)
                {
                        p = d[n];
                        q = e[n];

                        // Real vector

                        if (q == 0)
                        {
                                int l = n;
                                H[n][n] = 1.0;
                                for (int i = n - 1; i >= 0; i--)
                                {
                                        w = H[i][i] - p;
                                        r = 0.0;
                                        for (int j = l; j <= n; j++)
                                        {
                                                r = r + H[i][j] * H[j][n];
                                        }
                                        if (e[i] < 0.0)
                                        {
                                                z = w;
                                                s = r;
                                        }
                                        else
                                        {
                                                l = i;
                                                if (e[i] == 0.0)
                                                {
                                                        if (w != 0.0)
                                                        {
                                                                H[i][n] = -r / w;
                                                        }
                                                        else
                                                        {
                                                                H[i][n] = -r / (eps * norm);
                                                        }

                                                        // Solve real equations

                                                }
                                                else
                                                {
                                                        x = H[i][i + 1];
                                                        y = H[i + 1][i];
                                                        q = (d[i] - p) * (d[i] - p) + e[i] * e[i];
                                                        t = (x * s - z * r) / q;
                                                        H[i][n] = t;
                                                        if (std::abs(x) > std::abs(z))
                                                        {
                                                                H[i + 1][n] = (-r - w * t) / x;
                                                        }
                                                        else
                                                        {
                                                                H[i + 1][n] = (-s - y * t) / z;
                                                        }
                                                }

                                                // Overflow control

                                                t = std::abs(H[i][n]);
                                                if ((eps * t) * t > 1)
                                                {
                                                        for (int j = i; j <= n; j++)
                                                        {
                                                                H[j][n] = H[j][n] / t;
                                                        }
                                                }
                                        }
                                }

                                // Complex vector

                        }
                        else if (q < 0)
                        {
                                int l = n - 1;

                                // Last vector component imaginary so matrix is triangular

                                if (std::abs(H[n][n - 1]) > std::abs(H[n - 1][n]))
                                {
                                        H[n - 1][n - 1] = q / H[n][n - 1];
                                        H[n - 1][n] = -(H[n][n] - p) / H[n][n - 1];
                                }
                                else
                                {
                                        cdiv(0.0, -H[n - 1][n], H[n - 1][n - 1] - p, q);
                                        H[n - 1][n - 1] = cdivr;
                                        H[n - 1][n] = cdivi;
                                }
                                H[n][n - 1] = 0.0;
                                H[n][n] = 1.0;
                                for (int i = n - 2; i >= 0; i--)
                                {
                                        double ra, sa, vr, vi;
                                        ra = 0.0;
                                        sa = 0.0;
                                        for (int j = l; j <= n; j++)
                                        {
                                                ra = ra + H[i][j] * H[j][n - 1];
                                                sa = sa + H[i][j] * H[j][n];
                                        }
                                        w = H[i][i] - p;

                                        if (e[i] < 0.0)
                                        {
                                                z = w;
                                                r = ra;
                                                s = sa;
                                        }
                                        else
                                        {
                                                l = i;
                                                if (e[i] == 0)
                                                {
                                                        cdiv(-ra, -sa, w, q);
                                                        H[i][n - 1] = cdivr;
                                                        H[i][n] = cdivi;
                                                }
                                                else
                                                {

                                                        // Solve complex equations

                                                        x = H[i][i + 1];
                                                        y = H[i + 1][i];
                                                        vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q;
                                                        vi = (d[i] - p) * 2.0 * q;
                                                        if (vr == 0.0 & vi == 0.0)
                                                        {
                                                                vr = eps * norm * (std::abs(w) + std::abs(q) + std::abs(x) + std::abs(y) + std::abs(z));
                                                        }
                                                        cdiv(x * r - z * ra + q * sa, x * s - z * sa - q * ra, vr, vi);
                                                        H[i][n - 1] = cdivr;
                                                        H[i][n] = cdivi;
                                                        if (std::abs(x) > (std::abs(z) + std::abs(q)))
                                                        {
                                                                H[i + 1][n - 1] = (-ra - w * H[i][n - 1] + q * H[i][n]) / x;
                                                                H[i + 1][n] = (-sa - w * H[i][n] - q * H[i][n - 1]) / x;
                                                        }
                                                        else
                                                        {
                                                                cdiv(-r - y * H[i][n - 1], -s - y * H[i][n], z, q);
                                                                H[i + 1][n - 1] = cdivr;
                                                                H[i + 1][n] = cdivi;
                                                        }
                                                }

                                                // Overflow control

                                                t = max(std::abs(H[i][n - 1]), std::abs(H[i][n]));
                                                if ((eps * t) * t > 1)
                                                {
                                                        for (int j = i; j <= n; j++)
                                                        {
                                                                H[j][n - 1] = H[j][n - 1] / t;
                                                                H[j][n] = H[j][n] / t;
                                                        }
                                                }
                                        }
                                }
                        }
                }

                // Vectors of isolated roots

                for (int i = 0; i < nn; i++)
                {
                        if (i < low | i > high)
                        {
                                for (int j = i; j < nn; j++)
                                {
                                        V[i][j] = H[i][j];
                                }
                        }
                }

                // Back transformation to get eigenvectors of original matrix

                for (int j = nn - 1; j >= low; j--)
                {
                        for (int i = low; i <= high; i++)
                        {
                                z = 0.0;
                                for (int k = low; k <= min(j, high); k++)
                                {
                                        z = z + V[i][k] * H[k][j];
                                }
                                V[i][j] = z;
                        }
                }
        }

        // Nonsymmetric reduction to Hessenberg form.
        void orthes()
        {
                //  This is derived from the Algol procedures orthes and ortran,
                //  by Martin and Wilkinson, Handbook for Auto. Comp.,
                //  Vol.ii-Linear Algebra, and the corresponding
                //  Fortran subroutines in EISPACK.
                int low = 0;
                int high = n - 1;

                for (int m = low + 1; m <= high - 1; m++)
                {

                        // Scale column.

                        double scale = 0.0;
                        for (int i = m; i <= high; i++)
                        {
                                scale = scale + std::abs(H[i][m - 1]);
                        }
                        if (scale != 0.0)
                        {

                                // Compute Householder transformation.

                                double h = 0.0;
                                for (int i = high; i >= m; i--)
                                {
                                        ort[i] = H[i][m - 1] / scale;
                                        h += ort[i] * ort[i];
                                }
                                double g = sqrt(h);
                                if (ort[m] > 0)
                                {
                                        g = -g;
                                }
                                h = h - ort[m] * g;
                                ort[m] = ort[m] - g;

                                // Apply Householder similarity transformation
                                // H = (I-u*u'/h)*H*(I-u*u')/h)

                                for (int j = m; j < n; j++)
                                {
                                        double f = 0.0;
                                        for (int i = high; i >= m; i--)
                                        {
                                                f += ort[i] * H[i][j];
                                        }
                                        f = f / h;
                                        for (int i = m; i <= high; i++)
                                        {
                                                H[i][j] -= f * ort[i];
                                        }
                                }

                                for (int i = 0; i <= high; i++)
                                {
                                        double f = 0.0;
                                        for (int j = high; j >= m; j--)
                                        {
                                                f += ort[j] * H[i][j];
                                        }
                                        f = f / h;
                                        for (int j = m; j <= high; j++)
                                        {
                                                H[i][j] -= f * ort[j];
                                        }
                                }
                                ort[m] = scale * ort[m];
                                H[m][m - 1] = scale * g;
                        }
                }

                // Accumulate transformations (Algol's ortran).

                for (int i = 0; i < n; i++)
                {
                        for (int j = 0; j < n; j++)
                        {
                                V[i][j] = (i == j ? 1.0 : 0.0);
                        }
                }

                for (int m = high - 1; m >= low + 1; m--)
                {
                        if (H[m][m - 1] != 0.0)
                        {
                                for (int i = m + 1; i <= high; i++)
                                {
                                        ort[i] = H[i][m - 1];
                                }
                                for (int j = m; j <= high; j++)
                                {
                                        double g = 0.0;
                                        for (int i = m; i <= high; i++)
                                        {
                                                g += ort[i] * V[i][j];
                                        }
                                        // Double division avoids possible underflow
                                        g = (g / ort[m]) / H[m][m - 1];
                                        for (int i = m; i <= high; i++)
                                        {
                                                V[i][j] += g * ort[i];
                                        }
                                }
                        }
                }
        }

        // Releases all internal working memory.
        void release()
        {
                // releases the working data
                delete[] d, e, ort;
                for (int i = 0; i < n; i++)
                {
                        delete[] H[i];
                        delete[] V[i];
                }
                delete[] H, V;
        }

        // Computes the Eigenvalue Decomposition for a matrix given in H.
        void compute()
        {
                // Allocate memory for the working data.
                V = alloc_2d<double>(n, n, 0.0);
                d = alloc_1d<double>(n);
                e = alloc_1d<double>(n);
                ort = alloc_1d<double>(n);
                // Reduce to Hessenberg form.
                orthes();
                // Reduce Hessenberg to real Schur form.
                hqr2();
                // Copy eigenvalues to OpenCV Matrix.
                _eigenvalues.create(1, n, CV_64FC1);
                for (int i = 0; i < n; i++)
                {
                        _eigenvalues.at<double>(0, i) = d[i];
                }
                // Copy eigenvectors to OpenCV Matrix.
                _eigenvectors.create(n, n, CV_64FC1);
                for (int i = 0; i < n; i++)
                        for (int j = 0; j < n; j++)
                                _eigenvectors.at<double>(i, j) = V[i][j];
                // Deallocate the memory by releasing all internal working data.
                release();
        }

public:
        EigenvalueDecomposition() :
                n(0)
        {
        }

        // Initializes & computes the Eigenvalue Decomposition for a general matrix
        // given in src. This function is a port of the EigenvalueSolver in JAMA,
        // which has been released to public domain by The MathWorks and the
        // National Institute of Standards and Technology (NIST).
        EigenvalueDecomposition(InputArray src)
        {
                compute(src);
        }

        // This function computes the Eigenvalue Decomposition for a general matrix
        // given in src. This function is a port of the EigenvalueSolver in JAMA,
        // which has been released to public domain by The MathWorks and the
        // National Institute of Standards and Technology (NIST).
        void compute(InputArray src);

        ~EigenvalueDecomposition()
        {
        }

        // Returns the eigenvalues of the Eigenvalue Decomposition.
        Mat eigenvalues()
        {
                return _eigenvalues;
        }
        // Returns the eigenvectors of the Eigenvalue Decomposition.
        Mat eigenvectors()
        {
                return _eigenvectors;
        }
};

} // namespace
#endif