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

Last change on this file was 1055, checked in by Maciej Komosinski, 4 months ago

Added a workaround (potentially incorrect) for numerical instability in SVD calculation and the embarcadero compiler

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