Changeset 389 for cpp/frams/model
- Timestamp:
- 05/24/15 21:03:46 (10 years ago)
- Location:
- cpp/frams/model/similarity/SVD
- Files:
-
- 4 edited
Legend:
- Unmodified
- Added
- Removed
-
cpp/frams/model/similarity/SVD/lapack.cpp
r388 r389 65 65 #endif 66 66 67 template<typename _Tp, size_t fixed_size = 1024 / sizeof 67 template<typename _Tp, size_t fixed_size = 1024 / sizeof(_Tp) + 8 > class AutoBuffer 68 68 { 69 69 public: 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 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 97 98 98 protected: 99 100 101 102 103 104 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 105 }; 106 106 … … 110 110 AutoBuffer<_Tp, fixed_size>::AutoBuffer() 111 111 { 112 113 112 ptr = buf; 113 sz = fixed_size; 114 114 } 115 115 … … 117 117 AutoBuffer<_Tp, fixed_size>::AutoBuffer(size_t _size) 118 118 { 119 120 121 119 ptr = buf; 120 sz = fixed_size; 121 allocate(_size); 122 122 } 123 123 … … 125 125 AutoBuffer<_Tp, fixed_size>::AutoBuffer(const AutoBuffer<_Tp, fixed_size>& abuf) 126 126 { 127 128 129 130 131 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 132 } 133 133 … … 135 135 AutoBuffer<_Tp, fixed_size>::operator=(const AutoBuffer<_Tp, fixed_size>& abuf) 136 136 { 137 138 139 140 141 142 143 144 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 145 } 146 146 … … 148 148 AutoBuffer<_Tp, fixed_size>::~AutoBuffer() 149 149 { 150 150 deallocate(); 151 151 } 152 152 … … 154 154 AutoBuffer<_Tp, fixed_size>::allocate(size_t _size) 155 155 { 156 157 158 159 160 161 162 163 164 165 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 } 167 167 } 168 168 … … 170 170 AutoBuffer<_Tp, fixed_size>::deallocate() 171 171 { 172 173 174 175 176 177 172 if (ptr != buf) 173 { 174 delete[] ptr; 175 ptr = buf; 176 sz = fixed_size; 177 } 178 178 } 179 179 … … 181 181 AutoBuffer<_Tp, fixed_size>::resize(size_t _size) 182 182 { 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 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 203 } 204 204 … … 206 206 AutoBuffer<_Tp, fixed_size>::size() const 207 207 { 208 208 return sz; 209 209 } 210 210 … … 212 212 AutoBuffer<_Tp, fixed_size>::operator _Tp* () 213 213 { 214 214 return ptr; 215 215 } 216 216 … … 218 218 AutoBuffer<_Tp, fixed_size>::operator const _Tp* () const 219 219 { 220 220 return ptr; 221 221 } 222 222 … … 224 224 { 225 225 226 227 228 229 230 231 232 233 234 235 236 237 238 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 } 240 240 }; 241 241 242 242 template<typename _Tp> void 243 243 JacobiSVDImpl_(_Tp* At, size_t astep, _Tp* _W, _Tp* Vt, size_t vstep, 244 245 { 246 247 248 249 250 251 252 astep /= sizeof(At[0]);253 vstep /= sizeof(Vt[0]);254 255 256 257 258 259 260 sd += (double)t*t;261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 p += (double)Ai[k] * Aj[k];284 285 if (std::abs(p) <= eps * std::sqrt((double)a * b))286 287 288 289 double beta = a - b, gamma = hypot((double)p, beta);290 291 292 293 s = (_Tp)std::sqrt(delta / gamma);294 c = (_Tp)(p / (gamma * s * 2));295 296 297 298 c = (_Tp)std::sqrt((gamma + beta) / (gamma * 2));299 s = (_Tp)(p / (gamma * c * 2));300 301 302 303 304 305 306 307 308 309 310 a += (double)t0*t0;311 b += (double)t1*t1;312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 sd += (double)t*t;342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 _W[i] = (_Tp)W[i];370 371 372 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 373 374 374 uint8_t rndstate = 0x02; //PRBS-7 from http://en.wikipedia.org/wiki/Pseudorandom_binary_sequence 375 375 for (i = 0; i < n1; i++) 376 377 378 379 380 381 382 383 384 const _Tp val0 = (_Tp)(1. / m);385 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 { 387 387 int rndbit = (((rndstate >> 6) ^ (rndstate >> 5)) & 1); 388 388 rndstate = ((rndstate << 1) | rndbit) & 0x7f; 389 389 _Tp val = rndbit == 0 ? val0 : -val0; 390 391 392 393 394 395 396 397 398 399 400 401 402 _Tp t = (_Tp)(At[i * astep + k] - sd * At[j * astep + k]);403 404 405 406 407 408 409 410 411 412 413 414 415 sd += (double)t*t;416 417 418 419 420 s = (_Tp)(1 / sd);421 422 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 } 424 424 } 425 425 426 426 void Lapack::JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1 = -1) 427 427 { 428 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 5 5 { 6 6 public: 7 7 static void JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1); 8 8 }; 9 9 -
cpp/frams/model/similarity/SVD/matrix_tools.cpp
r370 r389 15 15 double *Create(int nSize) 16 16 { 17 double *matrix = (double *) malloc(nSize * sizeof(double));18 19 20 21 22 23 24 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; 25 25 } 26 26 27 27 double *Multiply(double *&a, double *&b, int nrow, int ncol, double ncol2, double *&toDel, int delSize) 28 28 { 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 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; 44 44 } 45 45 46 46 double *Power(double *&array, int nrow, int ncol, double pow, double *&toDel, int delSize) 47 47 { 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 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; 76 76 } 77 77 78 78 void Print(double *&mat, int nelems) 79 79 { 80 81 82 80 for (int i = 0; i < nelems; i++) 81 printf("%6.2f ", mat[i]); 82 printf("\n"); 83 83 84 84 } … … 86 86 double *Transpose(double *&A, int arow, int acol) 87 87 { 88 89 90 91 92 93 94 95 96 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; 97 97 98 98 } 99 99 100 100 /** 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 104 105 106 107 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 */ 109 109 void MatrixTools::SVD(std::vector<double> &vdEigenvalues, int nSize, double *pDistances, Pt3D *&Coordinates) 110 110 { 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 Z[i * nSize + j] = 1.0 / ((double)nSize) * Ones[i * nSize + j];150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 size_t astep = nSize * sizeof(double);184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 Coordinates[ i].x = dCoordinates[i * nSize];210 Coordinates[ i ].y = dCoordinates[i * nSize + 1];211 212 Coordinates[ i ].z = dCoordinates[i * nSize + 2];213 214 Coordinates[ i].z = 0;215 216 217 218 219 220 221 222 223 224 225 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 12 12 { 13 13 public: 14 14 static void SVD(std::vector<double> &vdEigenvalues, int nSize, double *pDistances, Pt3D *&Coordinates); 15 15 }; 16 16
Note: See TracChangeset
for help on using the changeset viewer.