Changeset 389 for cpp/frams


Ignore:
Timestamp:
05/24/15 21:03:46 (9 years ago)
Author:
Maciej Komosinski
Message:

Code formatting

Location:
cpp/frams/model/similarity/SVD
Files:
4 edited

Legend:

Unmodified
Added
Removed
  • cpp/frams/model/similarity/SVD/lapack.cpp

    r388 r389  
    6565#endif
    6666
    67 template<typename _Tp, size_t fixed_size = 1024 / sizeof (_Tp) + 8 > class AutoBuffer
     67template<typename _Tp, size_t fixed_size = 1024 / sizeof(_Tp) + 8 > class AutoBuffer
    6868{
    6969public:
    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;
     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;
    9797
    9898protected:
    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];
     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];
    105105};
    106106
     
    110110AutoBuffer<_Tp, fixed_size>::AutoBuffer()
    111111{
    112     ptr = buf;
    113     sz = fixed_size;
     112        ptr = buf;
     113        sz = fixed_size;
    114114}
    115115
     
    117117AutoBuffer<_Tp, fixed_size>::AutoBuffer(size_t _size)
    118118{
    119     ptr = buf;
    120     sz = fixed_size;
    121     allocate(_size);
     119        ptr = buf;
     120        sz = fixed_size;
     121        allocate(_size);
    122122}
    123123
     
    125125AutoBuffer<_Tp, fixed_size>::AutoBuffer(const AutoBuffer<_Tp, fixed_size>& abuf)
    126126{
    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];
     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];
    132132}
    133133
     
    135135AutoBuffer<_Tp, fixed_size>::operator=(const AutoBuffer<_Tp, fixed_size>& abuf)
    136136{
    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;
     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;
    145145}
    146146
     
    148148AutoBuffer<_Tp, fixed_size>::~AutoBuffer()
    149149{
    150     deallocate();
     150        deallocate();
    151151}
    152152
     
    154154AutoBuffer<_Tp, fixed_size>::allocate(size_t _size)
    155155{
    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     }
     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        }
    167167}
    168168
     
    170170AutoBuffer<_Tp, fixed_size>::deallocate()
    171171{
    172     if (ptr != buf)
    173     {
    174         delete[] ptr;
    175         ptr = buf;
    176         sz = fixed_size;
    177     }
     172        if (ptr != buf)
     173        {
     174                delete[] ptr;
     175                ptr = buf;
     176                sz = fixed_size;
     177        }
    178178}
    179179
     
    181181AutoBuffer<_Tp, fixed_size>::resize(size_t _size)
    182182{
    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;
     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;
    203203}
    204204
     
    206206AutoBuffer<_Tp, fixed_size>::size() const
    207207{
    208     return sz;
     208        return sz;
    209209}
    210210
     
    212212AutoBuffer<_Tp, fixed_size>::operator _Tp* ()
    213213{
    214     return ptr;
     214        return ptr;
    215215}
    216216
     
    218218AutoBuffer<_Tp, fixed_size>::operator const _Tp* () const
    219219{
    220     return ptr;
     220        return ptr;
    221221}
    222222
     
    224224{
    225225
    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     }
     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        }
    240240};
    241241
    242242template<typename _Tp> void
    243243JacobiSVDImpl_(_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;
     244int 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;
    373373
    374374        uint8_t rndstate = 0x02; //PRBS-7 from http://en.wikipedia.org/wiki/Pseudorandom_binary_sequence
    375375        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             {
     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                        {
    387387                                int rndbit = (((rndstate >> 6) ^ (rndstate >> 5)) & 1);
    388388                                rndstate = ((rndstate << 1) | rndbit) & 0x7f;
    389389                                _Tp val = rndbit == 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     }
     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        }
    424424}
    425425
    426426void Lapack::JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1 = -1)
    427427{
    428     JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, DBL_MIN, DBL_EPSILON * 10);
    429 }
     428        JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, DBL_MIN, DBL_EPSILON * 10);
     429}
  • cpp/frams/model/similarity/SVD/lapack.h

    r357 r389  
    55{
    66public:
    7     static void JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1);
     7        static void JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1);
    88};
    99
  • cpp/frams/model/similarity/SVD/matrix_tools.cpp

    r370 r389  
    1515double *Create(int nSize)
    1616{
    17         double *matrix = (double *) malloc(nSize * sizeof (double));
    18 
    19     for (int i = 0; i < nSize; i++)
    20     {
    21         matrix[i] = 0;
    22     }
    23 
    24     return matrix;
     17        double *matrix = (double *)malloc(nSize * sizeof(double));
     18
     19        for (int i = 0; i < nSize; i++)
     20        {
     21                matrix[i] = 0;
     22        }
     23
     24        return matrix;
    2525}
    2626
    2727double *Multiply(double *&a, double *&b, int nrow, int ncol, double ncol2, double *&toDel, int delSize)
    2828{
    29     double *c = Create(nrow * ncol2);
    30     int i = 0, j = 0, k = 0;
    31 
    32     for (i = 0; i < nrow; i++)
    33     {
    34         for (j = 0; j < ncol2; j++)
    35         {
    36             for (k = 0; k < ncol; k++)
    37                 c[i * nrow + j] += a[i * nrow + k] * b[k * ncol + j];
    38         }
    39     }
    40 
    41     if (delSize != 0)
    42         free(toDel);
    43     return c;
     29        double *c = Create(nrow * ncol2);
     30        int i = 0, j = 0, k = 0;
     31
     32        for (i = 0; i < nrow; i++)
     33        {
     34                for (j = 0; j < ncol2; j++)
     35                {
     36                        for (k = 0; k < ncol; k++)
     37                                c[i * nrow + j] += a[i * nrow + k] * b[k * ncol + j];
     38                }
     39        }
     40
     41        if (delSize != 0)
     42                free(toDel);
     43        return c;
    4444}
    4545
    4646double *Power(double *&array, int nrow, int ncol, double pow, double *&toDel, int delSize)
    4747{
    48     double *m_Power = Create(nrow * ncol);
    49     if (pow == 2)
    50     {
    51         for (int i = 0; i < nrow; i++)
    52         {
    53             for (int j = 0; j < ncol; j++)
    54             {
    55                 m_Power[i * nrow + j] = array[i * nrow + j] * array[i * nrow + j];
    56             }
    57 
    58         }
    59     }
    60     else
    61     {
    62         for (int i = 0; i < nrow; i++)
    63         {
    64             for (int j = 0; j < ncol; j++)
    65             {
    66                 m_Power[i * nrow + j] = sqrt(array[i * nrow + j]);
    67             }
    68 
    69         }
    70     }
    71 
    72     if (delSize != 0)
    73         free(toDel);
    74 
    75     return m_Power;
     48        double *m_Power = Create(nrow * ncol);
     49        if (pow == 2)
     50        {
     51                for (int i = 0; i < nrow; i++)
     52                {
     53                        for (int j = 0; j < ncol; j++)
     54                        {
     55                                m_Power[i * nrow + j] = array[i * nrow + j] * array[i * nrow + j];
     56                        }
     57
     58                }
     59        }
     60        else
     61        {
     62                for (int i = 0; i < nrow; i++)
     63                {
     64                        for (int j = 0; j < ncol; j++)
     65                        {
     66                                m_Power[i * nrow + j] = sqrt(array[i * nrow + j]);
     67                        }
     68
     69                }
     70        }
     71
     72        if (delSize != 0)
     73                free(toDel);
     74
     75        return m_Power;
    7676}
    7777
    7878void Print(double *&mat, int nelems)
    7979{
    80     for (int i = 0; i < nelems; i++)
    81         printf("%6.2f ", mat[i]);
    82     printf("\n");
     80        for (int i = 0; i < nelems; i++)
     81                printf("%6.2f ", mat[i]);
     82        printf("\n");
    8383
    8484}
     
    8686double *Transpose(double *&A, int arow, int acol)
    8787{
    88     double *result = Create(acol * arow);
    89 
    90     for (int i = 0; i < acol; i++)
    91         for (int j = 0; j < arow; j++)
    92         {
    93             result[i * arow + j] = A[j * acol + i];
    94         }
    95 
    96     return result;
     88        double *result = Create(acol * arow);
     89
     90        for (int i = 0; i < acol; i++)
     91                for (int j = 0; j < arow; j++)
     92                {
     93                result[i * arow + j] = A[j * acol + i];
     94                }
     95
     96        return result;
    9797
    9898}
    9999
    100100/** Computes the SVD of the nSize x nSize distance matrix
    101         @param vdEigenvalues [OUT] Vector of doubles. On return holds the eigenvalues of the
    102         decomposed distance matrix (or rather, to be strict, of the matrix of scalar products
    103         created from the matrix of distances). The vector is assumed to be empty before the function call and
    104         all variance percentages are pushed at the end of it.
    105         @param nSize size of the matrix of distances.
    106         @param pDistances [IN] matrix of distances between parts.
    107         @param Coordinates [OUT] array of three dimensional coordinates obtained from SVD of pDistances matrix.
    108  */
     101                @param vdEigenvalues [OUT] Vector of doubles. On return holds the eigenvalues of the
     102                decomposed distance matrix (or rather, to be strict, of the matrix of scalar products
     103                created from the matrix of distances). The vector is assumed to be empty before the function call and
     104                all variance percentages are pushed at the end of it.
     105                @param nSize size of the matrix of distances.
     106                @param pDistances [IN] matrix of distances between parts.
     107                @param Coordinates [OUT] array of three dimensional coordinates obtained from SVD of pDistances matrix.
     108                */
    109109void MatrixTools::SVD(std::vector<double> &vdEigenvalues, int nSize, double *pDistances, Pt3D *&Coordinates)
    110110{
    111     // compute squares of elements of this array
    112     // compute the matrix B that is the parameter of SVD
    113     double *B = Create(nSize * nSize);
    114     {
    115         // use additional scope to delete temporary matrices
    116         double *Ones, *Eye, *Z, *D;
    117 
    118         D = Create(nSize * nSize);
    119         D = Power(pDistances, nSize, nSize, 2.0, D, nSize);
    120 
    121         Ones = Create(nSize * nSize);
    122         for (int i = 0; i < nSize; i++)
    123             for (int j = 0; j < nSize; j++)
    124             {
    125                 Ones[i * nSize + j] = 1;
    126             }
    127 
    128         Eye = Create(nSize * nSize);
    129         for (int i = 0; i < nSize; i++)
    130         {
    131             for (int j = 0; j < nSize; j++)
    132             {
    133                 if (i == j)
    134                 {
    135                     Eye[i * nSize + j] = 1;
    136                 }
    137                 else
    138                 {
    139                     Eye[i * nSize + j] = 0;
    140                 }
    141             }
    142         }
    143 
    144         Z = Create(nSize * nSize);
    145         for (int i = 0; i < nSize; i++)
    146         {
    147             for (int j = 0; j < nSize; j++)
    148             {
    149                 Z[i * nSize + j] = 1.0 / ((double) nSize) * Ones[i * nSize + j];
    150             }
    151         }
    152 
    153         for (int i = 0; i < nSize; i++)
    154         {
    155             for (int j = 0; j < nSize; j++)
    156             {
    157                 Z[i * nSize + j] = Eye[i * nSize + j] - Z[i * nSize + j];
    158             }
    159         }
    160 
    161         for (int i = 0; i < nSize; i++)
    162         {
    163             for (int j = 0; j < nSize; j++)
    164             {
    165                 B[i * nSize + j] = Z[i * nSize + j] * -0.5;
    166             }
    167         }
    168 
    169         B = Multiply(B, D, nSize, nSize, nSize, B, nSize);
    170         B = Multiply(B, Z, nSize, nSize, nSize, B, nSize);
    171 
    172         free(Ones);
    173         free(Eye);
    174         free(Z);
    175         free(D);
    176     }
    177 
    178     double *Eigenvalues = Create(nSize);
    179     double *S = Create(nSize * nSize);
    180 
    181     // call SVD function
    182     double *Vt = Create(nSize * nSize);
    183     size_t astep = nSize * sizeof (double);
    184     Lapack::JacobiSVD(B, astep, Eigenvalues, Vt, astep, nSize, nSize, nSize);
    185 
    186     double *W = Transpose(Vt, nSize, nSize);
    187 
    188     free(B);
    189     free(Vt);
    190 
    191     for (int i = 0; i < nSize; i++)
    192         for (int j = 0; j < nSize; j++)
    193         {
    194             if (i == j)
    195                 S[i * nSize + j] = Eigenvalues[i];
    196             else
    197                 S[i * nSize + j] = 0;
    198         }
    199 
    200     // compute coordinates of points
    201     double *sqS, *dCoordinates;
    202     sqS = Power(S, nSize, nSize, 0.5, S, nSize);
    203     dCoordinates = Multiply(W, sqS, nSize, nSize, nSize, W, nSize);
    204     free(sqS);
    205 
    206     for (int i = 0; i < nSize; i++)
    207     {
    208         // set coordinate from the SVD solution
    209         Coordinates[ i ].x = dCoordinates[i * nSize];
    210         Coordinates[ i ].y = dCoordinates[i * nSize + 1 ];
    211         if (nSize > 2)
    212             Coordinates[ i ].z = dCoordinates[i * nSize + 2 ];
    213         else
    214             Coordinates[ i ].z = 0;
    215     }
    216 
    217     // store the eigenvalues in the output vector
    218     for (int i = 0; i < nSize; i++)
    219     {
    220         double dElement = Eigenvalues[i];
    221         vdEigenvalues.push_back(dElement);
    222     }
    223 
    224     free(Eigenvalues);
    225     free(dCoordinates);
    226 }
     111        // compute squares of elements of this array
     112        // compute the matrix B that is the parameter of SVD
     113        double *B = Create(nSize * nSize);
     114        {
     115                // use additional scope to delete temporary matrices
     116                double *Ones, *Eye, *Z, *D;
     117
     118                D = Create(nSize * nSize);
     119                D = Power(pDistances, nSize, nSize, 2.0, D, nSize);
     120
     121                Ones = Create(nSize * nSize);
     122                for (int i = 0; i < nSize; i++)
     123                        for (int j = 0; j < nSize; j++)
     124                        {
     125                        Ones[i * nSize + j] = 1;
     126                        }
     127
     128                Eye = Create(nSize * nSize);
     129                for (int i = 0; i < nSize; i++)
     130                {
     131                        for (int j = 0; j < nSize; j++)
     132                        {
     133                                if (i == j)
     134                                {
     135                                        Eye[i * nSize + j] = 1;
     136                                }
     137                                else
     138                                {
     139                                        Eye[i * nSize + j] = 0;
     140                                }
     141                        }
     142                }
     143
     144                Z = Create(nSize * nSize);
     145                for (int i = 0; i < nSize; i++)
     146                {
     147                        for (int j = 0; j < nSize; j++)
     148                        {
     149                                Z[i * nSize + j] = 1.0 / ((double)nSize) * Ones[i * nSize + j];
     150                        }
     151                }
     152
     153                for (int i = 0; i < nSize; i++)
     154                {
     155                        for (int j = 0; j < nSize; j++)
     156                        {
     157                                Z[i * nSize + j] = Eye[i * nSize + j] - Z[i * nSize + j];
     158                        }
     159                }
     160
     161                for (int i = 0; i < nSize; i++)
     162                {
     163                        for (int j = 0; j < nSize; j++)
     164                        {
     165                                B[i * nSize + j] = Z[i * nSize + j] * -0.5;
     166                        }
     167                }
     168
     169                B = Multiply(B, D, nSize, nSize, nSize, B, nSize);
     170                B = Multiply(B, Z, nSize, nSize, nSize, B, nSize);
     171
     172                free(Ones);
     173                free(Eye);
     174                free(Z);
     175                free(D);
     176        }
     177
     178        double *Eigenvalues = Create(nSize);
     179        double *S = Create(nSize * nSize);
     180
     181        // call SVD function
     182        double *Vt = Create(nSize * nSize);
     183        size_t astep = nSize * sizeof(double);
     184        Lapack::JacobiSVD(B, astep, Eigenvalues, Vt, astep, nSize, nSize, nSize);
     185
     186        double *W = Transpose(Vt, nSize, nSize);
     187
     188        free(B);
     189        free(Vt);
     190
     191        for (int i = 0; i < nSize; i++)
     192                for (int j = 0; j < nSize; j++)
     193                {
     194                if (i == j)
     195                        S[i * nSize + j] = Eigenvalues[i];
     196                else
     197                        S[i * nSize + j] = 0;
     198                }
     199
     200        // compute coordinates of points
     201        double *sqS, *dCoordinates;
     202        sqS = Power(S, nSize, nSize, 0.5, S, nSize);
     203        dCoordinates = Multiply(W, sqS, nSize, nSize, nSize, W, nSize);
     204        free(sqS);
     205
     206        for (int i = 0; i < nSize; i++)
     207        {
     208                // set coordinate from the SVD solution
     209                Coordinates[i].x = dCoordinates[i * nSize];
     210                Coordinates[i].y = dCoordinates[i * nSize + 1];
     211                if (nSize > 2)
     212                        Coordinates[i].z = dCoordinates[i * nSize + 2];
     213                else
     214                        Coordinates[i].z = 0;
     215        }
     216
     217        // store the eigenvalues in the output vector
     218        for (int i = 0; i < nSize; i++)
     219        {
     220                double dElement = Eigenvalues[i];
     221                vdEigenvalues.push_back(dElement);
     222        }
     223
     224        free(Eigenvalues);
     225        free(dCoordinates);
     226}
  • cpp/frams/model/similarity/SVD/matrix_tools.h

    r357 r389  
    1212{
    1313public:
    14     static void SVD(std::vector<double> &vdEigenvalues, int nSize, double *pDistances, Pt3D *&Coordinates);
     14        static void SVD(std::vector<double> &vdEigenvalues, int nSize, double *pDistances, Pt3D *&Coordinates);
    1515};
    1616
Note: See TracChangeset for help on using the changeset viewer.