source: cpp/frams/model/similarity/SVD/lapack.cpp @ 1302

// This file is a part of Framsticks SDK.  http://www.framsticks.com/
// Copyright (C) 1999-2015  Maciej Komosinski and Szymon Ulatowski.
// See LICENSE.txt for details.

////////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's 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.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//
/*
 * AutoBuffer from https://github.com/Itseez/opencv/blob/master/modules/core/include/opencv2/core/utility.hpp
 * VBLAS, JacobiSVDImpl_ from https://github.com/Itseez/opencv/blob/master/modules/core/src/lapack.cpp
 * changes MK, May 2015:
 * - "RNG rng(0x12345678)" and "rng.next()" replaced with a local PRBS-7 implementation so that this file does not depend on external random number generators.
 */
#include <cstdlib>
#include <algorithm>
#include <cmath>
#include <limits>
#include <cfloat>
//#include <assert.h>
#include <math.h> //hypot(), embarcadero
#include <stdint.h> //uint8_t
#include "lapack.h"
#include <common/log.h>


#if defined _M_IX86 && defined _MSC_VER && _MSC_VER < 1700
#pragma float_control(precise, on)
#endif

template<typename _Tp, size_t fixed_size = 1024 / sizeof(_Tp) + 8 > class AutoBuffer
{
public:
        typedef _Tp value_type;

        //! the default constructor
        AutoBuffer();
        //! constructor taking the real buffer size
        AutoBuffer(size_t _size);

        //! the copy constructor
        AutoBuffer(const AutoBuffer<_Tp, fixed_size>& buf);
        //! the assignment operator
        AutoBuffer<_Tp, fixed_size>& operator=(const AutoBuffer<_Tp, fixed_size>& buf);

        //! destructor. calls deallocate()
        ~AutoBuffer();

        //! allocates the new buffer of size _size. if the _size is small enough, stack-allocated buffer is used
        void allocate(size_t _size);
        //! deallocates the buffer if it was dynamically allocated
        void deallocate();
        //! resizes the buffer and preserves the content
        void resize(size_t _size);
        //! returns the current buffer size
        size_t size() const;
        //! returns pointer to the real buffer, stack-allocated or head-allocated
        operator _Tp* ();
        //! returns read-only pointer to the real buffer, stack-allocated or head-allocated
        operator const _Tp* () const;

protected:
        //! pointer to the real buffer, can point to buf if the buffer is small enough
        _Tp* ptr;
        //! size of the real buffer
        size_t sz;
        //! pre-allocated buffer. At least 1 element to confirm C++ standard reqirements
        _Tp buf[(fixed_size > 0) ? fixed_size : 1];
};

/////////////////////////////// AutoBuffer implementation ////////////////////////////////////////

template<typename _Tp, size_t fixed_size> inline
AutoBuffer<_Tp, fixed_size>::AutoBuffer()
{
        ptr = buf;
        sz = fixed_size;
}

template<typename _Tp, size_t fixed_size> inline
AutoBuffer<_Tp, fixed_size>::AutoBuffer(size_t _size)
{
        ptr = buf;
        sz = fixed_size;
        allocate(_size);
}

template<typename _Tp, size_t fixed_size> inline
AutoBuffer<_Tp, fixed_size>::AutoBuffer(const AutoBuffer<_Tp, fixed_size>& abuf)
{
        ptr = buf;
        sz = fixed_size;
        allocate(abuf.size());
        for (size_t i = 0; i < sz; i++)
                ptr[i] = abuf.ptr[i];
}

template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>&
AutoBuffer<_Tp, fixed_size>::operator=(const AutoBuffer<_Tp, fixed_size>& abuf)
{
        if (this != &abuf)
        {
                deallocate();
                allocate(abuf.size());
                for (size_t i = 0; i < sz; i++)
                        ptr[i] = abuf.ptr[i];
        }
        return *this;
}

template<typename _Tp, size_t fixed_size> inline
AutoBuffer<_Tp, fixed_size>::~AutoBuffer()
{
        deallocate();
}

template<typename _Tp, size_t fixed_size> inline void
AutoBuffer<_Tp, fixed_size>::allocate(size_t _size)
{
        if (_size <= sz)
        {
                sz = _size;
                return;
        }
        deallocate();
        if (_size > fixed_size)
        {
                ptr = new _Tp[_size];
                sz = _size;
        }
}

template<typename _Tp, size_t fixed_size> inline void
AutoBuffer<_Tp, fixed_size>::deallocate()
{
        if (ptr != buf)
        {
                delete[] ptr;
                ptr = buf;
                sz = fixed_size;
        }
}

template<typename _Tp, size_t fixed_size> inline void
AutoBuffer<_Tp, fixed_size>::resize(size_t _size)
{
        if (_size <= sz)
        {
                sz = _size;
                return;
        }
        //size_t i, prevsize = sz, minsize = MIN(prevsize, _size);
        size_t i, prevsize = sz, minsize = prevsize < _size ? prevsize : _size;
        _Tp* prevptr = ptr;

        ptr = _size > fixed_size ? new _Tp[_size] : buf;
        sz = _size;

        if (ptr != prevptr)
                for (i = 0; i < minsize; i++)
                        ptr[i] = prevptr[i];
        for (i = prevsize; i < _size; i++)
                ptr[i] = _Tp();

        if (prevptr != buf)
                delete[] prevptr;
}

template<typename _Tp, size_t fixed_size> inline size_t
AutoBuffer<_Tp, fixed_size>::size() const
{
        return sz;
}

template<typename _Tp, size_t fixed_size> inline
AutoBuffer<_Tp, fixed_size>::operator _Tp* ()
{
        return ptr;
}

template<typename _Tp, size_t fixed_size> inline
AutoBuffer<_Tp, fixed_size>::operator const _Tp* () const
{
        return ptr;
}

template<typename T> struct VBLAS
{

        int dot(const T*, const T*, int, T*) const
        {
                return 0;
        }

        int givens(T*, T*, int, T, T) const
        {
                return 0;
        }

        int givensx(T*, T*, int, T, T, T*, T*) const
        {
                return 0;
        }
};

template<typename _Tp> void
JacobiSVDImpl_(_Tp* At, size_t astep, _Tp* _W, _Tp* Vt, size_t vstep,
int m, int n, int n1, double minval, _Tp eps)
{
        VBLAS<_Tp> vblas;
        AutoBuffer<double> Wbuf(n);
        double* W = Wbuf;
        int i, j, k, iter, max_iter = std::max(m, 30);
        _Tp c, s;
        double sd;
        astep /= sizeof(At[0]);
        vstep /= sizeof(Vt[0]);

        for (i = 0; i < n; i++)
        {
                for (k = 0, sd = 0; k < m; k++)
                {
                        _Tp t = At[i * astep + k];
                        sd += (double)t*t;
                }
                W[i] = sd;

                if (Vt)
                {
                        for (k = 0; k < n; k++)
                                Vt[i * vstep + k] = 0;
                        Vt[i * vstep + i] = 1;
                }
        }

        for (iter = 0; iter < max_iter; iter++)
        {
                bool changed = false;

                for (i = 0; i < n - 1; i++)
                        for (j = i + 1; j < n; j++)
                        {
                        _Tp *Ai = At + i*astep, *Aj = At + j*astep;
                        double a = W[i], p = 0, b = W[j];

                        for (k = 0; k < m; k++)
                                p += (double)Ai[k] * Aj[k];

                        if (std::abs(p) <= eps * std::sqrt((double)a * b))
                                continue;

                        p *= 2;
                        double beta = a - b, gamma = hypot((double)p, beta);
                        if (beta < 0)
                        {
                                double delta = (gamma - beta)*0.5;
                                s = (_Tp)std::sqrt(delta / gamma);
                                c = (_Tp)(p / (gamma * s * 2));
                        }
                        else
                        {
                                if (gamma == 0) //MK 2020-12: workaround for embarcadero compiler
                                {
#ifndef __BORLANDC__ //It happens so often in embarcadero-produced executables that don't even bother the user with messages. Visual c++ and gcc/clang do not experience this problem.
                                        logPrintf("Lapack", "JacobiSVD", LOG_ERROR, "Numerical instability in JacobiSVDImpl_(), likely 0.0/0.0, specific to floating point handling by this particular compiler. p=%.17g, gamma=%.17g, beta=%.17g. Recovering with bogus results.", p, gamma, beta);
#endif
                                        c = s = 0;
                                }
                                else
                                {
                                        c = (_Tp)std::sqrt((gamma + beta) / (gamma * 2));
                                        s = (_Tp)(p / (gamma * c * 2));
                                }
                        }

                        a = b = 0;
                        for (k = 0; k < m; k++)
                        {
                                _Tp t0 = c * Ai[k] + s * Aj[k];
                                _Tp t1 = -s * Ai[k] + c * Aj[k];
                                Ai[k] = t0;
                                Aj[k] = t1;

                                a += (double)t0*t0;
                                b += (double)t1*t1;
                        }
                        W[i] = a;
                        W[j] = b;

                        changed = true;

                        if (Vt)
                        {
                                _Tp *Vi = Vt + i*vstep, *Vj = Vt + j*vstep;
                                k = vblas.givens(Vi, Vj, n, c, s);

                                for (; k < n; k++)
                                {
                                        _Tp t0 = c * Vi[k] + s * Vj[k];
                                        _Tp t1 = -s * Vi[k] + c * Vj[k];
                                        Vi[k] = t0;
                                        Vj[k] = t1;
                                }
                        }
                        }
                if (!changed)
                        break;
        }

        for (i = 0; i < n; i++)
        {
                for (k = 0, sd = 0; k < m; k++)
                {
                        _Tp t = At[i * astep + k];
                        sd += (double)t*t;
                }
                W[i] = std::sqrt(sd);
        }

        for (i = 0; i < n - 1; i++)
        {
                j = i;
                for (k = i + 1; k < n; k++)
                {
                        if (W[j] < W[k])
                                j = k;
                }
                if (i != j)
                {
                        std::swap(W[i], W[j]);
                        if (Vt)
                        {
                                for (k = 0; k < m; k++)
                                        std::swap(At[i * astep + k], At[j * astep + k]);

                                for (k = 0; k < n; k++)
                                        std::swap(Vt[i * vstep + k], Vt[j * vstep + k]);
                        }
                }
        }

        for (i = 0; i < n; i++)
                _W[i] = (_Tp)W[i];

        if (!Vt)
                return;

        uint8_t rndstate = 0x02; //PRBS-7 from http://en.wikipedia.org/wiki/Pseudorandom_binary_sequence
        for (i = 0; i < n1; i++)
        {
                sd = i < n ? W[i] : 0;

                while (sd <= minval)
                {
                        // if we got a zero singular value, then in order to get the corresponding left singular vector
                        // we generate a random vector, project it to the previously computed left singular vectors,
                        // subtract the projection and normalize the difference.
                        const _Tp val0 = (_Tp)(1. / m);
                        for (k = 0; k < m; k++)
                        {
                                int rndbit = (((rndstate >> 6) ^ (rndstate >> 5)) & 1);
                                rndstate = ((rndstate << 1) | rndbit) & 0x7f;
                                _Tp val = rndbit == 0 ? val0 : -val0;
                                At[i * astep + k] = val;
                        }
                        for (iter = 0; iter < 2; iter++)
                        {
                                for (j = 0; j < i; j++)
                                {
                                        sd = 0;
                                        for (k = 0; k < m; k++)
                                                sd += At[i * astep + k] * At[j * astep + k];
                                        _Tp asum = 0;
                                        for (k = 0; k < m; k++)
                                        {
                                                _Tp t = (_Tp)(At[i * astep + k] - sd * At[j * astep + k]);
                                                At[i * astep + k] = t;
                                                asum += std::abs(t);
                                        }
                                        asum = asum ? 1 / asum : 0;
                                        for (k = 0; k < m; k++)
                                                At[i * astep + k] *= asum;
                                }
                        }
                        sd = 0;
                        for (k = 0; k < m; k++)
                        {
                                _Tp t = At[i * astep + k];
                                sd += (double)t*t;
                        }
                        sd = std::sqrt(sd);
                }

                s = (_Tp)(1 / sd);
                for (k = 0; k < m; k++)
                        At[i * astep + k] *= s;
        }
}

void Lapack::JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1 = -1)
{
        JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, DBL_MIN, DBL_EPSILON * 10);
}