MatrixRmn.cpp 33 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029
  1. /*
  2. *
  3. * Mathematics Subpackage (VrMath)
  4. *
  5. *
  6. * Author: Samuel R. Buss, [email protected].
  7. * Web page: http://math.ucsd.edu/~sbuss/MathCG
  8. *
  9. *
  10. This software is provided 'as-is', without any express or implied warranty.
  11. In no event will the authors be held liable for any damages arising from the use of this software.
  12. Permission is granted to anyone to use this software for any purpose,
  13. including commercial applications, and to alter it and redistribute it freely,
  14. subject to the following restrictions:
  15. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  16. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  17. 3. This notice may not be removed or altered from any source distribution.
  18. *
  19. *
  20. */
  21. //
  22. // MatrixRmn.cpp: Matrix over reals (Variable dimensional vector)
  23. //
  24. // Not very sophisticated yet. Needs more functionality
  25. // To do: better handling of resizing.
  26. //
  27. #include "MatrixRmn.h"
  28. MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix
  29. // Fill the diagonal entries with the value d. The rest of the matrix is unchanged.
  30. void MatrixRmn::SetDiagonalEntries( double d )
  31. {
  32. long diagLen = Min( NumRows, NumCols );
  33. double* dPtr = x;
  34. for ( ; diagLen>0; diagLen-- ) {
  35. *dPtr = d;
  36. dPtr += NumRows+1;
  37. }
  38. }
  39. // Fill the diagonal entries with values in vector d. The rest of the matrix is unchanged.
  40. void MatrixRmn::SetDiagonalEntries( const VectorRn& d )
  41. {
  42. long diagLen = Min( NumRows, NumCols );
  43. assert ( d.length == diagLen );
  44. double* dPtr = x;
  45. double* from = d.x;
  46. for ( ; diagLen>0; diagLen-- ) {
  47. *dPtr = *(from++);
  48. dPtr += NumRows+1;
  49. }
  50. }
  51. // Fill the superdiagonal entries with the value d. The rest of the matrix is unchanged.
  52. void MatrixRmn::SetSuperDiagonalEntries( double d )
  53. {
  54. long sDiagLen = Min( NumRows, (long)(NumCols-1) );
  55. double* to = x + NumRows;
  56. for ( ; sDiagLen>0; sDiagLen-- ) {
  57. *to = d;
  58. to += NumRows+1;
  59. }
  60. }
  61. // Fill the superdiagonal entries with values in vector d. The rest of the matrix is unchanged.
  62. void MatrixRmn::SetSuperDiagonalEntries( const VectorRn& d )
  63. {
  64. long sDiagLen = Min( (long)(NumRows-1), NumCols );
  65. assert ( sDiagLen == d.length );
  66. double* to = x + NumRows;
  67. double* from = d.x;
  68. for ( ; sDiagLen>0; sDiagLen-- ) {
  69. *to = *(from++);
  70. to += NumRows+1;
  71. }
  72. }
  73. // Fill the subdiagonal entries with the value d. The rest of the matrix is unchanged.
  74. void MatrixRmn::SetSubDiagonalEntries( double d )
  75. {
  76. long sDiagLen = Min( NumRows, NumCols ) - 1;
  77. double* to = x + 1;
  78. for ( ; sDiagLen>0; sDiagLen-- ) {
  79. *to = d;
  80. to += NumRows+1;
  81. }
  82. }
  83. // Fill the subdiagonal entries with values in vector d. The rest of the matrix is unchanged.
  84. void MatrixRmn::SetSubDiagonalEntries( const VectorRn& d )
  85. {
  86. long sDiagLen = Min( NumRows, NumCols ) - 1;
  87. assert ( sDiagLen == d.length );
  88. double* to = x + 1;
  89. double* from = d.x;
  90. for ( ; sDiagLen>0; sDiagLen-- ) {
  91. *to = *(from++);
  92. to += NumRows+1;
  93. }
  94. }
  95. // Set the i-th column equal to d.
  96. void MatrixRmn::SetColumn(long i, const VectorRn& d )
  97. {
  98. assert ( NumRows==d.GetLength() );
  99. double* to = x+i*NumRows;
  100. const double* from = d.x;
  101. for ( i=NumRows; i>0; i-- ) {
  102. *(to++) = *(from++);
  103. }
  104. }
  105. // Set the i-th column equal to d.
  106. void MatrixRmn::SetRow(long i, const VectorRn& d )
  107. {
  108. assert ( NumCols==d.GetLength() );
  109. double* to = x+i;
  110. const double* from = d.x;
  111. for ( i=NumRows; i>0; i-- ) {
  112. *to = *(from++);
  113. to += NumRows;
  114. }
  115. }
  116. // Sets a "linear" portion of the array with the values from a vector d
  117. // The first row and column position are given by startRow, startCol.
  118. // Successive positions are found by using the deltaRow, deltaCol values
  119. // to increment the row and column indices. There is no wrapping around.
  120. void MatrixRmn::SetSequence( const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol )
  121. {
  122. long length = d.length;
  123. assert( startRow>=0 && startRow<NumRows && startCol>=0 && startCol<NumCols );
  124. assert( startRow+(length-1)*deltaRow>=0 && startRow+(length-1)*deltaRow<NumRows );
  125. assert( startCol+(length-1)*deltaCol>=0 && startCol+(length-1)*deltaCol<NumCols );
  126. double *to = x + startRow + NumRows*startCol;
  127. double *from = d.x;
  128. long stride = deltaRow + NumRows*deltaCol;
  129. for ( ; length>0; length-- ) {
  130. *to = *(from++);
  131. to += stride;
  132. }
  133. }
  134. // The matrix A is loaded, in into "this" matrix, based at (0,0).
  135. // The size of "this" matrix must be large enough to accomodate A.
  136. // The rest of "this" matrix is left unchanged. It is not filled with zeroes!
  137. void MatrixRmn::LoadAsSubmatrix( const MatrixRmn& A )
  138. {
  139. assert( A.NumRows<=NumRows && A.NumCols<=NumCols );
  140. int extraColStep = NumRows - A.NumRows;
  141. double *to = x;
  142. double *from = A.x;
  143. for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, one per time thru loop
  144. for ( long j=A.NumRows; j>0; j-- ) { // Copy all elements of this column of A
  145. *(to++) = *(from++);
  146. }
  147. to += extraColStep;
  148. }
  149. }
  150. // The matrix A is loaded, in transposed order into "this" matrix, based at (0,0).
  151. // The size of "this" matrix must be large enough to accomodate A.
  152. // The rest of "this" matrix is left unchanged. It is not filled with zeroes!
  153. void MatrixRmn::LoadAsSubmatrixTranspose( const MatrixRmn& A )
  154. {
  155. assert( A.NumRows<=NumCols && A.NumCols<=NumRows );
  156. double* rowPtr = x;
  157. double* from = A.x;
  158. for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, once per loop
  159. double* to = rowPtr;
  160. for ( long j=A.NumRows; j>0; j-- ) { // Loop copying values from the column of A
  161. *to = *(from++);
  162. to += NumRows;
  163. }
  164. rowPtr ++;
  165. }
  166. }
  167. // Calculate the Frobenius Norm (square root of sum of squares of entries of the matrix)
  168. double MatrixRmn::FrobeniusNorm() const
  169. {
  170. return sqrt( FrobeniusNormSq() );
  171. }
  172. // Multiply this matrix by column vector v.
  173. // Result is column vector "result"
  174. void MatrixRmn::Multiply( const VectorRn& v, VectorRn& result ) const
  175. {
  176. assert ( v.GetLength()==NumCols && result.GetLength()==NumRows );
  177. double* out = result.GetPtr(); // Points to entry in result vector
  178. const double* rowPtr = x; // Points to beginning of next row in matrix
  179. for ( long j = NumRows; j>0; j-- ) {
  180. const double* in = v.GetPtr();
  181. const double* m = rowPtr++;
  182. *out = 0.0f;
  183. for ( long i = NumCols; i>0; i-- ) {
  184. *out += (*(in++)) * (*m);
  185. m += NumRows;
  186. }
  187. out++;
  188. }
  189. }
  190. // Multiply transpose of this matrix by column vector v.
  191. // Result is column vector "result"
  192. // Equivalent to mult by row vector on left
  193. void MatrixRmn::MultiplyTranspose( const VectorRn& v, VectorRn& result ) const
  194. {
  195. assert ( v.GetLength()==NumRows && result.GetLength()==NumCols );
  196. double* out = result.GetPtr(); // Points to entry in result vector
  197. const double* colPtr = x; // Points to beginning of next column in matrix
  198. for ( long i=NumCols; i>0; i-- ) {
  199. const double* in=v.GetPtr();
  200. *out = 0.0f;
  201. for ( long j = NumRows; j>0; j-- ) {
  202. *out += (*(in++)) * (*(colPtr++));
  203. }
  204. out++;
  205. }
  206. }
  207. // Form the dot product of a vector v with the i-th column of the array
  208. double MatrixRmn::DotProductColumn( const VectorRn& v, long colNum ) const
  209. {
  210. assert ( v.GetLength()==NumRows );
  211. double* ptrC = x+colNum*NumRows;
  212. double* ptrV = v.x;
  213. double ret = 0.0;
  214. for ( long i = NumRows; i>0; i-- ) {
  215. ret += (*(ptrC++))*(*(ptrV++));
  216. }
  217. return ret;
  218. }
  219. // Add a constant to each entry on the diagonal
  220. MatrixRmn& MatrixRmn::AddToDiagonal( double d ) // Adds d to each diagonal entry
  221. {
  222. long diagLen = Min( NumRows, NumCols );
  223. double* dPtr = x;
  224. for ( ; diagLen>0; diagLen-- ) {
  225. *dPtr += d;
  226. dPtr += NumRows+1;
  227. }
  228. return *this;
  229. }
  230. // Multiply two MatrixRmn's
  231. MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
  232. {
  233. assert( A.NumCols == B.NumRows && A.NumRows == dst.NumRows && B.NumCols == dst.NumCols );
  234. long length = A.NumCols;
  235. double *bPtr = B.x; // Points to beginning of column in B
  236. double *dPtr = dst.x;
  237. for ( long i = dst.NumCols; i>0; i-- ) {
  238. double *aPtr = A.x; // Points to beginning of row in A
  239. for ( long j = dst.NumRows; j>0; j-- ) {
  240. *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, 1 );
  241. dPtr++;
  242. aPtr++;
  243. }
  244. bPtr += B.NumRows;
  245. }
  246. return dst;
  247. }
  248. // Multiply two MatrixRmn's, Transpose the first matrix before multiplying
  249. MatrixRmn& MatrixRmn::TransposeMultiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
  250. {
  251. assert( A.NumRows == B.NumRows && A.NumCols == dst.NumRows && B.NumCols == dst.NumCols );
  252. long length = A.NumRows;
  253. double *bPtr = B.x; // bPtr Points to beginning of column in B
  254. double *dPtr = dst.x;
  255. for ( long i = dst.NumCols; i>0; i-- ) { // Loop over all columns of dst
  256. double *aPtr = A.x; // aPtr Points to beginning of column in A
  257. for ( long j = dst.NumRows; j>0; j-- ) { // Loop over all rows of dst
  258. *dPtr = DotArray( length, aPtr, 1, bPtr, 1 );
  259. dPtr ++;
  260. aPtr += A.NumRows;
  261. }
  262. bPtr += B.NumRows;
  263. }
  264. return dst;
  265. }
  266. // Multiply two MatrixRmn's. Transpose the second matrix before multiplying
  267. MatrixRmn& MatrixRmn::MultiplyTranspose( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
  268. {
  269. assert( A.NumCols == B.NumCols && A.NumRows == dst.NumRows && B.NumRows == dst.NumCols );
  270. long length = A.NumCols;
  271. double *bPtr = B.x; // Points to beginning of row in B
  272. double *dPtr = dst.x;
  273. for ( long i = dst.NumCols; i>0; i-- ) {
  274. double *aPtr = A.x; // Points to beginning of row in A
  275. for ( long j = dst.NumRows; j>0; j-- ) {
  276. *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, B.NumRows );
  277. dPtr++;
  278. aPtr++;
  279. }
  280. bPtr++;
  281. }
  282. return dst;
  283. }
  284. // Solves the equation (*this)*xVec = b;
  285. // Uses row operations. Assumes *this is square and invertible.
  286. // No error checking for divide by zero or instability (except with asserts)
  287. void MatrixRmn::Solve( const VectorRn& b, VectorRn* xVec ) const
  288. {
  289. assert ( NumRows==NumCols && NumCols==xVec->GetLength() && NumRows==b.GetLength() );
  290. // Copy this matrix and b into an Augmented Matrix
  291. MatrixRmn& AugMat = GetWorkMatrix( NumRows, NumCols+1 );
  292. AugMat.LoadAsSubmatrix( *this );
  293. AugMat.SetColumn( NumRows, b );
  294. // Put into row echelon form with row operations
  295. AugMat.ConvertToRefNoFree();
  296. // Solve for x vector values using back substitution
  297. double* xLast = xVec->x+NumRows-1; // Last entry in xVec
  298. double* endRow = AugMat.x+NumRows*NumCols-1; // Last entry in the current row of the coefficient part of Augmented Matrix
  299. double* bPtr = endRow+NumRows; // Last entry in augmented matrix (end of last column, in augmented part)
  300. for ( long i = NumRows; i>0; i-- ) {
  301. double accum = *(bPtr--);
  302. // Next loop computes back substitution terms
  303. double* rowPtr = endRow; // Points to entries of the current row for back substitution.
  304. double* xPtr = xLast; // Points to entries in the x vector (also for back substitution)
  305. for ( long j=NumRows-i; j>0; j-- ) {
  306. accum -= (*rowPtr)*(*(xPtr--));
  307. rowPtr -= NumCols; // Previous entry in the row
  308. }
  309. assert( *rowPtr != 0.0 ); // Are not supposed to be any free variables in this matrix
  310. *xPtr = accum/(*rowPtr);
  311. endRow--;
  312. }
  313. }
  314. // ConvertToRefNoFree
  315. // Converts the matrix (in place) to row echelon form
  316. // For us, row echelon form allows any non-zero values, not just 1's, in the
  317. // position for a lead variable.
  318. // The "NoFree" version operates on the assumption that no free variable will be found.
  319. // Algorithm uses row operations and row pivoting (only).
  320. // Augmented matrix is correctly accomodated. Only the first square part participates
  321. // in the main work of row operations.
  322. void MatrixRmn::ConvertToRefNoFree()
  323. {
  324. // Loop over all columns (variables)
  325. // Find row with most non-zero entry.
  326. // Swap to the highest active row
  327. // Subtract appropriately from all the lower rows (row op of type 3)
  328. long numIters = Min(NumRows,NumCols);
  329. double* rowPtr1 = x;
  330. const long diagStep = NumRows+1;
  331. long lenRowLeft = NumCols;
  332. for ( ; numIters>1; numIters-- ) {
  333. // Find row with most non-zero entry.
  334. double* rowPtr2 = rowPtr1;
  335. double maxAbs = fabs(*rowPtr1);
  336. double *rowPivot = rowPtr1;
  337. long i;
  338. for ( i=numIters-1; i>0; i-- ) {
  339. const double& newMax = *(++rowPivot);
  340. if ( newMax > maxAbs ) {
  341. maxAbs = *rowPivot;
  342. rowPtr2 = rowPivot;
  343. }
  344. else if ( -newMax > maxAbs ) {
  345. maxAbs = -newMax;
  346. rowPtr2 = rowPivot;
  347. }
  348. }
  349. // Pivot step: Swap the row with highest entry to the current row
  350. if ( rowPtr1 != rowPtr2 ) {
  351. double *to = rowPtr1;
  352. for ( long i=lenRowLeft; i>0; i-- ) {
  353. double temp = *to;
  354. *to = *rowPtr2;
  355. *rowPtr2 = temp;
  356. to += NumRows;
  357. rowPtr2 += NumRows;
  358. }
  359. }
  360. // Subtract this row appropriately from all the lower rows (row operation of type 3)
  361. rowPtr2 = rowPtr1;
  362. for ( i=numIters-1; i>0; i-- ) {
  363. rowPtr2++;
  364. double* to = rowPtr2;
  365. double* from = rowPtr1;
  366. assert( *from != 0.0 );
  367. double alpha = (*to)/(*from);
  368. *to = 0.0;
  369. for ( long j=lenRowLeft-1; j>0; j-- ) {
  370. to += NumRows;
  371. from += NumRows;
  372. *to -= (*from)*alpha;
  373. }
  374. }
  375. // Update for next iteration of loop
  376. rowPtr1 += diagStep;
  377. lenRowLeft--;
  378. }
  379. }
  380. // Calculate the c=cosine and s=sine values for a Givens transformation.
  381. // The matrix M = ( (c, -s), (s, c) ) in row order transforms the
  382. // column vector (a, b)^T to have y-coordinate zero.
  383. void MatrixRmn::CalcGivensValues( double a, double b, double *c, double *s )
  384. {
  385. double denomInv = sqrt(a*a + b*b);
  386. if ( denomInv==0.0 ) {
  387. *c = 1.0;
  388. *s = 0.0;
  389. }
  390. else {
  391. denomInv = 1.0/denomInv;
  392. *c = a*denomInv;
  393. *s = -b*denomInv;
  394. }
  395. }
  396. // Applies Givens transform to columns i and i+1.
  397. // Equivalent to postmultiplying by the matrix
  398. // ( c -s )
  399. // ( s c )
  400. // with non-zero entries in rows i and i+1 and columns i and i+1
  401. void MatrixRmn::PostApplyGivens( double c, double s, long idx )
  402. {
  403. assert ( 0<=idx && idx<NumCols );
  404. double *colA = x + idx*NumRows;
  405. double *colB = colA + NumRows;
  406. for ( long i = NumRows; i>0; i-- ) {
  407. double temp = *colA;
  408. *colA = (*colA)*c + (*colB)*s;
  409. *colB = (*colB)*c - temp*s;
  410. colA++;
  411. colB++;
  412. }
  413. }
  414. // Applies Givens transform to columns idx1 and idx2.
  415. // Equivalent to postmultiplying by the matrix
  416. // ( c -s )
  417. // ( s c )
  418. // with non-zero entries in rows idx1 and idx2 and columns idx1 and idx2
  419. void MatrixRmn::PostApplyGivens( double c, double s, long idx1, long idx2 )
  420. {
  421. assert ( idx1!=idx2 && 0<=idx1 && idx1<NumCols && 0<=idx2 && idx2<NumCols );
  422. double *colA = x + idx1*NumRows;
  423. double *colB = x + idx2*NumRows;
  424. for ( long i = NumRows; i>0; i-- ) {
  425. double temp = *colA;
  426. *colA = (*colA)*c + (*colB)*s;
  427. *colB = (*colB)*c - temp*s;
  428. colA++;
  429. colB++;
  430. }
  431. }
  432. // ********************************************************************************************
  433. // Singular value decomposition.
  434. // Return othogonal matrices U and V and diagonal matrix with diagonal w such that
  435. // (this) = U * Diag(w) * V^T (V^T is V-transpose.)
  436. // Diagonal entries have all non-zero entries before all zero entries, but are not
  437. // necessarily sorted. (Someday, I will write ComputedSortedSVD that handles
  438. // sorting the eigenvalues by magnitude.)
  439. // ********************************************************************************************
  440. void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
  441. {
  442. assert ( U.NumRows==NumRows && V.NumCols==NumCols
  443. && U.NumRows==U.NumCols && V.NumRows==V.NumCols
  444. && w.GetLength()==Min(NumRows,NumCols) );
  445. double temp=0.0;
  446. VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 ); // Some extra work space. Will get passed around.
  447. // Choose larger of U, V to hold intermediate results
  448. // If U is larger than V, use U to store intermediate results
  449. // Otherwise use V. In the latter case, we form the SVD of A transpose,
  450. // (which is essentially identical to the SVD of A).
  451. MatrixRmn* leftMatrix;
  452. MatrixRmn* rightMatrix;
  453. if ( NumRows >= NumCols ) {
  454. U.LoadAsSubmatrix( *this ); // Copy A into U
  455. leftMatrix = &U;
  456. rightMatrix = &V;
  457. }
  458. else {
  459. V.LoadAsSubmatrixTranspose( *this ); // Copy A-transpose into V
  460. leftMatrix = &V;
  461. rightMatrix = &U;
  462. }
  463. // Do the actual work to calculate the SVD
  464. // Now matrix has at least as many rows as columns
  465. CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag );
  466. ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag );
  467. }
  468. void MatrixRmn::ComputeInverse( MatrixRmn& R) const
  469. {
  470. assert ( this->NumRows==this->NumCols );
  471. MatrixRmn U(this->NumRows, this->NumCols);
  472. VectorRn w(this->NumRows);
  473. MatrixRmn V(this->NumRows, this->NumCols);
  474. this->ComputeSVD(U, w, V);
  475. assert(this->DebugCheckSVD(U, w , V));
  476. double PseudoInverseThresholdFactor = 0.01;
  477. double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs();
  478. MatrixRmn VD(this->NumRows, this->NumCols);
  479. MatrixRmn D(this->NumRows, this->NumCols);
  480. D.SetZero();
  481. long diagLength = w.GetLength();
  482. double* wPtr = w.GetPtr();
  483. for ( long i = 0; i < diagLength; ++i ) {
  484. double alpha = *(wPtr++);
  485. if ( fabs(alpha)>pseudoInverseThreshold ) {
  486. D.Set(i, i, 1.0/alpha);
  487. }
  488. }
  489. Multiply(V,D,VD);
  490. MultiplyTranspose(VD,U,R);
  491. }
  492. // ************************************************ CalcBidiagonal **************************
  493. // Helper routine for SVD computation
  494. // U is a matrix to be bidiagonalized.
  495. // On return, U and V are orthonormal and w holds the new diagonal
  496. // elements and superDiag holds the super diagonal elements.
  497. void MatrixRmn::CalcBidiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag )
  498. {
  499. assert ( U.NumRows>=V.NumRows );
  500. // The diagonal and superdiagonal entries of the bidiagonalized
  501. // version of the U matrix
  502. // are stored in the vectors w and superDiag (temporarily).
  503. // Apply Householder transformations to U.
  504. // Householder transformations come in pairs.
  505. // First, on the left, we map a portion of a column to zeros
  506. // Second, on the right, we map a portion of a row to zeros
  507. const long rowStep = U.NumCols;
  508. const long diagStep = U.NumCols+1;
  509. double *diagPtr = U.x;
  510. double* wPtr = w.x;
  511. double* superDiagPtr = superDiag.x;
  512. long colLengthLeft = U.NumRows;
  513. long rowLengthLeft = V.NumCols;
  514. while (true) {
  515. // Apply a Householder xform on left to zero part of a column
  516. SvdHouseholder( diagPtr, colLengthLeft, rowLengthLeft, 1, rowStep, wPtr );
  517. if ( rowLengthLeft==2 ) {
  518. *superDiagPtr = *(diagPtr+rowStep);
  519. break;
  520. }
  521. // Apply a Householder xform on the right to zero part of a row
  522. SvdHouseholder( diagPtr+rowStep, rowLengthLeft-1, colLengthLeft, rowStep, 1, superDiagPtr );
  523. rowLengthLeft--;
  524. colLengthLeft--;
  525. diagPtr += diagStep;
  526. wPtr++;
  527. superDiagPtr++;
  528. }
  529. int extra = 0;
  530. diagPtr += diagStep;
  531. wPtr++;
  532. if ( colLengthLeft > 2 ) {
  533. extra = 1;
  534. // Do one last Householder transformation when the matrix is not square
  535. colLengthLeft--;
  536. SvdHouseholder( diagPtr, colLengthLeft, 1, 1, 0, wPtr );
  537. }
  538. else {
  539. *wPtr = *diagPtr;
  540. }
  541. // Form U and V from the Householder transformations
  542. V.ExpandHouseholders( V.NumCols-2, 1, U.x+U.NumRows, U.NumRows, 1 );
  543. U.ExpandHouseholders( V.NumCols-1+extra, 0, U.x, 1, U.NumRows );
  544. // Done with bidiagonalization
  545. return;
  546. }
  547. // Helper routine for CalcBidiagonal
  548. // Performs a series of Householder transformations on a matrix
  549. // Stores results compactly into the matrix: The Householder vector u (normalized)
  550. // is stored into the first row/column being transformed.
  551. // The leading term of that row (= plus/minus its magnitude is returned
  552. // separately into "retFirstEntry"
  553. void MatrixRmn::SvdHouseholder( double* basePt,
  554. long colLength, long numCols, long colStride, long rowStride,
  555. double* retFirstEntry )
  556. {
  557. // Calc norm of vector u
  558. double* cPtr = basePt;
  559. double norm = 0.0;
  560. long i;
  561. for ( i=colLength; i>0 ; i-- ) {
  562. norm += Square( *cPtr );
  563. cPtr += colStride;
  564. }
  565. norm = sqrt(norm); // Norm of vector to reflect to axis e_1
  566. // Handle sign issues
  567. double imageVal; // Choose sign to maximize distance
  568. if ( (*basePt) < 0.0 ) {
  569. imageVal = norm;
  570. norm = 2.0*norm*(norm-(*basePt));
  571. }
  572. else {
  573. imageVal = -norm;
  574. norm = 2.0*norm*(norm+(*basePt));
  575. }
  576. norm = sqrt(norm); // Norm is norm of reflection vector
  577. if ( norm==0.0 ) { // If the vector being transformed is equal to zero
  578. // Force to zero in case of roundoff errors
  579. cPtr = basePt;
  580. for ( i=colLength; i>0; i-- ) {
  581. *cPtr = 0.0;
  582. cPtr += colStride;
  583. }
  584. *retFirstEntry = 0.0;
  585. return;
  586. }
  587. *retFirstEntry = imageVal;
  588. // Set up the normalized Householder vector
  589. *basePt -= imageVal; // First component changes. Rest stay the same.
  590. // Normalize the vector
  591. norm = 1.0/norm; // Now it is the inverse norm
  592. cPtr = basePt;
  593. for ( i=colLength; i>0 ; i-- ) {
  594. *cPtr *= norm;
  595. cPtr += colStride;
  596. }
  597. // Transform the rest of the U matrix with the Householder transformation
  598. double *rPtr = basePt;
  599. for ( long j=numCols-1; j>0; j-- ) {
  600. rPtr += rowStride;
  601. // Calc dot product with Householder transformation vector
  602. double dotP = DotArray( colLength, basePt, colStride, rPtr, colStride );
  603. // Transform with I - 2*dotP*(Householder vector)
  604. AddArrayScale( colLength, basePt, colStride, rPtr, colStride, -2.0*dotP );
  605. }
  606. }
  607. // ********************************* ExpandHouseholders ********************************************
  608. // The matrix will be square.
  609. // numXforms = number of Householder transformations to concatenate
  610. // Each Householder transformation is represented by a unit vector
  611. // Each successive Householder transformation starts one position later
  612. // and has one more implied leading zero
  613. // basePt = beginning of the first Householder transform
  614. // colStride, rowStride: Householder xforms are stored in "columns"
  615. // numZerosSkipped is the number of implicit zeros on the front each
  616. // Householder transformation vector (only values supported are 0 and 1).
  617. void MatrixRmn::ExpandHouseholders( long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride )
  618. {
  619. // Number of applications of the last Householder transform
  620. // (That are not trivial!)
  621. long numToTransform = NumCols-numXforms+1-numZerosSkipped;
  622. assert( numToTransform>0 );
  623. if ( numXforms==0 ) {
  624. SetIdentity();
  625. return;
  626. }
  627. // Handle the first one separately as a special case,
  628. // "this" matrix will be treated to simulate being preloaded with the identity
  629. long hDiagStride = rowStride+colStride;
  630. const double* hBase = basePt + hDiagStride*(numXforms-1); // Pointer to the last Householder vector
  631. const double* hDiagPtr = hBase + colStride*(numToTransform-1); // Pointer to last entry in that vector
  632. long i;
  633. double* diagPtr = x+NumCols*NumRows-1; // Last entry in matrix (points to diagonal entry)
  634. double* colPtr = diagPtr-(numToTransform-1); // Pointer to column in matrix
  635. for ( i=numToTransform; i>0; i-- ) {
  636. CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hDiagPtr) );
  637. *diagPtr += 1.0; // Add back in 1 to the diagonal entry (since xforming the identity)
  638. diagPtr -= (NumRows+1); // Next diagonal entry in this matrix
  639. colPtr -= NumRows; // Next column in this matrix
  640. hDiagPtr -= colStride;
  641. }
  642. // Now handle the general case
  643. // A row of zeros must be in effect added to the top of each old column (in each loop)
  644. double* colLastPtr = x + NumRows*NumCols - numToTransform - 1;
  645. for ( i = numXforms-1; i>0; i-- ) {
  646. numToTransform++; // Number of non-trivial applications of this Householder transformation
  647. hBase -= hDiagStride; // Pointer to the beginning of the Householder transformation
  648. colPtr = colLastPtr;
  649. for ( long j = numToTransform-1; j>0; j-- ) {
  650. // Get dot product
  651. double dotProd2N = -2.0*DotArray( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1 );
  652. *colPtr = dotProd2N*(*hBase); // Adding onto zero at initial point
  653. AddArrayScale( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1, dotProd2N );
  654. colPtr -= NumRows;
  655. }
  656. // Do last one as a special case (may overwrite the Householder vector)
  657. CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hBase) );
  658. *colPtr += 1.0; // Add back one one as identity
  659. // Done with this Householder transformation
  660. colLastPtr --;
  661. }
  662. if ( numZerosSkipped!=0 ) {
  663. assert( numZerosSkipped==1 );
  664. // Fill first row and column with identity (More generally: first numZerosSkipped many rows and columns)
  665. double* d = x;
  666. *d = 1;
  667. double* d2 = d;
  668. for ( i=NumRows-1; i>0; i-- ) {
  669. *(++d) = 0;
  670. *(d2+=NumRows) = 0;
  671. }
  672. }
  673. }
  674. // **************** ConvertBidiagToDiagonal ***********************************************
  675. // Do the iterative transformation from bidiagonal form to diagonal form using
  676. // Givens transformation. (Golub-Reinsch)
  677. // U and V are square. Size of U less than or equal to that of U.
  678. void MatrixRmn::ConvertBidiagToDiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) const
  679. {
  680. // These two index into the last bidiagonal block (last in the matrix, it will be
  681. // first one handled.
  682. long lastBidiagIdx = V.NumRows-1;
  683. long firstBidiagIdx = 0;
  684. double eps = 1.0e-15 * Max(w.MaxAbs(), superDiag.MaxAbs());
  685. while ( true ) {
  686. bool workLeft = UpdateBidiagIndices( &firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps );
  687. if ( !workLeft ) {
  688. break;
  689. }
  690. // Get ready for first Givens rotation
  691. // Push non-zero to M[2,1] with Givens transformation
  692. double* wPtr = w.x+firstBidiagIdx;
  693. double* sdPtr = superDiag.x+firstBidiagIdx;
  694. double extraOffDiag=0.0;
  695. if ( (*wPtr)==0.0 ) {
  696. ClearRowWithDiagonalZero( firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr, eps );
  697. if ( firstBidiagIdx>0 ) {
  698. if ( NearZero( *(--sdPtr), eps ) ) {
  699. *sdPtr = 0.0;
  700. }
  701. else {
  702. ClearColumnWithDiagonalZero( firstBidiagIdx, V, wPtr, sdPtr, eps );
  703. }
  704. }
  705. continue;
  706. }
  707. // Estimate an eigenvalue from bottom four entries of M
  708. // This gives a lambda value which will shift the Givens rotations
  709. // Last four entries of M^T * M are ( ( A, B ), ( B, C ) ).
  710. double A;
  711. A = (firstBidiagIdx<lastBidiagIdx-1) ? Square(superDiag[lastBidiagIdx-2]): 0.0;
  712. double BSq = Square(w[lastBidiagIdx-1]);
  713. A += BSq; // The "A" entry of M^T * M
  714. double C = Square(superDiag[lastBidiagIdx-1]);
  715. BSq *= C; // The squared "B" entry
  716. C += Square(w[lastBidiagIdx]); // The "C" entry
  717. double lambda; // lambda will hold the estimated eigenvalue
  718. lambda = sqrt( Square((A-C)*0.5) + BSq ); // Use the lambda value that is closest to C.
  719. if ( A > C ) {
  720. lambda = -lambda;
  721. }
  722. lambda += (A+C)*0.5; // Now lambda equals the estimate for the last eigenvalue
  723. double t11 = Square(w[firstBidiagIdx]);
  724. double t12 = w[firstBidiagIdx]*superDiag[firstBidiagIdx];
  725. double c, s;
  726. CalcGivensValues( t11-lambda, t12, &c, &s );
  727. ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, wPtr+1 );
  728. V.PostApplyGivens( c, -s, firstBidiagIdx );
  729. long i;
  730. for ( i=firstBidiagIdx; i<lastBidiagIdx-1; i++ ) {
  731. // Push non-zero from M[i+1,i] to M[i,i+2]
  732. CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
  733. ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr+1, sdPtr+1 );
  734. U.PostApplyGivens( c, -s, i );
  735. // Push non-zero from M[i,i+2] to M[1+2,i+1]
  736. CalcGivensValues( *sdPtr, extraOffDiag, &c, &s );
  737. ApplyGivensCBTD( c, s, sdPtr, wPtr+1, &extraOffDiag, extraOffDiag, sdPtr+1, wPtr+2 );
  738. V.PostApplyGivens( c, -s, i+1 );
  739. wPtr++;
  740. sdPtr++;
  741. }
  742. // Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1
  743. CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
  744. ApplyGivensCBTD( c, s, wPtr, &extraOffDiag, sdPtr, wPtr+1 );
  745. U.PostApplyGivens( c, -s, i );
  746. // DEBUG
  747. // DebugCalcBidiagCheck( V, w, superDiag, U );
  748. }
  749. }
  750. // This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry on the same row.
  751. // We use Givens rotations to "chase" the non-zero entry across the row; when it reaches the last
  752. // column, it is finally zeroed away.
  753. // wPtr points to the zero entry on the diagonal. sdPtr points to the non-zero superdiagonal entry on the same row.
  754. void MatrixRmn::ClearRowWithDiagonalZero( long firstBidiagIdx, long lastBidiagIdx, MatrixRmn& U, double *wPtr, double *sdPtr, double eps )
  755. {
  756. double curSd = *sdPtr; // Value being chased across the row
  757. *sdPtr = 0.0;
  758. long i=firstBidiagIdx+1;
  759. while (true) {
  760. // Rotate row i and row firstBidiagIdx (Givens rotation)
  761. double c, s;
  762. CalcGivensValues( *(++wPtr), curSd, &c, &s );
  763. U.PostApplyGivens( c, -s, i, firstBidiagIdx );
  764. *wPtr = c*(*wPtr) - s*curSd;
  765. if ( i==lastBidiagIdx ) {
  766. break;
  767. }
  768. curSd = s*(*(++sdPtr)); // New value pops up one column over to the right
  769. *sdPtr = c*(*sdPtr);
  770. i++;
  771. }
  772. }
  773. // This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry in the same column.
  774. // We use Givens rotations to "chase" the non-zero entry up the column; when it reaches the last
  775. // column, it is finally zeroed away.
  776. // wPtr points to the zero entry on the diagonal. sdPtr points to the non-zero superdiagonal entry in the same column.
  777. void MatrixRmn::ClearColumnWithDiagonalZero( long endIdx, MatrixRmn& V, double *wPtr, double *sdPtr, double eps )
  778. {
  779. double curSd = *sdPtr; // Value being chased up the column
  780. *sdPtr = 0.0;
  781. long i = endIdx-1;
  782. while ( true ) {
  783. double c, s;
  784. CalcGivensValues( *(--wPtr), curSd, &c, &s );
  785. V.PostApplyGivens( c, -s, i, endIdx );
  786. *wPtr = c*(*wPtr) - s*curSd;
  787. if ( i==0 ) {
  788. break;
  789. }
  790. curSd = s*(*(--sdPtr)); // New value pops up one row above
  791. if ( NearZero( curSd, eps ) ) {
  792. break;
  793. }
  794. *sdPtr = c*(*sdPtr);
  795. i--;
  796. }
  797. }
  798. // Matrix A is ( ( a c ) ( b d ) ), i.e., given in column order.
  799. // Mult's G[c,s] times A, replaces A.
  800. void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, double *d )
  801. {
  802. double temp = *a;
  803. *a = cosine*(*a) - sine*(*b);
  804. *b = sine*temp + cosine*(*b);
  805. temp = *c;
  806. *c = cosine*(*c) - sine*(*d);
  807. *d = sine*temp + cosine*(*d);
  808. }
  809. // Now matrix A given in row order, A = ( ( a b c ) ( d e f ) ).
  810. // Return G[c,s] * A, replace A. d becomes zero, no need to return.
  811. // Also, it is certain the old *c value is taken to be zero!
  812. void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c,
  813. double d, double *e, double *f )
  814. {
  815. *a = cosine*(*a) - sine*d;
  816. double temp = *b;
  817. *b = cosine*(*b) - sine*(*e);
  818. *e = sine*temp + cosine*(*e);
  819. *c = -sine*(*f);
  820. *f = cosine*(*f);
  821. }
  822. // Helper routine for SVD conversion from bidiagonal to diagonal
  823. bool MatrixRmn::UpdateBidiagIndices( long *firstBidiagIdx, long *lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps )
  824. {
  825. long lastIdx = *lastBidiagIdx;
  826. double* sdPtr = superDiag.GetPtr( lastIdx-1 ); // Entry above the last diagonal entry
  827. while ( NearZero(*sdPtr, eps) ) {
  828. *(sdPtr--) = 0.0;
  829. lastIdx--;
  830. if ( lastIdx == 0 ) {
  831. return false;
  832. }
  833. }
  834. *lastBidiagIdx = lastIdx;
  835. long firstIdx = lastIdx-1;
  836. double* wPtr = w.GetPtr( firstIdx );
  837. while ( firstIdx > 0 ) {
  838. if ( NearZero( *wPtr, eps ) ) { // If this diagonal entry (near) zero
  839. *wPtr = 0.0;
  840. break;
  841. }
  842. if ( NearZero(*(--sdPtr), eps) ) { // If the entry above the diagonal entry is (near) zero
  843. *sdPtr = 0.0;
  844. break;
  845. }
  846. wPtr--;
  847. firstIdx--;
  848. }
  849. *firstBidiagIdx = firstIdx;
  850. return true;
  851. }
  852. // ******************************************DEBUG STUFFF
  853. bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const
  854. {
  855. // Special SVD test code
  856. MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
  857. IV.SetIdentity();
  858. MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
  859. MatrixRmn::TransposeMultiply( V, V, VTV );
  860. IV -= VTV;
  861. double error = IV.FrobeniusNorm();
  862. MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
  863. IU.SetIdentity();
  864. MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
  865. MatrixRmn::TransposeMultiply( U, U, UTU );
  866. IU -= UTU;
  867. error += IU.FrobeniusNorm();
  868. MatrixRmn Diag( U.GetNumRows(), V.GetNumRows() );
  869. Diag.SetZero();
  870. Diag.SetDiagonalEntries( w );
  871. MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
  872. MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
  873. MatrixRmn::Multiply( U, Diag, B );
  874. MatrixRmn::MultiplyTranspose( B, V, C );
  875. C -= *this;
  876. error += C.FrobeniusNorm();
  877. bool ret = ( fabs(error)<=1.0e-13*w.MaxAbs() );
  878. assert ( ret );
  879. return ret;
  880. }
  881. bool MatrixRmn::DebugCheckInverse( const MatrixRmn& MInv ) const
  882. {
  883. assert ( this->NumRows==this->NumCols );
  884. assert ( MInv.NumRows==MInv.NumCols );
  885. MatrixRmn I(this->NumRows, this->NumCols);
  886. I.SetIdentity();
  887. MatrixRmn MMInv(this->NumRows, this->NumCols);
  888. Multiply(*this, MInv, MMInv);
  889. I -= MMInv;
  890. double error = I.FrobeniusNorm();
  891. bool ret = ( fabs(error)<=1.0e-13 );
  892. assert ( ret );
  893. return ret;
  894. }
  895. bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
  896. {
  897. // Special SVD test code
  898. MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
  899. IV.SetIdentity();
  900. MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
  901. MatrixRmn::TransposeMultiply( V, V, VTV );
  902. IV -= VTV;
  903. double error = IV.FrobeniusNorm();
  904. MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
  905. IU.SetIdentity();
  906. MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
  907. MatrixRmn::TransposeMultiply( U, U, UTU );
  908. IU -= UTU;
  909. error += IU.FrobeniusNorm();
  910. MatrixRmn DiagAndSuper( U.GetNumRows(), V.GetNumRows() );
  911. DiagAndSuper.SetZero();
  912. DiagAndSuper.SetDiagonalEntries( w );
  913. if ( this->GetNumRows()>=this->GetNumColumns() ) {
  914. DiagAndSuper.SetSequence( superDiag, 0, 1, 1, 1 );
  915. }
  916. else {
  917. DiagAndSuper.SetSequence( superDiag, 1, 0, 1, 1 );
  918. }
  919. MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
  920. MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
  921. MatrixRmn::Multiply( U, DiagAndSuper, B );
  922. MatrixRmn::MultiplyTranspose( B, V, C );
  923. C -= *this;
  924. error += C.FrobeniusNorm();
  925. bool ret = ( fabs(error)<1.0e-13*Max(w.MaxAbs(),superDiag.MaxAbs()) );
  926. assert ( ret );
  927. return ret;
  928. }