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

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

Commented out "srand(time(NULL))" - this line would affect everything else that uses the standard global r.n.g., and would introduce uncontrolled indeterminism

  • Property svn:eol-style set to native
File size: 12.4 KB
Line 
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.
4
5////////////////////////////////////////////////////////////////////////////////////////
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//
45//
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
49 * changes:
50 * - "RNG rng(0x12345678)" replaced with "srand(time(NULL))"
51 * - "rng.next()" replaced with "rand()"
52 * MK, 04.2015:
53 * - commented out "srand(time(NULL))" - this line would affect everything else that uses the standard global r.n.g., and would introduce uncontrolled indeterminism
54 */
55#include <cstdlib>
56#include <algorithm>
57#include <cmath>
58#include <limits>
59#include <cfloat>
60#include <assert.h>
61#include "lapack.h"
62
63typedef unsigned char uchar;
64
65#if defined _M_IX86 && defined _MSC_VER && _MSC_VER < 1700
66#pragma float_control(precise, on)
67#endif
68
69template<typename _Tp, size_t fixed_size = 1024 / sizeof (_Tp) + 8 > class AutoBuffer
70{
71public:
72    typedef _Tp value_type;
73
74    //! the default constructor
75    AutoBuffer();
76    //! constructor taking the real buffer size
77    AutoBuffer(size_t _size);
78
79    //! the copy constructor
80    AutoBuffer(const AutoBuffer<_Tp, fixed_size>& buf);
81    //! the assignment operator
82    AutoBuffer<_Tp, fixed_size>& operator=(const AutoBuffer<_Tp, fixed_size>& buf);
83
84    //! destructor. calls deallocate()
85    ~AutoBuffer();
86
87    //! allocates the new buffer of size _size. if the _size is small enough, stack-allocated buffer is used
88    void allocate(size_t _size);
89    //! deallocates the buffer if it was dynamically allocated
90    void deallocate();
91    //! resizes the buffer and preserves the content
92    void resize(size_t _size);
93    //! returns the current buffer size
94    size_t size() const;
95    //! returns pointer to the real buffer, stack-allocated or head-allocated
96    operator _Tp* ();
97    //! returns read-only pointer to the real buffer, stack-allocated or head-allocated
98    operator const _Tp* () const;
99
100protected:
101    //! pointer to the real buffer, can point to buf if the buffer is small enough
102    _Tp* ptr;
103    //! size of the real buffer
104    size_t sz;
105    //! pre-allocated buffer. At least 1 element to confirm C++ standard reqirements
106    _Tp buf[(fixed_size > 0) ? fixed_size : 1];
107};
108
109/////////////////////////////// AutoBuffer implementation ////////////////////////////////////////
110
111template<typename _Tp, size_t fixed_size> inline
112AutoBuffer<_Tp, fixed_size>::AutoBuffer()
113{
114    ptr = buf;
115    sz = fixed_size;
116}
117
118template<typename _Tp, size_t fixed_size> inline
119AutoBuffer<_Tp, fixed_size>::AutoBuffer(size_t _size)
120{
121    ptr = buf;
122    sz = fixed_size;
123    allocate(_size);
124}
125
126template<typename _Tp, size_t fixed_size> inline
127AutoBuffer<_Tp, fixed_size>::AutoBuffer(const AutoBuffer<_Tp, fixed_size>& abuf)
128{
129    ptr = buf;
130    sz = fixed_size;
131    allocate(abuf.size());
132    for (size_t i = 0; i < sz; i++)
133        ptr[i] = abuf.ptr[i];
134}
135
136template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>&
137AutoBuffer<_Tp, fixed_size>::operator=(const AutoBuffer<_Tp, fixed_size>& abuf)
138{
139    if (this != &abuf)
140    {
141        deallocate();
142        allocate(abuf.size());
143        for (size_t i = 0; i < sz; i++)
144            ptr[i] = abuf.ptr[i];
145    }
146    return *this;
147}
148
149template<typename _Tp, size_t fixed_size> inline
150AutoBuffer<_Tp, fixed_size>::~AutoBuffer()
151{
152    deallocate();
153}
154
155template<typename _Tp, size_t fixed_size> inline void
156AutoBuffer<_Tp, fixed_size>::allocate(size_t _size)
157{
158    if (_size <= sz)
159    {
160        sz = _size;
161        return;
162    }
163    deallocate();
164    if (_size > fixed_size)
165    {
166        ptr = new _Tp[_size];
167        sz = _size;
168    }
169}
170
171template<typename _Tp, size_t fixed_size> inline void
172AutoBuffer<_Tp, fixed_size>::deallocate()
173{
174    if (ptr != buf)
175    {
176        delete[] ptr;
177        ptr = buf;
178        sz = fixed_size;
179    }
180}
181
182template<typename _Tp, size_t fixed_size> inline void
183AutoBuffer<_Tp, fixed_size>::resize(size_t _size)
184{
185    if (_size <= sz)
186    {
187        sz = _size;
188        return;
189    }
190    //size_t i, prevsize = sz, minsize = MIN(prevsize, _size);
191    size_t i, prevsize = sz, minsize = prevsize < _size ? prevsize : _size;
192    _Tp* prevptr = ptr;
193
194    ptr = _size > fixed_size ? new _Tp[_size] : buf;
195    sz = _size;
196
197    if (ptr != prevptr)
198        for (i = 0; i < minsize; i++)
199            ptr[i] = prevptr[i];
200    for (i = prevsize; i < _size; i++)
201        ptr[i] = _Tp();
202
203    if (prevptr != buf)
204        delete[] prevptr;
205}
206
207template<typename _Tp, size_t fixed_size> inline size_t
208AutoBuffer<_Tp, fixed_size>::size() const
209{
210    return sz;
211}
212
213template<typename _Tp, size_t fixed_size> inline
214AutoBuffer<_Tp, fixed_size>::operator _Tp* ()
215{
216    return ptr;
217}
218
219template<typename _Tp, size_t fixed_size> inline
220AutoBuffer<_Tp, fixed_size>::operator const _Tp* () const
221{
222    return ptr;
223}
224
225template<typename T> struct VBLAS
226{
227
228    int dot(const T*, const T*, int, T*) const
229    {
230        return 0;
231    }
232
233    int givens(T*, T*, int, T, T) const
234    {
235        return 0;
236    }
237
238    int givensx(T*, T*, int, T, T, T*, T*) const
239    {
240        return 0;
241    }
242};
243
244template<typename _Tp> void
245JacobiSVDImpl_(_Tp* At, size_t astep, _Tp* _W, _Tp* Vt, size_t vstep,
246               int m, int n, int n1, double minval, _Tp eps)
247{
248    VBLAS<_Tp> vblas;
249    AutoBuffer<double> Wbuf(n);
250    double* W = Wbuf;
251    int i, j, k, iter, max_iter = std::max(m, 30);
252    _Tp c, s;
253    double sd;
254    astep /= sizeof (At[0]);
255    vstep /= sizeof (Vt[0]);
256
257    for (i = 0; i < n; i++)
258    {
259        for (k = 0, sd = 0; k < m; k++)
260        {
261            _Tp t = At[i * astep + k];
262            sd += (double) t*t;
263        }
264        W[i] = sd;
265
266        if (Vt)
267        {
268            for (k = 0; k < n; k++)
269                Vt[i * vstep + k] = 0;
270            Vt[i * vstep + i] = 1;
271        }
272    }
273
274    for (iter = 0; iter < max_iter; iter++)
275    {
276        bool changed = false;
277
278        for (i = 0; i < n - 1; i++)
279            for (j = i + 1; j < n; j++)
280            {
281                _Tp *Ai = At + i*astep, *Aj = At + j*astep;
282                double a = W[i], p = 0, b = W[j];
283
284                for (k = 0; k < m; k++)
285                    p += (double) Ai[k] * Aj[k];
286
287                if (std::abs(p) <= eps * std::sqrt((double) a * b))
288                    continue;
289
290                p *= 2;
291                double beta = a - b, gamma = hypot((double) p, beta);
292                if (beta < 0)
293                {
294                    double delta = (gamma - beta)*0.5;
295                    s = (_Tp) std::sqrt(delta / gamma);
296                    c = (_Tp) (p / (gamma * s * 2));
297                }
298                else
299                {
300                    c = (_Tp) std::sqrt((gamma + beta) / (gamma * 2));
301                    s = (_Tp) (p / (gamma * c * 2));
302                }
303
304                a = b = 0;
305                for (k = 0; k < m; k++)
306                {
307                    _Tp t0 = c * Ai[k] + s * Aj[k];
308                    _Tp t1 = -s * Ai[k] + c * Aj[k];
309                    Ai[k] = t0;
310                    Aj[k] = t1;
311
312                    a += (double) t0*t0;
313                    b += (double) t1*t1;
314                }
315                W[i] = a;
316                W[j] = b;
317
318                changed = true;
319
320                if (Vt)
321                {
322                    _Tp *Vi = Vt + i*vstep, *Vj = Vt + j*vstep;
323                    k = vblas.givens(Vi, Vj, n, c, s);
324
325                    for (; k < n; k++)
326                    {
327                        _Tp t0 = c * Vi[k] + s * Vj[k];
328                        _Tp t1 = -s * Vi[k] + c * Vj[k];
329                        Vi[k] = t0;
330                        Vj[k] = t1;
331                    }
332                }
333            }
334        if (!changed)
335            break;
336    }
337
338    for (i = 0; i < n; i++)
339    {
340        for (k = 0, sd = 0; k < m; k++)
341        {
342            _Tp t = At[i * astep + k];
343            sd += (double) t*t;
344        }
345        W[i] = std::sqrt(sd);
346    }
347
348    for (i = 0; i < n - 1; i++)
349    {
350        j = i;
351        for (k = i + 1; k < n; k++)
352        {
353            if (W[j] < W[k])
354                j = k;
355        }
356        if (i != j)
357        {
358            std::swap(W[i], W[j]);
359            if (Vt)
360            {
361                for (k = 0; k < m; k++)
362                    std::swap(At[i * astep + k], At[j * astep + k]);
363
364                for (k = 0; k < n; k++)
365                    std::swap(Vt[i * vstep + k], Vt[j * vstep + k]);
366            }
367        }
368    }
369
370    for (i = 0; i < n; i++)
371        _W[i] = (_Tp) W[i];
372
373    if (!Vt)
374        return;
375
376    //srand(time(NULL));
377    for (i = 0; i < n1; i++)
378    {
379        sd = i < n ? W[i] : 0;
380
381        while (sd <= minval)
382        {
383            // if we got a zero singular value, then in order to get the corresponding left singular vector
384            // we generate a random vector, project it to the previously computed left singular vectors,
385            // subtract the projection and normalize the difference.
386            const _Tp val0 = (_Tp) (1. / m);
387            for (k = 0; k < m; k++)
388            {
389                _Tp val = (rand() & 256) != 0 ? val0 : -val0;
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        }
419
420        s = (_Tp) (1 / sd);
421        for (k = 0; k < m; k++)
422            At[i * astep + k] *= s;
423    }
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{
428    JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, DBL_MIN, DBL_EPSILON * 10);
429}
Note: See TracBrowser for help on using the repository browser.