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

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

Cosmetic

File size: 12.2 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//M*/
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 */
53#include <cstdlib>
54#include <algorithm>
55#include <cmath>
56#include <limits>
57#include <cfloat>
58#include <assert.h>
59#include "lapack.h"
60
61typedef unsigned char uchar;
62
63#if defined _M_IX86 && defined _MSC_VER && _MSC_VER < 1700
64#pragma float_control(precise, on)
65#endif
66
67template<typename _Tp, size_t fixed_size = 1024 / sizeof (_Tp) + 8 > class AutoBuffer
68{
69public:
70    typedef _Tp value_type;
71
72    //! the default constructor
73    AutoBuffer();
74    //! constructor taking the real buffer size
75    AutoBuffer(size_t _size);
76
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);
81
82    //! destructor. calls deallocate()
83    ~AutoBuffer();
84
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;
97
98protected:
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];
105};
106
107/////////////////////////////// AutoBuffer implementation ////////////////////////////////////////
108
109template<typename _Tp, size_t fixed_size> inline
110AutoBuffer<_Tp, fixed_size>::AutoBuffer()
111{
112    ptr = buf;
113    sz = fixed_size;
114}
115
116template<typename _Tp, size_t fixed_size> inline
117AutoBuffer<_Tp, fixed_size>::AutoBuffer(size_t _size)
118{
119    ptr = buf;
120    sz = fixed_size;
121    allocate(_size);
122}
123
124template<typename _Tp, size_t fixed_size> inline
125AutoBuffer<_Tp, fixed_size>::AutoBuffer(const AutoBuffer<_Tp, fixed_size>& abuf)
126{
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];
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{
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;
145}
146
147template<typename _Tp, size_t fixed_size> inline
148AutoBuffer<_Tp, fixed_size>::~AutoBuffer()
149{
150    deallocate();
151}
152
153template<typename _Tp, size_t fixed_size> inline void
154AutoBuffer<_Tp, fixed_size>::allocate(size_t _size)
155{
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    }
167}
168
169template<typename _Tp, size_t fixed_size> inline void
170AutoBuffer<_Tp, fixed_size>::deallocate()
171{
172    if (ptr != buf)
173    {
174        delete[] ptr;
175        ptr = buf;
176        sz = fixed_size;
177    }
178}
179
180template<typename _Tp, size_t fixed_size> inline void
181AutoBuffer<_Tp, fixed_size>::resize(size_t _size)
182{
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;
191
192    ptr = _size > fixed_size ? new _Tp[_size] : buf;
193    sz = _size;
194
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();
200
201    if (prevptr != buf)
202        delete[] prevptr;
203}
204
205template<typename _Tp, size_t fixed_size> inline size_t
206AutoBuffer<_Tp, fixed_size>::size() const
207{
208    return sz;
209}
210
211template<typename _Tp, size_t fixed_size> inline
212AutoBuffer<_Tp, fixed_size>::operator _Tp* ()
213{
214    return ptr;
215}
216
217template<typename _Tp, size_t fixed_size> inline
218AutoBuffer<_Tp, fixed_size>::operator const _Tp* () const
219{
220    return ptr;
221}
222
223template<typename T> struct VBLAS
224{
225
226    int dot(const T*, const T*, int, T*) const
227    {
228        return 0;
229    }
230
231    int givens(T*, T*, int, T, T) const
232    {
233        return 0;
234    }
235
236    int givensx(T*, T*, int, T, T, T*, T*) const
237    {
238        return 0;
239    }
240};
241
242template<typename _Tp> void
243JacobiSVDImpl_(_Tp* At, size_t astep, _Tp* _W, _Tp* Vt, size_t vstep,
244               int m, int n, int n1, double minval, _Tp eps)
245{
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]);
254
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;
263
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    }
271
272    for (iter = 0; iter < max_iter; iter++)
273    {
274        bool changed = false;
275
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];
281
282                for (k = 0; k < m; k++)
283                    p += (double) Ai[k] * Aj[k];
284
285                if (std::abs(p) <= eps * std::sqrt((double) a * b))
286                    continue;
287
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                }
301
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;
309
310                    a += (double) t0*t0;
311                    b += (double) t1*t1;
312                }
313                W[i] = a;
314                W[j] = b;
315
316                changed = true;
317
318                if (Vt)
319                {
320                    _Tp *Vi = Vt + i*vstep, *Vj = Vt + j*vstep;
321                    k = vblas.givens(Vi, Vj, n, c, s);
322
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    }
335
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    }
345
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]);
361
362                for (k = 0; k < n; k++)
363                    std::swap(Vt[i * vstep + k], Vt[j * vstep + k]);
364            }
365        }
366    }
367
368    for (i = 0; i < n; i++)
369        _W[i] = (_Tp) W[i];
370
371    if (!Vt)
372        return;
373
374    srand(time(NULL));
375    for (i = 0; i < n1; i++)
376    {
377        sd = i < n ? W[i] : 0;
378
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            {
387                _Tp val = (rand() & 256) != 0 ? val0 : -val0;
388                At[i * astep + k] = val;
389            }
390            for (iter = 0; iter < 2; iter++)
391            {
392                for (j = 0; j < i; j++)
393                {
394                    sd = 0;
395                    for (k = 0; k < m; k++)
396                        sd += At[i * astep + k] * At[j * astep + k];
397                    _Tp asum = 0;
398                    for (k = 0; k < m; k++)
399                    {
400                        _Tp t = (_Tp) (At[i * astep + k] - sd * At[j * astep + k]);
401                        At[i * astep + k] = t;
402                        asum += std::abs(t);
403                    }
404                    asum = asum ? 1 / asum : 0;
405                    for (k = 0; k < m; k++)
406                        At[i * astep + k] *= asum;
407                }
408            }
409            sd = 0;
410            for (k = 0; k < m; k++)
411            {
412                _Tp t = At[i * astep + k];
413                sd += (double) t*t;
414            }
415            sd = std::sqrt(sd);
416        }
417
418        s = (_Tp) (1 / sd);
419        for (k = 0; k < m; k++)
420            At[i * astep + k] *= s;
421    }
422}
423
424void Lapack::JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1 = -1)
425{
426    JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, DBL_MIN, DBL_EPSILON * 10);
427}
Note: See TracBrowser for help on using the repository browser.