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

Last change on this file since 389 was 389, checked in by Maciej Komosinski, 9 years ago

Code formatting

  • Property svn:eol-style set to native
File size: 10.6 KB
RevLine 
[349]1// This file is a part of Framsticks SDK.  http://www.framsticks.com/
2// Copyright (C) 1999-2015  Maciej Komosinski and Szymon Ulatowski.
3// See LICENSE.txt for details.
[350]4
5////////////////////////////////////////////////////////////////////////////////////////
[349]6//
7//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
8//
9//  By downloading, copying, installing or using the software you agree to this license.
10//  If you do not agree to this license, do not download, install,
11//  copy or use the software.
12//
13//
14//                           License Agreement
15//                For Open Source Computer Vision Library
16//
17// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
18// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
19// Third party copyrights are property of their respective owners.
20//
21// Redistribution and use in source and binary forms, with or without modification,
22// are permitted provided that the following conditions are met:
23//
24//   * Redistribution's of source code must retain the above copyright notice,
25//     this list of conditions and the following disclaimer.
26//
27//   * Redistribution's in binary form must reproduce the above copyright notice,
28//     this list of conditions and the following disclaimer in the documentation
29//     and/or other materials provided with the distribution.
30//
31//   * The name of the copyright holders may not be used to endorse or promote products
32//     derived from this software without specific prior written permission.
33//
34// This software is provided by the copyright holders and contributors "as is" and
35// any express or implied warranties, including, but not limited to, the implied
36// warranties of merchantability and fitness for a particular purpose are disclaimed.
37// In no event shall the Intel Corporation or contributors be liable for any direct,
38// indirect, incidental, special, exemplary, or consequential damages
39// (including, but not limited to, procurement of substitute goods or services;
40// loss of use, data, or profits; or business interruption) however caused
41// and on any theory of liability, whether in contract, strict liability,
42// or tort (including negligence or otherwise) arising in any way out of
43// the use of this software, even if advised of the possibility of such damage.
44//
[358]45//
[349]46/*
47 * AutoBuffer from https://github.com/Itseez/opencv/blob/master/modules/core/include/opencv2/core/utility.hpp
48 * VBLAS, JacobiSVDImpl_ from https://github.com/Itseez/opencv/blob/master/modules/core/src/lapack.cpp
[388]49 * changes MK, May 2015:
50 * - "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.
[349]51 */
52#include <cstdlib>
53#include <algorithm>
54#include <cmath>
55#include <limits>
56#include <cfloat>
[388]57//#include <assert.h>
58#include <math.h> //hypot(), embarcadero
59#include <stdint.h> //uint8_t
[349]60#include "lapack.h"
61
62
63#if defined _M_IX86 && defined _MSC_VER && _MSC_VER < 1700
64#pragma float_control(precise, on)
65#endif
66
[389]67template<typename _Tp, size_t fixed_size = 1024 / sizeof(_Tp) + 8 > class AutoBuffer
[349]68{
69public:
[389]70        typedef _Tp value_type;
[349]71
[389]72        //! the default constructor
73        AutoBuffer();
74        //! constructor taking the real buffer size
75        AutoBuffer(size_t _size);
[349]76
[389]77        //! the copy constructor
78        AutoBuffer(const AutoBuffer<_Tp, fixed_size>& buf);
79        //! the assignment operator
80        AutoBuffer<_Tp, fixed_size>& operator=(const AutoBuffer<_Tp, fixed_size>& buf);
[349]81
[389]82        //! destructor. calls deallocate()
83        ~AutoBuffer();
[349]84
[389]85        //! allocates the new buffer of size _size. if the _size is small enough, stack-allocated buffer is used
86        void allocate(size_t _size);
87        //! deallocates the buffer if it was dynamically allocated
88        void deallocate();
89        //! resizes the buffer and preserves the content
90        void resize(size_t _size);
91        //! returns the current buffer size
92        size_t size() const;
93        //! returns pointer to the real buffer, stack-allocated or head-allocated
94        operator _Tp* ();
95        //! returns read-only pointer to the real buffer, stack-allocated or head-allocated
96        operator const _Tp* () const;
[349]97
98protected:
[389]99        //! pointer to the real buffer, can point to buf if the buffer is small enough
100        _Tp* ptr;
101        //! size of the real buffer
102        size_t sz;
103        //! pre-allocated buffer. At least 1 element to confirm C++ standard reqirements
104        _Tp buf[(fixed_size > 0) ? fixed_size : 1];
[349]105};
106
107/////////////////////////////// AutoBuffer implementation ////////////////////////////////////////
108
109template<typename _Tp, size_t fixed_size> inline
110AutoBuffer<_Tp, fixed_size>::AutoBuffer()
111{
[389]112        ptr = buf;
113        sz = fixed_size;
[349]114}
115
116template<typename _Tp, size_t fixed_size> inline
117AutoBuffer<_Tp, fixed_size>::AutoBuffer(size_t _size)
118{
[389]119        ptr = buf;
120        sz = fixed_size;
121        allocate(_size);
[349]122}
123
124template<typename _Tp, size_t fixed_size> inline
125AutoBuffer<_Tp, fixed_size>::AutoBuffer(const AutoBuffer<_Tp, fixed_size>& abuf)
126{
[389]127        ptr = buf;
128        sz = fixed_size;
129        allocate(abuf.size());
130        for (size_t i = 0; i < sz; i++)
131                ptr[i] = abuf.ptr[i];
[349]132}
133
134template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>&
135AutoBuffer<_Tp, fixed_size>::operator=(const AutoBuffer<_Tp, fixed_size>& abuf)
136{
[389]137        if (this != &abuf)
138        {
139                deallocate();
140                allocate(abuf.size());
141                for (size_t i = 0; i < sz; i++)
142                        ptr[i] = abuf.ptr[i];
143        }
144        return *this;
[349]145}
146
147template<typename _Tp, size_t fixed_size> inline
148AutoBuffer<_Tp, fixed_size>::~AutoBuffer()
149{
[389]150        deallocate();
[349]151}
152
153template<typename _Tp, size_t fixed_size> inline void
154AutoBuffer<_Tp, fixed_size>::allocate(size_t _size)
155{
[389]156        if (_size <= sz)
157        {
158                sz = _size;
159                return;
160        }
161        deallocate();
162        if (_size > fixed_size)
163        {
164                ptr = new _Tp[_size];
165                sz = _size;
166        }
[349]167}
168
169template<typename _Tp, size_t fixed_size> inline void
170AutoBuffer<_Tp, fixed_size>::deallocate()
171{
[389]172        if (ptr != buf)
173        {
174                delete[] ptr;
175                ptr = buf;
176                sz = fixed_size;
177        }
[349]178}
179
180template<typename _Tp, size_t fixed_size> inline void
181AutoBuffer<_Tp, fixed_size>::resize(size_t _size)
182{
[389]183        if (_size <= sz)
184        {
185                sz = _size;
186                return;
187        }
188        //size_t i, prevsize = sz, minsize = MIN(prevsize, _size);
189        size_t i, prevsize = sz, minsize = prevsize < _size ? prevsize : _size;
190        _Tp* prevptr = ptr;
[349]191
[389]192        ptr = _size > fixed_size ? new _Tp[_size] : buf;
193        sz = _size;
[349]194
[389]195        if (ptr != prevptr)
196                for (i = 0; i < minsize; i++)
197                        ptr[i] = prevptr[i];
198        for (i = prevsize; i < _size; i++)
199                ptr[i] = _Tp();
[349]200
[389]201        if (prevptr != buf)
202                delete[] prevptr;
[349]203}
204
205template<typename _Tp, size_t fixed_size> inline size_t
206AutoBuffer<_Tp, fixed_size>::size() const
207{
[389]208        return sz;
[349]209}
210
211template<typename _Tp, size_t fixed_size> inline
212AutoBuffer<_Tp, fixed_size>::operator _Tp* ()
213{
[389]214        return ptr;
[349]215}
216
217template<typename _Tp, size_t fixed_size> inline
218AutoBuffer<_Tp, fixed_size>::operator const _Tp* () const
219{
[389]220        return ptr;
[349]221}
222
223template<typename T> struct VBLAS
224{
225
[389]226        int dot(const T*, const T*, int, T*) const
227        {
228                return 0;
229        }
[349]230
[389]231        int givens(T*, T*, int, T, T) const
232        {
233                return 0;
234        }
[349]235
[389]236        int givensx(T*, T*, int, T, T, T*, T*) const
237        {
238                return 0;
239        }
[349]240};
241
242template<typename _Tp> void
243JacobiSVDImpl_(_Tp* At, size_t astep, _Tp* _W, _Tp* Vt, size_t vstep,
[389]244int m, int n, int n1, double minval, _Tp eps)
[349]245{
[389]246        VBLAS<_Tp> vblas;
247        AutoBuffer<double> Wbuf(n);
248        double* W = Wbuf;
249        int i, j, k, iter, max_iter = std::max(m, 30);
250        _Tp c, s;
251        double sd;
252        astep /= sizeof(At[0]);
253        vstep /= sizeof(Vt[0]);
[349]254
[389]255        for (i = 0; i < n; i++)
256        {
257                for (k = 0, sd = 0; k < m; k++)
258                {
259                        _Tp t = At[i * astep + k];
260                        sd += (double)t*t;
261                }
262                W[i] = sd;
[349]263
[389]264                if (Vt)
265                {
266                        for (k = 0; k < n; k++)
267                                Vt[i * vstep + k] = 0;
268                        Vt[i * vstep + i] = 1;
269                }
270        }
[349]271
[389]272        for (iter = 0; iter < max_iter; iter++)
273        {
274                bool changed = false;
[349]275
[389]276                for (i = 0; i < n - 1; i++)
277                        for (j = i + 1; j < n; j++)
278                        {
279                        _Tp *Ai = At + i*astep, *Aj = At + j*astep;
280                        double a = W[i], p = 0, b = W[j];
[349]281
[389]282                        for (k = 0; k < m; k++)
283                                p += (double)Ai[k] * Aj[k];
[349]284
[389]285                        if (std::abs(p) <= eps * std::sqrt((double)a * b))
286                                continue;
[349]287
[389]288                        p *= 2;
289                        double beta = a - b, gamma = hypot((double)p, beta);
290                        if (beta < 0)
291                        {
292                                double delta = (gamma - beta)*0.5;
293                                s = (_Tp)std::sqrt(delta / gamma);
294                                c = (_Tp)(p / (gamma * s * 2));
295                        }
296                        else
297                        {
298                                c = (_Tp)std::sqrt((gamma + beta) / (gamma * 2));
299                                s = (_Tp)(p / (gamma * c * 2));
300                        }
[349]301
[389]302                        a = b = 0;
303                        for (k = 0; k < m; k++)
304                        {
305                                _Tp t0 = c * Ai[k] + s * Aj[k];
306                                _Tp t1 = -s * Ai[k] + c * Aj[k];
307                                Ai[k] = t0;
308                                Aj[k] = t1;
[349]309
[389]310                                a += (double)t0*t0;
311                                b += (double)t1*t1;
312                        }
313                        W[i] = a;
314                        W[j] = b;
[349]315
[389]316                        changed = true;
[349]317
[389]318                        if (Vt)
319                        {
320                                _Tp *Vi = Vt + i*vstep, *Vj = Vt + j*vstep;
321                                k = vblas.givens(Vi, Vj, n, c, s);
[349]322
[389]323                                for (; k < n; k++)
324                                {
325                                        _Tp t0 = c * Vi[k] + s * Vj[k];
326                                        _Tp t1 = -s * Vi[k] + c * Vj[k];
327                                        Vi[k] = t0;
328                                        Vj[k] = t1;
329                                }
330                        }
331                        }
332                if (!changed)
333                        break;
334        }
[349]335
[389]336        for (i = 0; i < n; i++)
337        {
338                for (k = 0, sd = 0; k < m; k++)
339                {
340                        _Tp t = At[i * astep + k];
341                        sd += (double)t*t;
342                }
343                W[i] = std::sqrt(sd);
344        }
[349]345
[389]346        for (i = 0; i < n - 1; i++)
347        {
348                j = i;
349                for (k = i + 1; k < n; k++)
350                {
351                        if (W[j] < W[k])
352                                j = k;
353                }
354                if (i != j)
355                {
356                        std::swap(W[i], W[j]);
357                        if (Vt)
358                        {
359                                for (k = 0; k < m; k++)
360                                        std::swap(At[i * astep + k], At[j * astep + k]);
[349]361
[389]362                                for (k = 0; k < n; k++)
363                                        std::swap(Vt[i * vstep + k], Vt[j * vstep + k]);
364                        }
365                }
366        }
[349]367
[389]368        for (i = 0; i < n; i++)
369                _W[i] = (_Tp)W[i];
[349]370
[389]371        if (!Vt)
372                return;
[349]373
[388]374        uint8_t rndstate = 0x02; //PRBS-7 from http://en.wikipedia.org/wiki/Pseudorandom_binary_sequence
375        for (i = 0; i < n1; i++)
[389]376        {
377                sd = i < n ? W[i] : 0;
[349]378
[389]379                while (sd <= minval)
380                {
381                        // if we got a zero singular value, then in order to get the corresponding left singular vector
382                        // we generate a random vector, project it to the previously computed left singular vectors,
383                        // subtract the projection and normalize the difference.
384                        const _Tp val0 = (_Tp)(1. / m);
385                        for (k = 0; k < m; k++)
386                        {
[388]387                                int rndbit = (((rndstate >> 6) ^ (rndstate >> 5)) & 1);
388                                rndstate = ((rndstate << 1) | rndbit) & 0x7f;
389                                _Tp val = rndbit == 0 ? val0 : -val0;
[389]390                                At[i * astep + k] = val;
391                        }
392                        for (iter = 0; iter < 2; iter++)
393                        {
394                                for (j = 0; j < i; j++)
395                                {
396                                        sd = 0;
397                                        for (k = 0; k < m; k++)
398                                                sd += At[i * astep + k] * At[j * astep + k];
399                                        _Tp asum = 0;
400                                        for (k = 0; k < m; k++)
401                                        {
402                                                _Tp t = (_Tp)(At[i * astep + k] - sd * At[j * astep + k]);
403                                                At[i * astep + k] = t;
404                                                asum += std::abs(t);
405                                        }
406                                        asum = asum ? 1 / asum : 0;
407                                        for (k = 0; k < m; k++)
408                                                At[i * astep + k] *= asum;
409                                }
410                        }
411                        sd = 0;
412                        for (k = 0; k < m; k++)
413                        {
414                                _Tp t = At[i * astep + k];
415                                sd += (double)t*t;
416                        }
417                        sd = std::sqrt(sd);
418                }
[349]419
[389]420                s = (_Tp)(1 / sd);
421                for (k = 0; k < m; k++)
422                        At[i * astep + k] *= s;
423        }
[349]424}
425
426void Lapack::JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1 = -1)
427{
[389]428        JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, DBL_MIN, DBL_EPSILON * 10);
[350]429}
Note: See TracBrowser for help on using the repository browser.