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

Last change on this file since 349 was 349, checked in by oriona, 10 years ago

implementation of the similarity measure

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