123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029 |
- /*
- *
- * Mathematics Subpackage (VrMath)
- *
- *
- * Author: Samuel R. Buss, [email protected].
- * Web page: http://math.ucsd.edu/~sbuss/MathCG
- *
- *
- This software is provided 'as-is', without any express or implied warranty.
- In no event will the authors be held liable for any damages arising from the use of this software.
- Permission is granted to anyone to use this software for any purpose,
- including commercial applications, and to alter it and redistribute it freely,
- subject to the following restrictions:
- 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.
- 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
- 3. This notice may not be removed or altered from any source distribution.
- *
- *
- */
- //
- // MatrixRmn.cpp: Matrix over reals (Variable dimensional vector)
- //
- // Not very sophisticated yet. Needs more functionality
- // To do: better handling of resizing.
- //
- #include "MatrixRmn.h"
- MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix
- // Fill the diagonal entries with the value d. The rest of the matrix is unchanged.
- void MatrixRmn::SetDiagonalEntries( double d )
- {
- long diagLen = Min( NumRows, NumCols );
- double* dPtr = x;
- for ( ; diagLen>0; diagLen-- ) {
- *dPtr = d;
- dPtr += NumRows+1;
- }
- }
- // Fill the diagonal entries with values in vector d. The rest of the matrix is unchanged.
- void MatrixRmn::SetDiagonalEntries( const VectorRn& d )
- {
- long diagLen = Min( NumRows, NumCols );
- assert ( d.length == diagLen );
- double* dPtr = x;
- double* from = d.x;
- for ( ; diagLen>0; diagLen-- ) {
- *dPtr = *(from++);
- dPtr += NumRows+1;
- }
- }
- // Fill the superdiagonal entries with the value d. The rest of the matrix is unchanged.
- void MatrixRmn::SetSuperDiagonalEntries( double d )
- {
- long sDiagLen = Min( NumRows, (long)(NumCols-1) );
- double* to = x + NumRows;
- for ( ; sDiagLen>0; sDiagLen-- ) {
- *to = d;
- to += NumRows+1;
- }
- }
- // Fill the superdiagonal entries with values in vector d. The rest of the matrix is unchanged.
- void MatrixRmn::SetSuperDiagonalEntries( const VectorRn& d )
- {
- long sDiagLen = Min( (long)(NumRows-1), NumCols );
- assert ( sDiagLen == d.length );
- double* to = x + NumRows;
- double* from = d.x;
- for ( ; sDiagLen>0; sDiagLen-- ) {
- *to = *(from++);
- to += NumRows+1;
- }
- }
- // Fill the subdiagonal entries with the value d. The rest of the matrix is unchanged.
- void MatrixRmn::SetSubDiagonalEntries( double d )
- {
- long sDiagLen = Min( NumRows, NumCols ) - 1;
- double* to = x + 1;
- for ( ; sDiagLen>0; sDiagLen-- ) {
- *to = d;
- to += NumRows+1;
- }
- }
- // Fill the subdiagonal entries with values in vector d. The rest of the matrix is unchanged.
- void MatrixRmn::SetSubDiagonalEntries( const VectorRn& d )
- {
- long sDiagLen = Min( NumRows, NumCols ) - 1;
- assert ( sDiagLen == d.length );
- double* to = x + 1;
- double* from = d.x;
- for ( ; sDiagLen>0; sDiagLen-- ) {
- *to = *(from++);
- to += NumRows+1;
- }
- }
- // Set the i-th column equal to d.
- void MatrixRmn::SetColumn(long i, const VectorRn& d )
- {
- assert ( NumRows==d.GetLength() );
- double* to = x+i*NumRows;
- const double* from = d.x;
- for ( i=NumRows; i>0; i-- ) {
- *(to++) = *(from++);
- }
- }
- // Set the i-th column equal to d.
- void MatrixRmn::SetRow(long i, const VectorRn& d )
- {
- assert ( NumCols==d.GetLength() );
- double* to = x+i;
- const double* from = d.x;
- for ( i=NumRows; i>0; i-- ) {
- *to = *(from++);
- to += NumRows;
- }
- }
- // Sets a "linear" portion of the array with the values from a vector d
- // The first row and column position are given by startRow, startCol.
- // Successive positions are found by using the deltaRow, deltaCol values
- // to increment the row and column indices. There is no wrapping around.
- void MatrixRmn::SetSequence( const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol )
- {
- long length = d.length;
- assert( startRow>=0 && startRow<NumRows && startCol>=0 && startCol<NumCols );
- assert( startRow+(length-1)*deltaRow>=0 && startRow+(length-1)*deltaRow<NumRows );
- assert( startCol+(length-1)*deltaCol>=0 && startCol+(length-1)*deltaCol<NumCols );
- double *to = x + startRow + NumRows*startCol;
- double *from = d.x;
- long stride = deltaRow + NumRows*deltaCol;
- for ( ; length>0; length-- ) {
- *to = *(from++);
- to += stride;
- }
- }
- // The matrix A is loaded, in into "this" matrix, based at (0,0).
- // The size of "this" matrix must be large enough to accomodate A.
- // The rest of "this" matrix is left unchanged. It is not filled with zeroes!
- void MatrixRmn::LoadAsSubmatrix( const MatrixRmn& A )
- {
- assert( A.NumRows<=NumRows && A.NumCols<=NumCols );
- int extraColStep = NumRows - A.NumRows;
- double *to = x;
- double *from = A.x;
- for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, one per time thru loop
- for ( long j=A.NumRows; j>0; j-- ) { // Copy all elements of this column of A
- *(to++) = *(from++);
- }
- to += extraColStep;
- }
- }
- // The matrix A is loaded, in transposed order into "this" matrix, based at (0,0).
- // The size of "this" matrix must be large enough to accomodate A.
- // The rest of "this" matrix is left unchanged. It is not filled with zeroes!
- void MatrixRmn::LoadAsSubmatrixTranspose( const MatrixRmn& A )
- {
- assert( A.NumRows<=NumCols && A.NumCols<=NumRows );
- double* rowPtr = x;
- double* from = A.x;
- for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, once per loop
- double* to = rowPtr;
- for ( long j=A.NumRows; j>0; j-- ) { // Loop copying values from the column of A
- *to = *(from++);
- to += NumRows;
- }
- rowPtr ++;
- }
- }
- // Calculate the Frobenius Norm (square root of sum of squares of entries of the matrix)
- double MatrixRmn::FrobeniusNorm() const
- {
- return sqrt( FrobeniusNormSq() );
- }
- // Multiply this matrix by column vector v.
- // Result is column vector "result"
- void MatrixRmn::Multiply( const VectorRn& v, VectorRn& result ) const
- {
- assert ( v.GetLength()==NumCols && result.GetLength()==NumRows );
- double* out = result.GetPtr(); // Points to entry in result vector
- const double* rowPtr = x; // Points to beginning of next row in matrix
- for ( long j = NumRows; j>0; j-- ) {
- const double* in = v.GetPtr();
- const double* m = rowPtr++;
- *out = 0.0f;
- for ( long i = NumCols; i>0; i-- ) {
- *out += (*(in++)) * (*m);
- m += NumRows;
- }
- out++;
- }
- }
- // Multiply transpose of this matrix by column vector v.
- // Result is column vector "result"
- // Equivalent to mult by row vector on left
- void MatrixRmn::MultiplyTranspose( const VectorRn& v, VectorRn& result ) const
- {
- assert ( v.GetLength()==NumRows && result.GetLength()==NumCols );
- double* out = result.GetPtr(); // Points to entry in result vector
- const double* colPtr = x; // Points to beginning of next column in matrix
- for ( long i=NumCols; i>0; i-- ) {
- const double* in=v.GetPtr();
- *out = 0.0f;
- for ( long j = NumRows; j>0; j-- ) {
- *out += (*(in++)) * (*(colPtr++));
- }
- out++;
- }
- }
- // Form the dot product of a vector v with the i-th column of the array
- double MatrixRmn::DotProductColumn( const VectorRn& v, long colNum ) const
- {
- assert ( v.GetLength()==NumRows );
- double* ptrC = x+colNum*NumRows;
- double* ptrV = v.x;
- double ret = 0.0;
- for ( long i = NumRows; i>0; i-- ) {
- ret += (*(ptrC++))*(*(ptrV++));
- }
- return ret;
- }
- // Add a constant to each entry on the diagonal
- MatrixRmn& MatrixRmn::AddToDiagonal( double d ) // Adds d to each diagonal entry
- {
- long diagLen = Min( NumRows, NumCols );
- double* dPtr = x;
- for ( ; diagLen>0; diagLen-- ) {
- *dPtr += d;
- dPtr += NumRows+1;
- }
- return *this;
- }
- // Multiply two MatrixRmn's
- MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
- {
- assert( A.NumCols == B.NumRows && A.NumRows == dst.NumRows && B.NumCols == dst.NumCols );
- long length = A.NumCols;
- double *bPtr = B.x; // Points to beginning of column in B
- double *dPtr = dst.x;
- for ( long i = dst.NumCols; i>0; i-- ) {
- double *aPtr = A.x; // Points to beginning of row in A
- for ( long j = dst.NumRows; j>0; j-- ) {
- *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, 1 );
- dPtr++;
- aPtr++;
- }
- bPtr += B.NumRows;
- }
- return dst;
- }
- // Multiply two MatrixRmn's, Transpose the first matrix before multiplying
- MatrixRmn& MatrixRmn::TransposeMultiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
- {
- assert( A.NumRows == B.NumRows && A.NumCols == dst.NumRows && B.NumCols == dst.NumCols );
- long length = A.NumRows;
- double *bPtr = B.x; // bPtr Points to beginning of column in B
- double *dPtr = dst.x;
- for ( long i = dst.NumCols; i>0; i-- ) { // Loop over all columns of dst
- double *aPtr = A.x; // aPtr Points to beginning of column in A
- for ( long j = dst.NumRows; j>0; j-- ) { // Loop over all rows of dst
- *dPtr = DotArray( length, aPtr, 1, bPtr, 1 );
- dPtr ++;
- aPtr += A.NumRows;
- }
- bPtr += B.NumRows;
- }
- return dst;
- }
- // Multiply two MatrixRmn's. Transpose the second matrix before multiplying
- MatrixRmn& MatrixRmn::MultiplyTranspose( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
- {
- assert( A.NumCols == B.NumCols && A.NumRows == dst.NumRows && B.NumRows == dst.NumCols );
- long length = A.NumCols;
- double *bPtr = B.x; // Points to beginning of row in B
- double *dPtr = dst.x;
- for ( long i = dst.NumCols; i>0; i-- ) {
- double *aPtr = A.x; // Points to beginning of row in A
- for ( long j = dst.NumRows; j>0; j-- ) {
- *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, B.NumRows );
- dPtr++;
- aPtr++;
- }
- bPtr++;
- }
- return dst;
- }
- // Solves the equation (*this)*xVec = b;
- // Uses row operations. Assumes *this is square and invertible.
- // No error checking for divide by zero or instability (except with asserts)
- void MatrixRmn::Solve( const VectorRn& b, VectorRn* xVec ) const
- {
- assert ( NumRows==NumCols && NumCols==xVec->GetLength() && NumRows==b.GetLength() );
- // Copy this matrix and b into an Augmented Matrix
- MatrixRmn& AugMat = GetWorkMatrix( NumRows, NumCols+1 );
- AugMat.LoadAsSubmatrix( *this );
- AugMat.SetColumn( NumRows, b );
- // Put into row echelon form with row operations
- AugMat.ConvertToRefNoFree();
- // Solve for x vector values using back substitution
- double* xLast = xVec->x+NumRows-1; // Last entry in xVec
- double* endRow = AugMat.x+NumRows*NumCols-1; // Last entry in the current row of the coefficient part of Augmented Matrix
- double* bPtr = endRow+NumRows; // Last entry in augmented matrix (end of last column, in augmented part)
- for ( long i = NumRows; i>0; i-- ) {
- double accum = *(bPtr--);
- // Next loop computes back substitution terms
- double* rowPtr = endRow; // Points to entries of the current row for back substitution.
- double* xPtr = xLast; // Points to entries in the x vector (also for back substitution)
- for ( long j=NumRows-i; j>0; j-- ) {
- accum -= (*rowPtr)*(*(xPtr--));
- rowPtr -= NumCols; // Previous entry in the row
- }
- assert( *rowPtr != 0.0 ); // Are not supposed to be any free variables in this matrix
- *xPtr = accum/(*rowPtr);
- endRow--;
- }
- }
- // ConvertToRefNoFree
- // Converts the matrix (in place) to row echelon form
- // For us, row echelon form allows any non-zero values, not just 1's, in the
- // position for a lead variable.
- // The "NoFree" version operates on the assumption that no free variable will be found.
- // Algorithm uses row operations and row pivoting (only).
- // Augmented matrix is correctly accomodated. Only the first square part participates
- // in the main work of row operations.
- void MatrixRmn::ConvertToRefNoFree()
- {
- // Loop over all columns (variables)
- // Find row with most non-zero entry.
- // Swap to the highest active row
- // Subtract appropriately from all the lower rows (row op of type 3)
- long numIters = Min(NumRows,NumCols);
- double* rowPtr1 = x;
- const long diagStep = NumRows+1;
- long lenRowLeft = NumCols;
- for ( ; numIters>1; numIters-- ) {
- // Find row with most non-zero entry.
- double* rowPtr2 = rowPtr1;
- double maxAbs = fabs(*rowPtr1);
- double *rowPivot = rowPtr1;
- long i;
- for ( i=numIters-1; i>0; i-- ) {
- const double& newMax = *(++rowPivot);
- if ( newMax > maxAbs ) {
- maxAbs = *rowPivot;
- rowPtr2 = rowPivot;
- }
- else if ( -newMax > maxAbs ) {
- maxAbs = -newMax;
- rowPtr2 = rowPivot;
- }
- }
- // Pivot step: Swap the row with highest entry to the current row
- if ( rowPtr1 != rowPtr2 ) {
- double *to = rowPtr1;
- for ( long i=lenRowLeft; i>0; i-- ) {
- double temp = *to;
- *to = *rowPtr2;
- *rowPtr2 = temp;
- to += NumRows;
- rowPtr2 += NumRows;
- }
- }
- // Subtract this row appropriately from all the lower rows (row operation of type 3)
- rowPtr2 = rowPtr1;
- for ( i=numIters-1; i>0; i-- ) {
- rowPtr2++;
- double* to = rowPtr2;
- double* from = rowPtr1;
- assert( *from != 0.0 );
- double alpha = (*to)/(*from);
- *to = 0.0;
- for ( long j=lenRowLeft-1; j>0; j-- ) {
- to += NumRows;
- from += NumRows;
- *to -= (*from)*alpha;
- }
- }
- // Update for next iteration of loop
- rowPtr1 += diagStep;
- lenRowLeft--;
- }
- }
- // Calculate the c=cosine and s=sine values for a Givens transformation.
- // The matrix M = ( (c, -s), (s, c) ) in row order transforms the
- // column vector (a, b)^T to have y-coordinate zero.
- void MatrixRmn::CalcGivensValues( double a, double b, double *c, double *s )
- {
- double denomInv = sqrt(a*a + b*b);
- if ( denomInv==0.0 ) {
- *c = 1.0;
- *s = 0.0;
- }
- else {
- denomInv = 1.0/denomInv;
- *c = a*denomInv;
- *s = -b*denomInv;
- }
- }
- // Applies Givens transform to columns i and i+1.
- // Equivalent to postmultiplying by the matrix
- // ( c -s )
- // ( s c )
- // with non-zero entries in rows i and i+1 and columns i and i+1
- void MatrixRmn::PostApplyGivens( double c, double s, long idx )
- {
- assert ( 0<=idx && idx<NumCols );
- double *colA = x + idx*NumRows;
- double *colB = colA + NumRows;
- for ( long i = NumRows; i>0; i-- ) {
- double temp = *colA;
- *colA = (*colA)*c + (*colB)*s;
- *colB = (*colB)*c - temp*s;
- colA++;
- colB++;
- }
- }
- // Applies Givens transform to columns idx1 and idx2.
- // Equivalent to postmultiplying by the matrix
- // ( c -s )
- // ( s c )
- // with non-zero entries in rows idx1 and idx2 and columns idx1 and idx2
- void MatrixRmn::PostApplyGivens( double c, double s, long idx1, long idx2 )
- {
- assert ( idx1!=idx2 && 0<=idx1 && idx1<NumCols && 0<=idx2 && idx2<NumCols );
- double *colA = x + idx1*NumRows;
- double *colB = x + idx2*NumRows;
- for ( long i = NumRows; i>0; i-- ) {
- double temp = *colA;
- *colA = (*colA)*c + (*colB)*s;
- *colB = (*colB)*c - temp*s;
- colA++;
- colB++;
- }
- }
- // ********************************************************************************************
- // Singular value decomposition.
- // Return othogonal matrices U and V and diagonal matrix with diagonal w such that
- // (this) = U * Diag(w) * V^T (V^T is V-transpose.)
- // Diagonal entries have all non-zero entries before all zero entries, but are not
- // necessarily sorted. (Someday, I will write ComputedSortedSVD that handles
- // sorting the eigenvalues by magnitude.)
- // ********************************************************************************************
- void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
- {
- assert ( U.NumRows==NumRows && V.NumCols==NumCols
- && U.NumRows==U.NumCols && V.NumRows==V.NumCols
- && w.GetLength()==Min(NumRows,NumCols) );
- double temp=0.0;
- VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 ); // Some extra work space. Will get passed around.
- // Choose larger of U, V to hold intermediate results
- // If U is larger than V, use U to store intermediate results
- // Otherwise use V. In the latter case, we form the SVD of A transpose,
- // (which is essentially identical to the SVD of A).
- MatrixRmn* leftMatrix;
- MatrixRmn* rightMatrix;
- if ( NumRows >= NumCols ) {
- U.LoadAsSubmatrix( *this ); // Copy A into U
- leftMatrix = &U;
- rightMatrix = &V;
- }
- else {
- V.LoadAsSubmatrixTranspose( *this ); // Copy A-transpose into V
- leftMatrix = &V;
- rightMatrix = &U;
- }
- // Do the actual work to calculate the SVD
- // Now matrix has at least as many rows as columns
- CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag );
- ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag );
- }
- void MatrixRmn::ComputeInverse( MatrixRmn& R) const
- {
- assert ( this->NumRows==this->NumCols );
- MatrixRmn U(this->NumRows, this->NumCols);
- VectorRn w(this->NumRows);
- MatrixRmn V(this->NumRows, this->NumCols);
-
- this->ComputeSVD(U, w, V);
-
- assert(this->DebugCheckSVD(U, w , V));
-
- double PseudoInverseThresholdFactor = 0.01;
- double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs();
-
- MatrixRmn VD(this->NumRows, this->NumCols);
- MatrixRmn D(this->NumRows, this->NumCols);
- D.SetZero();
- long diagLength = w.GetLength();
- double* wPtr = w.GetPtr();
- for ( long i = 0; i < diagLength; ++i ) {
- double alpha = *(wPtr++);
- if ( fabs(alpha)>pseudoInverseThreshold ) {
- D.Set(i, i, 1.0/alpha);
- }
- }
-
- Multiply(V,D,VD);
- MultiplyTranspose(VD,U,R);
- }
- // ************************************************ CalcBidiagonal **************************
- // Helper routine for SVD computation
- // U is a matrix to be bidiagonalized.
- // On return, U and V are orthonormal and w holds the new diagonal
- // elements and superDiag holds the super diagonal elements.
-
- void MatrixRmn::CalcBidiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag )
- {
- assert ( U.NumRows>=V.NumRows );
- // The diagonal and superdiagonal entries of the bidiagonalized
- // version of the U matrix
- // are stored in the vectors w and superDiag (temporarily).
- // Apply Householder transformations to U.
- // Householder transformations come in pairs.
- // First, on the left, we map a portion of a column to zeros
- // Second, on the right, we map a portion of a row to zeros
- const long rowStep = U.NumCols;
- const long diagStep = U.NumCols+1;
- double *diagPtr = U.x;
- double* wPtr = w.x;
- double* superDiagPtr = superDiag.x;
- long colLengthLeft = U.NumRows;
- long rowLengthLeft = V.NumCols;
- while (true) {
- // Apply a Householder xform on left to zero part of a column
- SvdHouseholder( diagPtr, colLengthLeft, rowLengthLeft, 1, rowStep, wPtr );
- if ( rowLengthLeft==2 ) {
- *superDiagPtr = *(diagPtr+rowStep);
- break;
- }
- // Apply a Householder xform on the right to zero part of a row
- SvdHouseholder( diagPtr+rowStep, rowLengthLeft-1, colLengthLeft, rowStep, 1, superDiagPtr );
- rowLengthLeft--;
- colLengthLeft--;
- diagPtr += diagStep;
- wPtr++;
- superDiagPtr++;
- }
- int extra = 0;
- diagPtr += diagStep;
- wPtr++;
- if ( colLengthLeft > 2 ) {
- extra = 1;
- // Do one last Householder transformation when the matrix is not square
- colLengthLeft--;
- SvdHouseholder( diagPtr, colLengthLeft, 1, 1, 0, wPtr );
- }
- else {
- *wPtr = *diagPtr;
- }
- // Form U and V from the Householder transformations
- V.ExpandHouseholders( V.NumCols-2, 1, U.x+U.NumRows, U.NumRows, 1 );
- U.ExpandHouseholders( V.NumCols-1+extra, 0, U.x, 1, U.NumRows );
- // Done with bidiagonalization
- return;
- }
- // Helper routine for CalcBidiagonal
- // Performs a series of Householder transformations on a matrix
- // Stores results compactly into the matrix: The Householder vector u (normalized)
- // is stored into the first row/column being transformed.
- // The leading term of that row (= plus/minus its magnitude is returned
- // separately into "retFirstEntry"
- void MatrixRmn::SvdHouseholder( double* basePt,
- long colLength, long numCols, long colStride, long rowStride,
- double* retFirstEntry )
- {
- // Calc norm of vector u
- double* cPtr = basePt;
- double norm = 0.0;
- long i;
- for ( i=colLength; i>0 ; i-- ) {
- norm += Square( *cPtr );
- cPtr += colStride;
- }
- norm = sqrt(norm); // Norm of vector to reflect to axis e_1
- // Handle sign issues
- double imageVal; // Choose sign to maximize distance
- if ( (*basePt) < 0.0 ) {
- imageVal = norm;
- norm = 2.0*norm*(norm-(*basePt));
- }
- else {
- imageVal = -norm;
- norm = 2.0*norm*(norm+(*basePt));
- }
- norm = sqrt(norm); // Norm is norm of reflection vector
- if ( norm==0.0 ) { // If the vector being transformed is equal to zero
- // Force to zero in case of roundoff errors
- cPtr = basePt;
- for ( i=colLength; i>0; i-- ) {
- *cPtr = 0.0;
- cPtr += colStride;
- }
- *retFirstEntry = 0.0;
- return;
- }
- *retFirstEntry = imageVal;
- // Set up the normalized Householder vector
- *basePt -= imageVal; // First component changes. Rest stay the same.
- // Normalize the vector
- norm = 1.0/norm; // Now it is the inverse norm
- cPtr = basePt;
- for ( i=colLength; i>0 ; i-- ) {
- *cPtr *= norm;
- cPtr += colStride;
- }
- // Transform the rest of the U matrix with the Householder transformation
- double *rPtr = basePt;
- for ( long j=numCols-1; j>0; j-- ) {
- rPtr += rowStride;
- // Calc dot product with Householder transformation vector
- double dotP = DotArray( colLength, basePt, colStride, rPtr, colStride );
- // Transform with I - 2*dotP*(Householder vector)
- AddArrayScale( colLength, basePt, colStride, rPtr, colStride, -2.0*dotP );
- }
- }
- // ********************************* ExpandHouseholders ********************************************
- // The matrix will be square.
- // numXforms = number of Householder transformations to concatenate
- // Each Householder transformation is represented by a unit vector
- // Each successive Householder transformation starts one position later
- // and has one more implied leading zero
- // basePt = beginning of the first Householder transform
- // colStride, rowStride: Householder xforms are stored in "columns"
- // numZerosSkipped is the number of implicit zeros on the front each
- // Householder transformation vector (only values supported are 0 and 1).
- void MatrixRmn::ExpandHouseholders( long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride )
- {
- // Number of applications of the last Householder transform
- // (That are not trivial!)
- long numToTransform = NumCols-numXforms+1-numZerosSkipped;
- assert( numToTransform>0 );
- if ( numXforms==0 ) {
- SetIdentity();
- return;
- }
- // Handle the first one separately as a special case,
- // "this" matrix will be treated to simulate being preloaded with the identity
- long hDiagStride = rowStride+colStride;
- const double* hBase = basePt + hDiagStride*(numXforms-1); // Pointer to the last Householder vector
- const double* hDiagPtr = hBase + colStride*(numToTransform-1); // Pointer to last entry in that vector
- long i;
- double* diagPtr = x+NumCols*NumRows-1; // Last entry in matrix (points to diagonal entry)
- double* colPtr = diagPtr-(numToTransform-1); // Pointer to column in matrix
- for ( i=numToTransform; i>0; i-- ) {
- CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hDiagPtr) );
- *diagPtr += 1.0; // Add back in 1 to the diagonal entry (since xforming the identity)
- diagPtr -= (NumRows+1); // Next diagonal entry in this matrix
- colPtr -= NumRows; // Next column in this matrix
- hDiagPtr -= colStride;
- }
- // Now handle the general case
- // A row of zeros must be in effect added to the top of each old column (in each loop)
- double* colLastPtr = x + NumRows*NumCols - numToTransform - 1;
- for ( i = numXforms-1; i>0; i-- ) {
- numToTransform++; // Number of non-trivial applications of this Householder transformation
- hBase -= hDiagStride; // Pointer to the beginning of the Householder transformation
- colPtr = colLastPtr;
- for ( long j = numToTransform-1; j>0; j-- ) {
- // Get dot product
- double dotProd2N = -2.0*DotArray( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1 );
- *colPtr = dotProd2N*(*hBase); // Adding onto zero at initial point
- AddArrayScale( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1, dotProd2N );
- colPtr -= NumRows;
- }
- // Do last one as a special case (may overwrite the Householder vector)
- CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hBase) );
- *colPtr += 1.0; // Add back one one as identity
- // Done with this Householder transformation
- colLastPtr --;
- }
- if ( numZerosSkipped!=0 ) {
- assert( numZerosSkipped==1 );
- // Fill first row and column with identity (More generally: first numZerosSkipped many rows and columns)
- double* d = x;
- *d = 1;
- double* d2 = d;
- for ( i=NumRows-1; i>0; i-- ) {
- *(++d) = 0;
- *(d2+=NumRows) = 0;
- }
- }
- }
- // **************** ConvertBidiagToDiagonal ***********************************************
- // Do the iterative transformation from bidiagonal form to diagonal form using
- // Givens transformation. (Golub-Reinsch)
- // U and V are square. Size of U less than or equal to that of U.
- void MatrixRmn::ConvertBidiagToDiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) const
- {
- // These two index into the last bidiagonal block (last in the matrix, it will be
- // first one handled.
- long lastBidiagIdx = V.NumRows-1;
- long firstBidiagIdx = 0;
- double eps = 1.0e-15 * Max(w.MaxAbs(), superDiag.MaxAbs());
- while ( true ) {
- bool workLeft = UpdateBidiagIndices( &firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps );
- if ( !workLeft ) {
- break;
- }
- // Get ready for first Givens rotation
- // Push non-zero to M[2,1] with Givens transformation
- double* wPtr = w.x+firstBidiagIdx;
- double* sdPtr = superDiag.x+firstBidiagIdx;
- double extraOffDiag=0.0;
- if ( (*wPtr)==0.0 ) {
- ClearRowWithDiagonalZero( firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr, eps );
- if ( firstBidiagIdx>0 ) {
- if ( NearZero( *(--sdPtr), eps ) ) {
- *sdPtr = 0.0;
- }
- else {
- ClearColumnWithDiagonalZero( firstBidiagIdx, V, wPtr, sdPtr, eps );
- }
- }
- continue;
- }
- // Estimate an eigenvalue from bottom four entries of M
- // This gives a lambda value which will shift the Givens rotations
- // Last four entries of M^T * M are ( ( A, B ), ( B, C ) ).
- double A;
- A = (firstBidiagIdx<lastBidiagIdx-1) ? Square(superDiag[lastBidiagIdx-2]): 0.0;
- double BSq = Square(w[lastBidiagIdx-1]);
- A += BSq; // The "A" entry of M^T * M
- double C = Square(superDiag[lastBidiagIdx-1]);
- BSq *= C; // The squared "B" entry
- C += Square(w[lastBidiagIdx]); // The "C" entry
- double lambda; // lambda will hold the estimated eigenvalue
- lambda = sqrt( Square((A-C)*0.5) + BSq ); // Use the lambda value that is closest to C.
- if ( A > C ) {
- lambda = -lambda;
- }
- lambda += (A+C)*0.5; // Now lambda equals the estimate for the last eigenvalue
- double t11 = Square(w[firstBidiagIdx]);
- double t12 = w[firstBidiagIdx]*superDiag[firstBidiagIdx];
- double c, s;
- CalcGivensValues( t11-lambda, t12, &c, &s );
- ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, wPtr+1 );
- V.PostApplyGivens( c, -s, firstBidiagIdx );
- long i;
- for ( i=firstBidiagIdx; i<lastBidiagIdx-1; i++ ) {
- // Push non-zero from M[i+1,i] to M[i,i+2]
- CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
- ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr+1, sdPtr+1 );
- U.PostApplyGivens( c, -s, i );
- // Push non-zero from M[i,i+2] to M[1+2,i+1]
- CalcGivensValues( *sdPtr, extraOffDiag, &c, &s );
- ApplyGivensCBTD( c, s, sdPtr, wPtr+1, &extraOffDiag, extraOffDiag, sdPtr+1, wPtr+2 );
- V.PostApplyGivens( c, -s, i+1 );
- wPtr++;
- sdPtr++;
- }
- // Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1
- CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
- ApplyGivensCBTD( c, s, wPtr, &extraOffDiag, sdPtr, wPtr+1 );
- U.PostApplyGivens( c, -s, i );
- // DEBUG
- // DebugCalcBidiagCheck( V, w, superDiag, U );
- }
- }
- // This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry on the same row.
- // We use Givens rotations to "chase" the non-zero entry across the row; when it reaches the last
- // column, it is finally zeroed away.
- // wPtr points to the zero entry on the diagonal. sdPtr points to the non-zero superdiagonal entry on the same row.
- void MatrixRmn::ClearRowWithDiagonalZero( long firstBidiagIdx, long lastBidiagIdx, MatrixRmn& U, double *wPtr, double *sdPtr, double eps )
- {
- double curSd = *sdPtr; // Value being chased across the row
- *sdPtr = 0.0;
- long i=firstBidiagIdx+1;
- while (true) {
- // Rotate row i and row firstBidiagIdx (Givens rotation)
- double c, s;
- CalcGivensValues( *(++wPtr), curSd, &c, &s );
- U.PostApplyGivens( c, -s, i, firstBidiagIdx );
- *wPtr = c*(*wPtr) - s*curSd;
- if ( i==lastBidiagIdx ) {
- break;
- }
- curSd = s*(*(++sdPtr)); // New value pops up one column over to the right
- *sdPtr = c*(*sdPtr);
- i++;
- }
- }
- // This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry in the same column.
- // We use Givens rotations to "chase" the non-zero entry up the column; when it reaches the last
- // column, it is finally zeroed away.
- // wPtr points to the zero entry on the diagonal. sdPtr points to the non-zero superdiagonal entry in the same column.
- void MatrixRmn::ClearColumnWithDiagonalZero( long endIdx, MatrixRmn& V, double *wPtr, double *sdPtr, double eps )
- {
- double curSd = *sdPtr; // Value being chased up the column
- *sdPtr = 0.0;
- long i = endIdx-1;
- while ( true ) {
- double c, s;
- CalcGivensValues( *(--wPtr), curSd, &c, &s );
- V.PostApplyGivens( c, -s, i, endIdx );
- *wPtr = c*(*wPtr) - s*curSd;
- if ( i==0 ) {
- break;
- }
- curSd = s*(*(--sdPtr)); // New value pops up one row above
- if ( NearZero( curSd, eps ) ) {
- break;
- }
- *sdPtr = c*(*sdPtr);
- i--;
- }
- }
- // Matrix A is ( ( a c ) ( b d ) ), i.e., given in column order.
- // Mult's G[c,s] times A, replaces A.
- void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, double *d )
- {
- double temp = *a;
- *a = cosine*(*a) - sine*(*b);
- *b = sine*temp + cosine*(*b);
- temp = *c;
- *c = cosine*(*c) - sine*(*d);
- *d = sine*temp + cosine*(*d);
- }
- // Now matrix A given in row order, A = ( ( a b c ) ( d e f ) ).
- // Return G[c,s] * A, replace A. d becomes zero, no need to return.
- // Also, it is certain the old *c value is taken to be zero!
- void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c,
- double d, double *e, double *f )
- {
- *a = cosine*(*a) - sine*d;
- double temp = *b;
- *b = cosine*(*b) - sine*(*e);
- *e = sine*temp + cosine*(*e);
- *c = -sine*(*f);
- *f = cosine*(*f);
- }
- // Helper routine for SVD conversion from bidiagonal to diagonal
- bool MatrixRmn::UpdateBidiagIndices( long *firstBidiagIdx, long *lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps )
- {
- long lastIdx = *lastBidiagIdx;
- double* sdPtr = superDiag.GetPtr( lastIdx-1 ); // Entry above the last diagonal entry
- while ( NearZero(*sdPtr, eps) ) {
- *(sdPtr--) = 0.0;
- lastIdx--;
- if ( lastIdx == 0 ) {
- return false;
- }
- }
- *lastBidiagIdx = lastIdx;
- long firstIdx = lastIdx-1;
- double* wPtr = w.GetPtr( firstIdx );
- while ( firstIdx > 0 ) {
- if ( NearZero( *wPtr, eps ) ) { // If this diagonal entry (near) zero
- *wPtr = 0.0;
- break;
- }
- if ( NearZero(*(--sdPtr), eps) ) { // If the entry above the diagonal entry is (near) zero
- *sdPtr = 0.0;
- break;
- }
- wPtr--;
- firstIdx--;
- }
- *firstBidiagIdx = firstIdx;
- return true;
- }
- // ******************************************DEBUG STUFFF
- bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const
- {
- // Special SVD test code
- MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
- IV.SetIdentity();
- MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
- MatrixRmn::TransposeMultiply( V, V, VTV );
- IV -= VTV;
- double error = IV.FrobeniusNorm();
- MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
- IU.SetIdentity();
- MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
- MatrixRmn::TransposeMultiply( U, U, UTU );
- IU -= UTU;
- error += IU.FrobeniusNorm();
- MatrixRmn Diag( U.GetNumRows(), V.GetNumRows() );
- Diag.SetZero();
- Diag.SetDiagonalEntries( w );
- MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
- MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
- MatrixRmn::Multiply( U, Diag, B );
- MatrixRmn::MultiplyTranspose( B, V, C );
- C -= *this;
- error += C.FrobeniusNorm();
- bool ret = ( fabs(error)<=1.0e-13*w.MaxAbs() );
- assert ( ret );
- return ret;
- }
- bool MatrixRmn::DebugCheckInverse( const MatrixRmn& MInv ) const
- {
- assert ( this->NumRows==this->NumCols );
- assert ( MInv.NumRows==MInv.NumCols );
- MatrixRmn I(this->NumRows, this->NumCols);
- I.SetIdentity();
- MatrixRmn MMInv(this->NumRows, this->NumCols);
- Multiply(*this, MInv, MMInv);
- I -= MMInv;
- double error = I.FrobeniusNorm();
- bool ret = ( fabs(error)<=1.0e-13 );
- assert ( ret );
- return ret;
- }
- bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
- {
- // Special SVD test code
- MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
- IV.SetIdentity();
- MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
- MatrixRmn::TransposeMultiply( V, V, VTV );
- IV -= VTV;
- double error = IV.FrobeniusNorm();
- MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
- IU.SetIdentity();
- MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
- MatrixRmn::TransposeMultiply( U, U, UTU );
- IU -= UTU;
- error += IU.FrobeniusNorm();
- MatrixRmn DiagAndSuper( U.GetNumRows(), V.GetNumRows() );
- DiagAndSuper.SetZero();
- DiagAndSuper.SetDiagonalEntries( w );
- if ( this->GetNumRows()>=this->GetNumColumns() ) {
- DiagAndSuper.SetSequence( superDiag, 0, 1, 1, 1 );
- }
- else {
- DiagAndSuper.SetSequence( superDiag, 1, 0, 1, 1 );
- }
- MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
- MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
- MatrixRmn::Multiply( U, DiagAndSuper, B );
- MatrixRmn::MultiplyTranspose( B, V, C );
- C -= *this;
- error += C.FrobeniusNorm();
- bool ret = ( fabs(error)<1.0e-13*Max(w.MaxAbs(),superDiag.MaxAbs()) );
- assert ( ret );
- return ret;
- }
|