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

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

#includes needed for Embarcadero

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