Working on breaking up the steps into manageable chunks
This commit is contained in:
+522
-177
@@ -113,7 +113,7 @@ void SVD::ApplyHouseholderRight(Matrix<5, 5> &W, const float *v,
|
||||
}
|
||||
}
|
||||
|
||||
void SVD::ComputeGivens(float x, float y, float &c, float &s) {
|
||||
[[gnu::unused]] void SVD::ComputeGivens(float x, float y, float &c, float &s) {
|
||||
float r = sqrtf(x * x + y * y);
|
||||
|
||||
if (r < 1e-30f) {
|
||||
@@ -126,7 +126,7 @@ void SVD::ComputeGivens(float x, float y, float &c, float &s) {
|
||||
s = y / r;
|
||||
}
|
||||
|
||||
void SVD::ApplyGivensLeft(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
|
||||
[[gnu::unused]] void SVD::ApplyGivensLeft(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
|
||||
float s, uint8_t startCol, uint8_t endCol) {
|
||||
// [c s] [row_i] = [new_row_i]
|
||||
// [-s c] [row_j] [new_row_j]
|
||||
@@ -138,7 +138,7 @@ void SVD::ApplyGivensLeft(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
|
||||
}
|
||||
}
|
||||
|
||||
void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
|
||||
[[gnu::unused]] void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
|
||||
float s, uint8_t startRow, uint8_t endRow) {
|
||||
// [col_i col_j] · [c -s] = [new_col_i new_col_j]
|
||||
// [s c]
|
||||
@@ -150,6 +150,411 @@ void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Phase 1: Householder Bidiagonalization
|
||||
// ============================================================================
|
||||
|
||||
void SVD::Bidiagonalize(Matrix<5, 5> &W,
|
||||
uint8_t m, uint8_t q, uint8_t p,
|
||||
Matrix<5, 5> &QL,
|
||||
Matrix<5, 5> &QR) {
|
||||
// Working matrix W is m×q (padded to 5×5).
|
||||
// QL and QR are initialized to identity by the caller.
|
||||
// We reduce W to upper bidiagonal form B using Householder reflections.
|
||||
|
||||
float hhVec[5]; // Householder vector storage
|
||||
|
||||
for (uint8_t k = 0; k < p; k++) {
|
||||
// --- Left Householder on column k, rows k..m-1 ---
|
||||
// Zero out subdiagonal elements below B[k+1][k]
|
||||
{
|
||||
uint8_t len = m - k;
|
||||
if (len <= 1)
|
||||
continue;
|
||||
|
||||
// Extract the column segment W[k..k+len-1][k]
|
||||
float x[5];
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
x[i] = W[k + i][k];
|
||||
}
|
||||
|
||||
// Compute Householder reflector
|
||||
float alpha;
|
||||
SVD::ComputeHouseholder(x, len, hhVec, alpha);
|
||||
|
||||
if (alpha == 0.0f)
|
||||
continue;
|
||||
|
||||
// Apply H from left to W: W = H·W (columns k..q-1)
|
||||
SVD::ApplyHouseholderLeft(W, hhVec, k, k + len - 1);
|
||||
|
||||
// Apply H from right to QL: QL = QL · H
|
||||
SVD::ApplyHouseholderRight(QL, hhVec, k, k + len - 1);
|
||||
}
|
||||
|
||||
// --- Right Householder on row k, columns k+1..q-1 ---
|
||||
// Zero out elements above the first superdiagonal in row k.
|
||||
// The Householder maps [W[k][k+1], ..., W[k][q-1]] to [gamma, 0, ..., 0],
|
||||
// preserving the first superdiagonal element (now gamma) and zeroing the rest.
|
||||
{
|
||||
int len = static_cast<int>(q) - 1 - k;
|
||||
if (len <= 1)
|
||||
continue; // Need at least 2 elements to zero something out
|
||||
|
||||
// Extract the row segment starting from column k+1
|
||||
float x[5];
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
x[i] = W[k][k + 1 + i];
|
||||
}
|
||||
|
||||
// Compute Householder reflector
|
||||
float alpha;
|
||||
SVD::ComputeHouseholder(x, len, hhVec, alpha);
|
||||
|
||||
if (alpha == 0.0f)
|
||||
continue;
|
||||
|
||||
// Apply H from right to W: W = W·H (columns k+1..k+len-1)
|
||||
SVD::ApplyHouseholderRight(W, hhVec, k + 1, k + len);
|
||||
|
||||
// Apply H from right to QR: QR = QR · H
|
||||
SVD::ApplyHouseholderRight(QR, hhVec, k + 1, k + len);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Phase 2 helpers: block solving of the bidiagonal matrix
|
||||
// ============================================================================
|
||||
|
||||
void SVD::DeflateBidiagonal(Matrix<5, 5> &W, uint8_t p, float tol) {
|
||||
// Zero out superdiagonal elements that are negligible relative to the
|
||||
// local diagonal scale. This deflates the bidiagonal matrix into
|
||||
// independent unreduced blocks, each of which can be solved on its own.
|
||||
if (p < 2)
|
||||
return;
|
||||
for (uint8_t i = 0; i < p - 1; i++) {
|
||||
float test = fabsf(W[i][i + 1]);
|
||||
float scale = fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]);
|
||||
// Use absolute threshold for small scales to avoid division issues
|
||||
if (test < tol * fmaxf(scale, 1e-10f)) {
|
||||
W[i][i + 1] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool SVD::BidiagonalIsDiagonal(const Matrix<5, 5> &W, uint8_t p, float tol) {
|
||||
// True when every superdiagonal element of the p×p bidiagonal matrix
|
||||
// has been reduced to (numerically) zero, i.e. the diagonal holds the
|
||||
// singular values and no unreduced blocks remain.
|
||||
if (p < 2)
|
||||
return true;
|
||||
for (uint8_t i = 0; i < p - 1; i++) {
|
||||
if (fabsf(W.Get(i, i + 1)) > tol * 1e-30f) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void SVD::SolveBidiagonalBlock2x2(float a, float b, float d, float Ublock[2][2],
|
||||
float Vblock[2][2], float sigma[2]) {
|
||||
// Full SVD of the 2×2 upper-bidiagonal block B = [[a, b], [0, d]]:
|
||||
// B = Ublock · diag(sigma[0], sigma[1]) · Vblockᵀ
|
||||
// where:
|
||||
// - sigma[0] ≥ sigma[1] ≥ 0
|
||||
// - columns of Ublock are the left singular vectors
|
||||
// - columns of Vblock are the right singular vectors (Vblock = scipy Vᵀᵀ)
|
||||
//
|
||||
// Uses eigen-decomposition of BᵀB = [[a², ab], [ab, b²+d²]] (symmetric
|
||||
// 2×2, closed form), then uᵢ = B·vᵢ/σᵢ.
|
||||
|
||||
// Singular values = sqrt of eigenvalues of BᵀB (trace/det closed form)
|
||||
float trace = a * a + b * b + d * d;
|
||||
float det = a * a * d * d;
|
||||
float disc = trace * trace - 4.0f * det;
|
||||
if (disc < 0)
|
||||
disc = 0;
|
||||
float sqrtDisc = sqrtf(disc);
|
||||
float hi = sqrtf((trace + sqrtDisc) / 2.0f);
|
||||
float lo = sqrtf((trace - sqrtDisc) / 2.0f);
|
||||
if (lo > hi) {
|
||||
float tmp = hi;
|
||||
hi = lo;
|
||||
lo = tmp;
|
||||
}
|
||||
sigma[0] = hi;
|
||||
sigma[1] = lo;
|
||||
|
||||
// Right singular vector v1: eigenvector of BᵀB for λ1 = hi².
|
||||
// Null-space vector of (BᵀB − λ1·I) is [ab, λ1 − a²].
|
||||
float a2 = a * a;
|
||||
float ab_val = a * b;
|
||||
float e1x = ab_val;
|
||||
float e1y = hi * hi - a2;
|
||||
float normE1 = sqrtf(e1x * e1x + e1y * e1y);
|
||||
float v1x, v1y;
|
||||
if (normE1 > 1e-30f) {
|
||||
v1x = e1x / normE1;
|
||||
v1y = e1y / normE1;
|
||||
} else {
|
||||
// Degenerate (e.g. b = 0 and |a| ≥ |d|): e₁ is already an eigenvector
|
||||
v1x = 1.0f;
|
||||
v1y = 0.0f;
|
||||
}
|
||||
|
||||
// v2 is the unit vector orthogonal to v1 (completes the 2D basis)
|
||||
float v2x = -v1y;
|
||||
float v2y = v1x;
|
||||
|
||||
// Vblock columns = right singular vectors
|
||||
Vblock[0][0] = v1x;
|
||||
Vblock[1][0] = v1y;
|
||||
Vblock[0][1] = v2x;
|
||||
Vblock[1][1] = v2y;
|
||||
|
||||
// Ublock columns: uᵢ = B·vᵢ / σᵢ, with a rank-deficiency guard.
|
||||
// When σᵢ ≈ 0, dividing produces inf/NaN; instead fill the U column with
|
||||
// the signed orthogonal complement of the other U column (keeps Ublock
|
||||
// orthogonal, and B·vᵢ ≈ 0 so any unit complement satisfies the SVD).
|
||||
float u1x, u1y, u2x, u2y;
|
||||
if (hi > 1e-30f) {
|
||||
u1x = (a * v1x + b * v1y) / hi;
|
||||
u1y = d * v1y / hi;
|
||||
} else {
|
||||
u1x = 1.0f;
|
||||
u1y = 0.0f;
|
||||
}
|
||||
if (lo > 1e-30f) {
|
||||
u2x = (a * v2x + b * v2y) / lo;
|
||||
u2y = d * v2y / lo;
|
||||
} else {
|
||||
u2x = -u1y;
|
||||
u2y = u1x;
|
||||
}
|
||||
|
||||
Ublock[0][0] = u1x;
|
||||
Ublock[1][0] = u1y;
|
||||
Ublock[0][1] = u2x;
|
||||
Ublock[1][1] = u2y;
|
||||
}
|
||||
|
||||
void SVD::JacobiEigenSymmetric(float T[5][5], uint8_t n, float evals[5],
|
||||
float V[5][5]) {
|
||||
// Cyclic Jacobi eigenvalue algorithm on symmetric n×n matrix T (in place).
|
||||
// On return:
|
||||
// - T is (near-)diagonal; its diagonal entries are the eigenvalues
|
||||
// - evals[i] = T[i][i] (unsorted)
|
||||
// - columns of V are the corresponding eigenvectors (V is accumulated
|
||||
// as V ← V·J so that T·V = V·Λ)
|
||||
float jacTol = 1e-10f;
|
||||
|
||||
// V starts as the identity: eigenvector accumulator
|
||||
for (uint8_t i = 0; i < n; i++)
|
||||
for (uint8_t j = 0; j < n; j++)
|
||||
V[i][j] = (i == j) ? 1.0f : 0.0f;
|
||||
|
||||
for (uint32_t jacIter = 0; jacIter < 100; jacIter++) {
|
||||
// Check convergence over ALL off-diagonal entries, not just the
|
||||
// tridiagonal band: cyclic Jacobi on a 3x3+ block fills non-band
|
||||
// entries (e.g. T[0][2]) during sweeps, so a band-only test can
|
||||
// declare convergence too early.
|
||||
bool converged = true;
|
||||
for (uint8_t i = 0; i < n - 1 && converged; i++) {
|
||||
for (uint8_t j = i + 1; j < n; j++) {
|
||||
float scale = fabsf(T[i][i]) + fabsf(T[j][j]);
|
||||
if (fabsf(T[i][j]) > jacTol * fmaxf(scale, 1e-30f)) {
|
||||
converged = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (converged)
|
||||
break;
|
||||
|
||||
// Cyclic Jacobi: zero out T[p][q] for p < q
|
||||
for (uint8_t p = 0; p < n - 1; p++) {
|
||||
for (uint8_t q = p + 1; q < n; q++) {
|
||||
float tPQ = T[p][q];
|
||||
if (fabsf(tPQ) < jacTol * 1e-30f)
|
||||
continue;
|
||||
|
||||
float tPP = T[p][p];
|
||||
float tQQ = T[q][q];
|
||||
float theta = (tQQ - tPP) / (2.0f * tPQ);
|
||||
float t;
|
||||
if (theta >= 0.0f)
|
||||
t = 1.0f / (theta + sqrtf(1.0f + theta * theta));
|
||||
else
|
||||
t = -1.0f / (-theta + sqrtf(1.0f + theta * theta));
|
||||
|
||||
float c = 1.0f / sqrtf(1.0f + t * t);
|
||||
float s = t * c;
|
||||
|
||||
// Update T
|
||||
T[p][p] = tPP - t * tPQ;
|
||||
T[q][q] = tQQ + t * tPQ;
|
||||
T[p][q] = 0.0f;
|
||||
T[q][p] = 0.0f;
|
||||
|
||||
// Update other elements
|
||||
for (uint8_t k = 0; k < n; k++) {
|
||||
if (k == p || k == q)
|
||||
continue;
|
||||
float tPK = T[k][p];
|
||||
float tQK = T[k][q];
|
||||
T[k][p] = c * tPK - s * tQK;
|
||||
T[p][k] = T[k][p];
|
||||
T[k][q] = s * tPK + c * tQK;
|
||||
T[q][k] = T[k][q];
|
||||
}
|
||||
|
||||
// Accumulate eigenvectors
|
||||
for (uint8_t k = 0; k < n; k++) {
|
||||
float vKP = V[k][p];
|
||||
float vKQ = V[k][q];
|
||||
V[k][p] = c * vKP - s * vKQ;
|
||||
V[k][q] = s * vKP + c * vKQ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
evals[i] = fabsf(T[i][i]);
|
||||
}
|
||||
}
|
||||
|
||||
void SVD::ApplyBlockFactorsToAccumulators(uint8_t blockStart, uint8_t blockSize,
|
||||
const float Ublock[5][5],
|
||||
const float Vblock[5][5],
|
||||
uint8_t rowsQL, uint8_t rowsQR,
|
||||
Matrix<5, 5> &QL,
|
||||
Matrix<5, 5> &QR) {
|
||||
// Fold the block SVD factors into the accumulated Householder
|
||||
// transformation matrices:
|
||||
// QL[:, blockStart..blockStart+blockSize-1] ← QL[:, ...] · Ublock
|
||||
// (over rows 0..rowsQL−1)
|
||||
// QR[:, blockStart..blockStart+blockSize-1] ← QR[:, ...] · Vblock
|
||||
// (over rows 0..rowsQR−1)
|
||||
//
|
||||
// rowsQL / rowsQR are the meaningful row extents of the accumulators:
|
||||
// for a transposed (wide) problem W = Aᵀ has n rows, so QL carries n
|
||||
// meaningful rows while in the normal case it carries m.
|
||||
|
||||
for (uint8_t j = 0; j < rowsQL; j++) {
|
||||
for (uint8_t i = 0; i < blockSize; i++) {
|
||||
float sum = 0.0f;
|
||||
for (uint8_t k = 0; k < blockSize; k++) {
|
||||
sum += QL[j][blockStart + k] * Ublock[k][i];
|
||||
}
|
||||
QL[j][blockStart + i] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t j = 0; j < rowsQR; j++) {
|
||||
for (uint8_t i = 0; i < blockSize; i++) {
|
||||
float sum = 0.0f;
|
||||
for (uint8_t k = 0; k < blockSize; k++) {
|
||||
sum += QR[j][blockStart + k] * Vblock[k][i];
|
||||
}
|
||||
QR[j][blockStart + i] = sum;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SVD::SolveBidiagonalBlockJacobi(Matrix<5, 5> &W, uint8_t blockStart,
|
||||
uint8_t blockSize, uint8_t rowsQL,
|
||||
uint8_t rowsQR, Matrix<5, 5> &QL,
|
||||
Matrix<5, 5> &QR, float tol) {
|
||||
// Full SVD of an unreduced upper-bidiagonal block of size > 2 via
|
||||
// eigen-decomposition of the tridiagonal T = BᵀB:
|
||||
// 1. Snapshot the ORIGINAL block diagonal/superdiagonal from W
|
||||
// 2. Form T = BᵀB (tridiagonal symmetric)
|
||||
// 3. JacobiEigenSymmetric → eigenvalues + eigenvector matrix V
|
||||
// 4. Sort eigenvalues descending, reordering V
|
||||
// 5. Ublock = B_orig · V · Σ⁻¹ (computed from the SNAPSHOT so that
|
||||
// overwriting W's diagonal does not corrupt it)
|
||||
// 6. Fold Ublock/Vblock into QL/QR via ApplyBlockFactorsToAccumulators
|
||||
// 7. Only now write sqrt(eigenvalues) into W's diagonal and zero the
|
||||
// block's superdiagonals
|
||||
(void)tol; // Jacobi convergence tolerance is internal
|
||||
|
||||
// Step 1: snapshot original block values (diagonal d[i], superdiag e[i])
|
||||
float d[5], e[4];
|
||||
for (uint8_t i = 0; i < blockSize; i++) {
|
||||
d[i] = W[blockStart + i][blockStart + i];
|
||||
}
|
||||
for (uint8_t i = 0; i < blockSize - 1; i++) {
|
||||
e[i] = W[blockStart + i][blockStart + i + 1];
|
||||
}
|
||||
|
||||
// Step 2: form T = BᵀB (tridiagonal)
|
||||
// T[i][i] = d[i]² + e[i−1]² (e[−1] = 0)
|
||||
// T[i][i+1] = d[i] · e[i]
|
||||
float T[5][5] = {{0}};
|
||||
for (uint8_t i = 0; i < blockSize; i++) {
|
||||
float diag = d[i] * d[i];
|
||||
if (i > 0) {
|
||||
diag += e[i - 1] * e[i - 1];
|
||||
}
|
||||
T[i][i] = diag;
|
||||
if (i < blockSize - 1) {
|
||||
float off = d[i] * e[i];
|
||||
T[i][i + 1] = off;
|
||||
T[i + 1][i] = off;
|
||||
}
|
||||
}
|
||||
|
||||
// Step 3: Jacobi eigenvalue algorithm
|
||||
float evals[5] = {0};
|
||||
float V[5][5] = {{0}};
|
||||
SVD::JacobiEigenSymmetric(T, blockSize, evals, V);
|
||||
|
||||
// Step 4: sort eigenvalues descending, reordering eigenvector columns
|
||||
for (uint8_t i = 0; i < blockSize - 1; i++) {
|
||||
for (uint8_t j = i + 1; j < blockSize; j++) {
|
||||
if (evals[j] > evals[i]) {
|
||||
float tmpE = evals[i];
|
||||
evals[i] = evals[j];
|
||||
evals[j] = tmpE;
|
||||
for (uint8_t k = 0; k < blockSize; k++) {
|
||||
float tmpV = V[k][i];
|
||||
V[k][i] = V[k][j];
|
||||
V[k][j] = tmpV;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Step 5: Ublock = B_orig · V · Σ⁻¹, from the SNAPSHOT values.
|
||||
// Column i of Ublock is u_i = (B_orig · v_i) / σ_i.
|
||||
float Ublock[5][5] = {{0}};
|
||||
for (uint8_t i = 0; i < blockSize; i++) {
|
||||
float sigmaI = sqrtf(evals[i]);
|
||||
for (uint8_t r = 0; r < blockSize; r++) {
|
||||
float result = d[r] * V[r][i];
|
||||
if (r + 1 < blockSize) {
|
||||
result += e[r] * V[r + 1][i];
|
||||
}
|
||||
Ublock[r][i] = (sigmaI > 1e-30f) ? result / sigmaI : 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// Step 6: fold the factors into the accumulators
|
||||
SVD::ApplyBlockFactorsToAccumulators(blockStart, blockSize, Ublock, V,
|
||||
rowsQL, rowsQR, QL, QR);
|
||||
|
||||
// Step 7: W last — write singular values onto the diagonal and zero
|
||||
// the block's superdiagonals
|
||||
for (uint8_t i = 0; i < blockSize; i++) {
|
||||
W[blockStart + i][blockStart + i] = sqrtf(evals[i]);
|
||||
if (i < blockSize - 1) {
|
||||
W[blockStart + i][blockStart + i + 1] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Phase 3: Extract and Sort Singular Values
|
||||
// ============================================================================
|
||||
@@ -159,8 +564,16 @@ void SVD::ExtractAndSortSingularValues(Matrix<5, 5> &W,
|
||||
uint8_t p,
|
||||
Matrix<5, 5> &QL,
|
||||
Matrix<5, 5> &QR) {
|
||||
// Extract singular values as absolute values of diagonal elements
|
||||
// Extract singular values as absolute values of diagonal elements.
|
||||
// If a diagonal element is negative, flip the sign of the corresponding
|
||||
// column in QL to maintain U * Sigma * Vt = A.
|
||||
for (uint8_t i = 0; i < p; i++) {
|
||||
if (W[i][i] < 0.0f) {
|
||||
// Flip sign of column i in QL
|
||||
for (uint8_t k = 0; k < 5; k++) {
|
||||
QL[k][i] = -QL[k][i];
|
||||
}
|
||||
}
|
||||
sigma[i][0] = fabsf(W[i][i]);
|
||||
}
|
||||
|
||||
@@ -209,19 +622,28 @@ void SVD::AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
|
||||
}
|
||||
|
||||
// ---- Compute Final U and Vt ----
|
||||
// If transposeNeeded (wide matrix), we computed SVD(Aᵀ) = Ũ·Σ·Ṽᵀ
|
||||
// Then U = Ṽ (= QR[:,0:p]) and Vt = Ũᵀ (= QL[:,0:p]ᵀ)
|
||||
// Otherwise, SVD(A) = QL[:,0:p] · Σ · (QR[:,0:p])ᵀ
|
||||
// So U = QL[:,0:p] and Vt = QR[:,0:p]ᵀ
|
||||
// After bidiagonalization, A = QL · W · QRᵀ (QL = product of left
|
||||
// Householders in application order, QR = product of right Householders),
|
||||
// and the block solvers fold the block SVD factors in: QL <- QL·U_block,
|
||||
// QR <- QR·V_block. Hence:
|
||||
//
|
||||
// Non-transposed (m ≥ n): A = (QL)·Σ·(QR)ᵀ
|
||||
// U = QL (first m rows, first p columns)
|
||||
// Vt = QRᵀ (first p rows of the n×n matrix)
|
||||
//
|
||||
// Transposed (wide, m < n): we computed SVD of Aᵀ = QL·W·QRᵀ, so
|
||||
// A = (QR)·Σ·(QL)ᵀ
|
||||
// U = QR (first m rows, first p columns) — NOT QRᵀ
|
||||
// Vt = QLᵀ (the FULL n×n transpose: QL is the left singular-vector
|
||||
// matrix of Aᵀ and has n = rows(W) meaningful rows, so Vt needs all
|
||||
// n rows, not just p)
|
||||
|
||||
for (uint8_t i = 0; i < m; i++) {
|
||||
for (uint8_t j = 0; j < n; j++) {
|
||||
if (j < p) {
|
||||
if (transposeNeeded) {
|
||||
// U = QR[:, 0:p]ᵀ → U[i][j] = QR[j][i]
|
||||
U[i][j] = QR.Get(j, i);
|
||||
U[i][j] = QR.Get(i, j);
|
||||
} else {
|
||||
// U = QL[:, 0:p]
|
||||
U[i][j] = QL.Get(i, j);
|
||||
}
|
||||
} else {
|
||||
@@ -231,15 +653,11 @@ void SVD::AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
for (uint8_t j = 0; j < m; j++) {
|
||||
if (i < p && j < m) {
|
||||
if (transposeNeeded) {
|
||||
// Vt = QL[:, 0:p]ᵀ → Vt[i][j] = QL[j][i]
|
||||
Vt[i][j] = QL.Get(j, i);
|
||||
} else {
|
||||
// Vt = QR[:, 0:p]ᵀ → Vt[i][j] = QR[j][i]
|
||||
Vt[i][j] = QR.Get(j, i);
|
||||
}
|
||||
for (uint8_t j = 0; j < n; j++) {
|
||||
if (transposeNeeded) {
|
||||
Vt[i][j] = QL.Get(j, i);
|
||||
} else if (i < p) {
|
||||
Vt[i][j] = QR.Get(j, i);
|
||||
} else {
|
||||
Vt[i][j] = 0;
|
||||
}
|
||||
@@ -302,173 +720,99 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
|
||||
}
|
||||
|
||||
// ---- Phase 1: Householder Bidiagonalization ----
|
||||
// Reduce W to upper bidiagonal form using Householder reflections.
|
||||
// For a p×q matrix (p ≤ q after transpose), we do p steps:
|
||||
// Step k: zero out subdiagonal in column k, then superdiagonal in row k
|
||||
|
||||
float hhVec[5]; // Householder vector storage
|
||||
|
||||
for (uint8_t k = 0; k < p; k++) {
|
||||
// --- Left Householder on column k, rows k..min(m,n)-1 ---
|
||||
{
|
||||
uint8_t len = (m > n) ? m - k : n - k;
|
||||
if (len <= 1)
|
||||
continue;
|
||||
|
||||
// Extract the column segment
|
||||
float x[5];
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
x[i] = W[k + i][k];
|
||||
}
|
||||
|
||||
// Use building block to compute Householder reflector
|
||||
float alpha;
|
||||
SVD::ComputeHouseholder(x, len, hhVec, alpha);
|
||||
|
||||
if (alpha == 0.0f)
|
||||
continue;
|
||||
|
||||
// Apply H from left to W: W = H·W
|
||||
SVD::ApplyHouseholderLeft(W, hhVec, k, k + len - 1);
|
||||
|
||||
// Apply H from right to QL: QL = QL · H
|
||||
SVD::ApplyHouseholderRight(QL, hhVec, k, k + len - 1);
|
||||
}
|
||||
|
||||
// --- Right Householder on row k, columns k+2..min(m,n)-1 ---
|
||||
// (column k+1 is the first superdiagonal element, preserved in bidiagonal form)
|
||||
{
|
||||
uint8_t len = (p > 1) ? p - 2 - k : 0;
|
||||
if (len <= 0)
|
||||
continue;
|
||||
|
||||
// Extract the row segment starting from column k+2
|
||||
float x[5];
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
x[i] = W[k][k + 2 + i];
|
||||
}
|
||||
|
||||
// Use building block to compute Householder reflector
|
||||
float alpha;
|
||||
SVD::ComputeHouseholder(x, len, hhVec, alpha);
|
||||
|
||||
if (alpha == 0.0f)
|
||||
continue;
|
||||
|
||||
// Apply H from right to W: W = W·H
|
||||
SVD::ApplyHouseholderRight(W, hhVec, k + 2, k + 1 + len);
|
||||
|
||||
// Apply H from right to QR: QR = QR · H
|
||||
SVD::ApplyHouseholderRight(QR, hhVec, k + 2, k + 1 + len);
|
||||
}
|
||||
// For non-transpose (m ≥ n): W is m×n, bidiagonalize to get B (m×n)
|
||||
// For transpose (m < n): W is n×m (= Aᵀ), bidiagonalize to get B (n×m)
|
||||
// Pass the ACTUAL dimensions of W: Bidiagonalize needs the full row count
|
||||
// so that the last left Householder (k = p-1, len = rowsW - k) folds the
|
||||
// extra rows into the last diagonal element and zeros them out.
|
||||
{
|
||||
uint8_t rowsW = transposeNeeded ? n : m; // rows of W
|
||||
uint8_t colsW = transposeNeeded ? m : n; // columns of W
|
||||
SVD::Bidiagonalize(W, rowsW, colsW, p, QL, QR);
|
||||
}
|
||||
|
||||
// ---- Phase 2: Implicit QR Iteration on Bidiagonal Matrix ----
|
||||
// ---- Phase 2: QR Iteration on Bidiagonal Matrix -->
|
||||
// W now contains the upper bidiagonal matrix B.
|
||||
// We apply implicit QR steps to diagonalize it.
|
||||
// We apply QR iterations to converge superdiagonal elements to zero,
|
||||
// leaving singular values on the diagonal.
|
||||
//
|
||||
// rowsQL / rowsQR are the meaningful row extents of the accumulators.
|
||||
// QL is the LEFT factor of the bidiagonalized working matrix W, so it
|
||||
// carries rowsW = (transposeNeeded ? n : m) meaningful rows: in the wide
|
||||
// (transposed) case Vt = QLᵀ needs ALL n rows, so block factors must be
|
||||
// applied over 0..n−1. QR is only ever read back over its first m rows
|
||||
// (as U), but applying factors over all n rows is harmless and matches
|
||||
// the full-row Householder application in Bidiagonalize.
|
||||
uint8_t rowsQL = transposeNeeded ? n : m;
|
||||
uint8_t rowsQR = n;
|
||||
|
||||
uint32_t maxIter = 1000;
|
||||
float tol = 1e-8f;
|
||||
|
||||
for (uint32_t iter = 0; iter < maxIter; iter++) {
|
||||
// Deflate: zero out negligible subdiagonal elements
|
||||
for (uint8_t i = p - 1; i > 0; i--) {
|
||||
float test = fabsf(W[i][i - 1]);
|
||||
float scale = fabsf(W[i - 1][i - 1]) + fabsf(W[i][i]);
|
||||
if (test < tol * (scale + 1e-30f)) {
|
||||
W[i][i - 1] = 0;
|
||||
// Deflate: zero out negligible SUPERDIAGONAL elements
|
||||
SVD::DeflateBidiagonal(W, p, tol);
|
||||
|
||||
// If all superdiagonal elements are zero, we're done
|
||||
if (SVD::BidiagonalIsDiagonal(W, p, tol))
|
||||
break;
|
||||
|
||||
// Process all unreduced blocks in the matrix
|
||||
bool processedAny = false;
|
||||
uint8_t blockStart = 0;
|
||||
|
||||
while (blockStart < p - 1) {
|
||||
// Find end of current unreduced block
|
||||
uint8_t blockEnd = blockStart;
|
||||
while (blockEnd < p - 1 &&
|
||||
fabsf(W[blockEnd][blockEnd + 1]) >
|
||||
tol * fmaxf(fabsf(W[blockEnd][blockEnd]) +
|
||||
fabsf(W[blockEnd + 1][blockEnd + 1]),
|
||||
1e-10f)) {
|
||||
blockEnd++;
|
||||
}
|
||||
|
||||
// blockStart..blockEnd is an unreduced block of size
|
||||
// (blockEnd - blockStart + 1)
|
||||
uint8_t blockSize = blockEnd - blockStart + 1;
|
||||
|
||||
if (blockSize == 2) {
|
||||
// Handle 2×2 block directly using closed-form solution
|
||||
float Ub2[2][2] = {{0}}, Vb2[2][2] = {{0}};
|
||||
float sig[2] = {0, 0};
|
||||
SVD::SolveBidiagonalBlock2x2(W[blockStart][blockStart],
|
||||
W[blockStart][blockEnd],
|
||||
W[blockEnd][blockEnd], Ub2, Vb2, sig);
|
||||
|
||||
float Ublock[5][5] = {{0}}, Vblock[5][5] = {{0}};
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
for (uint8_t j = 0; j < 2; j++) {
|
||||
Ublock[i][j] = Ub2[i][j];
|
||||
Vblock[i][j] = Vb2[i][j];
|
||||
}
|
||||
|
||||
// Apply block factors to the accumulators
|
||||
SVD::ApplyBlockFactorsToAccumulators(blockStart, 2, Ublock, Vblock,
|
||||
rowsQL, rowsQR, QL, QR);
|
||||
|
||||
// Store singular values on diagonal, zero the superdiagonal
|
||||
W[blockStart][blockStart] = sig[0];
|
||||
W[blockEnd][blockEnd] = sig[1];
|
||||
W[blockStart][blockEnd] = 0;
|
||||
} else if (blockSize > 2) {
|
||||
// Larger blocks: SVD via eigendecomposition of BᵀB (Jacobi)
|
||||
SVD::SolveBidiagonalBlockJacobi(W, blockStart, blockSize, rowsQL,
|
||||
rowsQR, QL, QR, tol);
|
||||
}
|
||||
|
||||
processedAny = true;
|
||||
blockStart = blockEnd + 1; // Move to next block
|
||||
}
|
||||
|
||||
// Find the smallest unreduced block [start..end]
|
||||
uint8_t start = 0, end = p - 1;
|
||||
for (uint8_t i = 0; i < p - 1; i++) {
|
||||
if (fabsf(W[i + 1][i]) >
|
||||
tol * (fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]) + 1e-30f)) {
|
||||
start = i + 1;
|
||||
}
|
||||
}
|
||||
for (int8_t i = (int8_t)p - 2; i >= 0; i--) {
|
||||
if (fabsf(W[i + 1][i]) >
|
||||
tol * (fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]) + 1e-30f)) {
|
||||
end = (uint8_t)i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Check convergence of the block
|
||||
if (start >= end) {
|
||||
continue;
|
||||
}
|
||||
|
||||
bool blockConverged = true;
|
||||
for (uint8_t i = start; i <= end; i++) {
|
||||
if (i > start && fabsf(W[i][i - 1]) > tol * (fabsf(W[i - 1][i - 1]) +
|
||||
fabsf(W[i][i]) + 1e-30f)) {
|
||||
blockConverged = false;
|
||||
break;
|
||||
}
|
||||
if (i < end &&
|
||||
fabsf(W[i][i + 1]) >
|
||||
tol * (fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]) + 1e-30f)) {
|
||||
blockConverged = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (blockConverged)
|
||||
continue;
|
||||
|
||||
// Wilkinson shift from bottom 2×2 corner
|
||||
float a = W[end - 1][end - 1];
|
||||
float b = W[end - 1][end];
|
||||
float c = W[end][end - 1];
|
||||
float d = W[end][end];
|
||||
|
||||
float trace = a + d;
|
||||
float det = a * d - b * c;
|
||||
float disc = trace * trace - 4.0f * det;
|
||||
float shift;
|
||||
if (disc >= 0) {
|
||||
float sqrtDisc = sqrtf(disc);
|
||||
float e1 = (trace + sqrtDisc) / 2.0f;
|
||||
float e2 = (trace - sqrtDisc) / 2.0f;
|
||||
shift = (fabsf(e1 - d) < fabsf(e2 - d)) ? e1 : e2;
|
||||
} else {
|
||||
shift = d;
|
||||
}
|
||||
|
||||
// --- Implicit QR step using Givens rotations ---
|
||||
// First, apply Givens rotation from the left to zero out (W[start][start-1]
|
||||
// - shift) For the bidiagonal structure, we process from top to bottom.
|
||||
|
||||
float x = W[start][start] - shift;
|
||||
float y = (start > 0) ? W[start][start - 1] : 0.0f;
|
||||
|
||||
for (uint8_t i = start; i <= end; i++) {
|
||||
float cs, sn;
|
||||
SVD::ComputeGivens(x, y, cs, sn);
|
||||
|
||||
// Apply Givens from left to rows i, i+1 of W (columns i..p-1)
|
||||
SVD::ApplyGivensLeft(W, i, i + 1, cs, sn, i, p - 1);
|
||||
|
||||
// Apply Givens from right to columns i, i+1 of W (rows 0..i)
|
||||
if (i > start) {
|
||||
SVD::ApplyGivensRight(W, i, i + 1, cs, sn, 0, i);
|
||||
}
|
||||
|
||||
// Accumulate right transformations into QR
|
||||
for (uint8_t j = 0; j < n; j++) {
|
||||
float t1 = QR[j][i];
|
||||
float t2 = QR[j][i + 1];
|
||||
QR[j][i] = cs * t1 + sn * t2;
|
||||
QR[j][i + 1] = -sn * t1 + cs * t2;
|
||||
}
|
||||
|
||||
// Prepare next Givens rotation
|
||||
if (i + 1 <= end) {
|
||||
x = W[i + 1][i];
|
||||
y = (i + 1 < end) ? W[i + 1][i + 1] : 0.0f;
|
||||
}
|
||||
if (!processedAny) {
|
||||
// No unreduced blocks found, but superdiagonal is not all zero
|
||||
// This can happen with numerical issues, just break
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -491,8 +835,9 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
|
||||
U[i][j] = UInternal.Get(i, j);
|
||||
}
|
||||
}
|
||||
// Vt is a columns×columns (n×n) matrix: BOTH bounds must run over columns.
|
||||
for (uint8_t i = 0; i < columns; i++) {
|
||||
for (uint8_t j = 0; j < rows; j++) {
|
||||
for (uint8_t j = 0; j < columns; j++) {
|
||||
Vt[i][j] = VtInternal.Get(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
+170
@@ -78,6 +78,176 @@ static void ApplyHouseholderLeft(Matrix<5, 5> &W, const float *v,
|
||||
static void ApplyHouseholderRight(Matrix<5, 5> &W, const float *v,
|
||||
uint8_t startCol, uint8_t endCol);
|
||||
|
||||
/**
|
||||
* @brief Reduce a matrix to upper bidiagonal form using Householder reflections.
|
||||
*
|
||||
* Applies a sequence of Householder reflections to reduce the input matrix
|
||||
* W (m×q, where q ≥ p) to upper bidiagonal form B (p×q), accumulating
|
||||
* the left and right transformation matrices in QL and QR respectively.
|
||||
*
|
||||
* Algorithm (Golub-Kahan bidiagonalization):
|
||||
* For k = 0 to p-1:
|
||||
* 1. Left HH on column k, rows k..m-1: zero out subdiagonal below B[k+1][k]
|
||||
* 2. Right HH on row k, cols k+2..q-1: zero out superdiagonal above B[k][k+1]
|
||||
*
|
||||
* The accumulated transformations satisfy:
|
||||
* QLᵀ · W_original · QR = B (upper bidiagonal)
|
||||
*
|
||||
* @param W Input/output: matrix to bidiagonalize (5×5, must be at least p×q)
|
||||
* @param m Number of rows in the working matrix
|
||||
* @param q Number of columns in the working matrix (q ≥ p)
|
||||
* @param p Rank = min(m, original_columns) — number of bidiagonalization steps
|
||||
* @param QL Input/output: left Householder accumulation (initialized to identity,
|
||||
* output: QLᵀ such that QLᵀ·W = B)
|
||||
* @param QR Input/output: right Householder accumulation (initialized to identity,
|
||||
* output: QR such that W·QR = B after left apply)
|
||||
*/
|
||||
static void Bidiagonalize(Matrix<5, 5> &W,
|
||||
uint8_t m, uint8_t q, uint8_t p,
|
||||
Matrix<5, 5> &QL,
|
||||
Matrix<5, 5> &QR);
|
||||
|
||||
/**
|
||||
* @brief Deflate a bidiagonal matrix by zeroing negligible superdiagonals.
|
||||
*
|
||||
* Scans the p×p upper-bidiagonal matrix stored in W and zeros out any
|
||||
* superdiagonal element W[i][i+1] whose magnitude is negligible relative to
|
||||
* the local diagonal scale (|W[i][i]| + |W[i+1][i+1]|). Deflating splits
|
||||
* the matrix into independent unreduced blocks that can each be solved
|
||||
* separately.
|
||||
*
|
||||
* @param W Input/output: bidiagonal matrix (5×5 working array, first p×p used)
|
||||
* @param p Size of the bidiagonal matrix (min(rows, columns))
|
||||
* @param tol Relative deflation tolerance (e.g. 1e-8f)
|
||||
*/
|
||||
static void DeflateBidiagonal(Matrix<5, 5> &W, uint8_t p, float tol);
|
||||
|
||||
/**
|
||||
* @brief Check whether a bidiagonal matrix has fully reduced to diagonal.
|
||||
*
|
||||
* Returns true when every superdiagonal element of the p×p bidiagonal
|
||||
* matrix in W is (numerically) zero, i.e. the diagonal entries are the
|
||||
* (unsorted) singular values and no unreduced blocks remain.
|
||||
*
|
||||
* @param W Input: bidiagonal matrix (5×5 working array, first p×p used)
|
||||
* @param p Size of the bidiagonal matrix (min(rows, columns))
|
||||
* @param tol Numerical zero threshold multiplier
|
||||
* @return true when all superdiagonal elements are ~0
|
||||
*/
|
||||
static bool BidiagonalIsDiagonal(const Matrix<5, 5> &W, uint8_t p, float tol);
|
||||
|
||||
/**
|
||||
* @brief Compute the full SVD of a 2×2 upper-bidiagonal block (pure).
|
||||
*
|
||||
* Decomposes B = [[a, b], [0, d]] as:
|
||||
* B = Ublock · diag(sigma[0], sigma[1]) · Vblockᵀ
|
||||
*
|
||||
* Guarantees:
|
||||
* - sigma[0] ≥ sigma[1] ≥ 0 (singular values, from eigenvalues of BᵀB)
|
||||
* - Ublock and Vblock are orthogonal (columns are the left/right
|
||||
* singular vectors respectively; Vblock = scipy's Vᵀᵀ)
|
||||
* - Ublock · diag(sigma) · Vblockᵀ == B (within float tolerance)
|
||||
*
|
||||
* Math: eigenvectors of BᵀB = [[a², ab], [ab, b²+d²]] give the right
|
||||
* singular vectors (v1 = normalize(ab, σ1²−a²) with a safe fallback when
|
||||
* that vector is ~0; v2 = (−v1y, v1x)); left singular vectors are
|
||||
* uᵢ = B·vᵢ/σᵢ with a rank-deficiency guard: when σᵢ ≈ 0 (i.e. ~1e-30),
|
||||
* that U column is filled with the signed orthogonal complement of the
|
||||
* other U column instead of dividing by ~0.
|
||||
*
|
||||
* @param a B[0][0] (first diagonal element)
|
||||
* @param b B[0][1] (superdiagonal element)
|
||||
* @param d B[1][1] (second diagonal element)
|
||||
* @param Ublock Output: 2×2 left singular vectors (columns)
|
||||
* @param Vblock Output: 2×2 right singular vectors (columns)
|
||||
* @param sigma Output: singular values, sigma[0] ≥ sigma[1] ≥ 0
|
||||
*/
|
||||
static void SolveBidiagonalBlock2x2(float a, float b, float d,
|
||||
float Ublock[2][2], float Vblock[2][2],
|
||||
float sigma[2]);
|
||||
|
||||
/**
|
||||
* @brief Cyclic Jacobi eigenvalue algorithm for a symmetric matrix (pure).
|
||||
*
|
||||
* Reduces symmetric n×n matrix T to (near-)diagonal form IN PLACE using
|
||||
* cyclic Jacobi rotations, accumulating the eigenvectors in V.
|
||||
*
|
||||
* On return:
|
||||
* - T's diagonal entries are the eigenvalues (off-diagonals ~0)
|
||||
* - evals[i] = T[i][i], UNSORTED
|
||||
* - columns of V are the corresponding eigenvectors (T·V = V·Λ)
|
||||
*
|
||||
* @param T Input/output: symmetric matrix (5×5 storage, first n×n used,
|
||||
* destroyed in place)
|
||||
* @param n Matrix size (≤ 5)
|
||||
* @param evals Output: eigenvalues, unsorted (evals[i] = T[i][i])
|
||||
* @param V Output: eigenvector matrix, columns are eigenvectors
|
||||
*/
|
||||
static void JacobiEigenSymmetric(float T[5][5], uint8_t n, float evals[5],
|
||||
float V[5][5]);
|
||||
|
||||
/**
|
||||
* @brief Fold a block SVD's factors into the QL/QR accumulators.
|
||||
*
|
||||
* Given the block SVD of a bidiagonal block, B = Ublock·Σ·Vblockᵀ, the
|
||||
* accumulated Householder matrices must absorb the block factors:
|
||||
* QL[:, blockStart..blockStart+blockSize−1] ← QL[:, ...] · Ublock
|
||||
* (rows 0..rowsQL−1)
|
||||
* QR[:, blockStart..blockStart+blockSize−1] ← QR[:, ...] · Vblock
|
||||
* (rows 0..rowsQR−1)
|
||||
*
|
||||
* rowsQL / rowsQR are the meaningful row extents of the accumulators
|
||||
* (e.g. for a wide matrix W = Aᵀ, QL carries n = rows(W) meaningful
|
||||
* rows while QR is read back over its first m rows).
|
||||
*
|
||||
* @param blockStart First column/row index of the block in W
|
||||
* @param blockSize Size of the block (2, or > 2 for the Jacobi path)
|
||||
* @param Ublock Left singular-vector factor of the block (blockSize×blockSize in 5×5 storage)
|
||||
* @param Vblock Right singular-vector factor of the block (blockSize×blockSize in 5×5 storage)
|
||||
* @param rowsQL Number of meaningful rows of QL
|
||||
* @param rowsQR Number of meaningful rows of QR
|
||||
* @param QL Input/output: left transformation accumulator
|
||||
* @param QR Input/output: right transformation accumulator
|
||||
*/
|
||||
static void ApplyBlockFactorsToAccumulators(uint8_t blockStart,
|
||||
uint8_t blockSize,
|
||||
const float Ublock[5][5],
|
||||
const float Vblock[5][5],
|
||||
uint8_t rowsQL, uint8_t rowsQR,
|
||||
Matrix<5, 5> &QL,
|
||||
Matrix<5, 5> &QR);
|
||||
|
||||
/**
|
||||
* @brief Solve a bidiagonal block larger than 2×2 via Jacobi eigen of BᵀB.
|
||||
*
|
||||
* Computes the full SVD of the unreduced upper-bidiagonal block
|
||||
* W[blockStart..blockStart+blockSize−1] via eigendecomposition of the
|
||||
* tridiagonal T = BᵀB:
|
||||
* 1. Snapshot the ORIGINAL block diagonal/superdiagonal from W
|
||||
* 2. JacobiEigenSymmetric on T → eigenvalues (unsorted) + V
|
||||
* 3. Sort eigenvalues descending, reordering V
|
||||
* 4. Ublock = B_orig · V · Σ⁻¹ (from the snapshot, so W is not
|
||||
* overwritten before Ublock is computed)
|
||||
* 5. Fold Ublock/Vblock into QL/QR via ApplyBlockFactorsToAccumulators
|
||||
* 6. Only then write sqrt(eigenvalues) into W's diagonal and zero the
|
||||
* block's superdiagonals
|
||||
*
|
||||
* @param W Input/output: bidiagonal matrix (5×5 working array); the block's
|
||||
* diagonal holds the singular values and its superdiagonals are
|
||||
* zeroed on return
|
||||
* @param blockStart First column/row index of the block
|
||||
* @param blockSize Size of the block (> 2, ≤ 5)
|
||||
* @param rowsQL Number of meaningful rows of QL
|
||||
* @param rowsQR Number of meaningful rows of QR
|
||||
* @param QL Input/output: left transformation accumulator
|
||||
* @param QR Input/output: right transformation accumulator
|
||||
* @param tol (unused: Jacobi convergence tolerance is internal)
|
||||
*/
|
||||
static void SolveBidiagonalBlockJacobi(Matrix<5, 5> &W, uint8_t blockStart,
|
||||
uint8_t blockSize, uint8_t rowsQL,
|
||||
uint8_t rowsQR, Matrix<5, 5> &QL,
|
||||
Matrix<5, 5> &QR, float tol);
|
||||
|
||||
/**
|
||||
* @brief Extract singular values from bidiagonal matrix diagonal and sort.
|
||||
*
|
||||
|
||||
+28
-28
@@ -390,7 +390,7 @@ TEST_CASE("Identity Matrix", "Matrix") {
|
||||
if (oneColumnIndex == column) {
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
|
||||
} else {
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
|
||||
}
|
||||
}
|
||||
oneColumnIndex++;
|
||||
@@ -407,7 +407,7 @@ TEST_CASE("Identity Matrix", "Matrix") {
|
||||
if (oneColumnIndex == column && row < 3) {
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
|
||||
} else {
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
|
||||
}
|
||||
}
|
||||
oneColumnIndex++;
|
||||
@@ -423,7 +423,7 @@ TEST_CASE("Identity Matrix", "Matrix") {
|
||||
if (oneColumnIndex == column) {
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
|
||||
} else {
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
|
||||
REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
|
||||
}
|
||||
}
|
||||
oneColumnIndex++;
|
||||
@@ -519,7 +519,7 @@ TEST_CASE("QR Decompositions", "Matrix") {
|
||||
// check that all R values are correct
|
||||
REQUIRE_THAT(R[0][0], Catch::Matchers::WithinRel(3.16228f, 1e-4f));
|
||||
REQUIRE_THAT(R[0][1], Catch::Matchers::WithinRel(4.42719f, 1e-4f));
|
||||
REQUIRE_THAT(R[1][0], Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(R[1][0], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(R[1][1], Catch::Matchers::WithinRel(0.63246f, 1e-4f));
|
||||
}
|
||||
|
||||
@@ -635,7 +635,7 @@ TEST_CASE("Eigenvalues and Vectors", "Matrix") {
|
||||
REQUIRE_THAT(vectors[1][0], Catch::Matchers::WithinRel(0.525322f, 1e-4f));
|
||||
REQUIRE_THAT(vectors[2][0], Catch::Matchers::WithinRel(0.81867f, 1e-4f));
|
||||
REQUIRE_THAT(values[0][0], Catch::Matchers::WithinRel(-1.11684f, 1e-4f));
|
||||
REQUIRE_THAT(values[1][0], Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(values[1][0], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(values[2][0], Catch::Matchers::WithinRel(16.1168f, 1e-4f));
|
||||
}
|
||||
}
|
||||
@@ -753,14 +753,14 @@ TEST_CASE("SVD: Simple 2x2 Matrix", "Matrix") {
|
||||
REQUIRE(isSortedDescending(sigma, 2));
|
||||
|
||||
// Verify U is orthogonal: UᵀU ≈ I
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
// Verify Vt is orthogonal: VtVᵀ ≈ I
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
// Verify reconstruction: A ≈ U Σ Vᵀ
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Symmetric Positive Definite 2x2", "Matrix") {
|
||||
@@ -777,7 +777,7 @@ TEST_CASE("SVD: Symmetric Positive Definite 2x2", "Matrix") {
|
||||
|
||||
// For symmetric PD matrices, U ≈ V (up to sign)
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Full-Rank 3x3 Matrix", "Matrix") {
|
||||
@@ -797,11 +797,11 @@ TEST_CASE("SVD: Full-Rank 3x3 Matrix", "Matrix") {
|
||||
Catch::Matchers::WithinRel(0.1968665211f, 1e-4f));
|
||||
|
||||
REQUIRE(isSortedDescending(sigma, 3));
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Rank-Deficient 3x3 Matrix", "Matrix") {
|
||||
@@ -821,7 +821,7 @@ TEST_CASE("SVD: Rank-Deficient 3x3 Matrix", "Matrix") {
|
||||
REQUIRE(sigma.Get(2, 0) < 1e-3f);
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Diagonal 3x3 Matrix", "Matrix") {
|
||||
@@ -837,7 +837,7 @@ TEST_CASE("SVD: Diagonal 3x3 Matrix", "Matrix") {
|
||||
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(2.0f, 1e-4f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Tall Matrix (4×3)", "Matrix") {
|
||||
@@ -858,10 +858,10 @@ TEST_CASE("SVD: Tall Matrix (4×3)", "Matrix") {
|
||||
REQUIRE(sigma.Get(2, 0) < 1e-3f);
|
||||
|
||||
// U should be 4×3 with orthonormal columns
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Wide Matrix (3×5)", "Matrix") {
|
||||
@@ -882,10 +882,10 @@ TEST_CASE("SVD: Wide Matrix (3×5)", "Matrix") {
|
||||
REQUIRE(sigma.Get(2, 0) < 1e-3f);
|
||||
|
||||
// Vt should be 5×5 with orthonormal rows (first k)
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: 5×5 Symmetric Tridiagonal", "Matrix") {
|
||||
@@ -908,11 +908,11 @@ TEST_CASE("SVD: 5×5 Symmetric Tridiagonal", "Matrix") {
|
||||
Catch::Matchers::WithinRel(0.2679491924f, 1e-4f));
|
||||
|
||||
REQUIRE(isSortedDescending(sigma, 5));
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Non-Square with Negative Values (2×3)", "Matrix") {
|
||||
@@ -931,7 +931,7 @@ TEST_CASE("SVD: Non-Square with Negative Values (2×3)", "Matrix") {
|
||||
Catch::Matchers::WithinRel(0.6646227432f, 1e-4f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Near-Singular 2×2 Matrix", "Matrix") {
|
||||
@@ -948,7 +948,7 @@ TEST_CASE("SVD: Near-Singular 2×2 Matrix", "Matrix") {
|
||||
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1e-6f, 1e-2f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-6f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Orthogonal Matrix (3×3)", "Matrix") {
|
||||
@@ -968,7 +968,7 @@ TEST_CASE("SVD: Orthogonal Matrix (3×3)", "Matrix") {
|
||||
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Identity Matrix", "Matrix") {
|
||||
@@ -984,7 +984,7 @@ TEST_CASE("SVD: Identity Matrix", "Matrix") {
|
||||
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-6f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: Zero Matrix", "Matrix") {
|
||||
@@ -1000,7 +1000,7 @@ TEST_CASE("SVD: Zero Matrix", "Matrix") {
|
||||
REQUIRE(sigma.Get(2, 0) < 1e-6f);
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-6f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: 2×1 Column Vector", "Matrix") {
|
||||
@@ -1016,7 +1016,7 @@ TEST_CASE("SVD: 2×1 Column Vector", "Matrix") {
|
||||
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-4f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD: 1×2 Row Vector", "Matrix") {
|
||||
@@ -1032,5 +1032,5 @@ TEST_CASE("SVD: 1×2 Row Vector", "Matrix") {
|
||||
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-4f));
|
||||
|
||||
float reconErr = svdReconstructionError(A, U, sigma, Vt);
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
}
|
||||
@@ -1025,3 +1025,384 @@ TEST_CASE("SVD Phase 4: AssembleUAndVt", "[Matrix][SVD]") {
|
||||
REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(-0.6f, 1e-6f));
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// TEST 13: Bidiagonalize (Phase 1) - Square matrix
|
||||
// ===========================================================================
|
||||
TEST_CASE("SVD Phase 1: Bidiagonalize square matrix", "[Matrix][SVD]") {
|
||||
// Test case 1: 3×3 matrix
|
||||
// C++ verified reference:
|
||||
// W[0] = [-4.123106, -5.335784, 6.548462]
|
||||
// W[1] = [ 0.000000, 7.037714, -8.107580]
|
||||
// W[2] = [ 0.000000, 0.000000, 0.620321]
|
||||
// Note: W[0][2]=6.548462 is NOT zeroed because right HH at k=0 has only 1 element
|
||||
{
|
||||
Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
|
||||
4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
|
||||
0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 3, 3, 3, QL, QR);
|
||||
|
||||
// Subdiagonal elements should be zero: W[1][0], W[2][0], W[2][1]
|
||||
REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
// Verify orthogonality
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
|
||||
// Test case 2: 2×2 matrix (simplest non-trivial case)
|
||||
// C++ verified reference:
|
||||
// W[0] = [-3.162278, -4.427189]
|
||||
// W[1] = [-0.000000, 0.632456]
|
||||
{
|
||||
Matrix<5, 5> W{3.0f, 4.0f, 0.0f, 0.0f, 0.0f,
|
||||
1.0f, 2.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 2, 2, 2, QL, QR);
|
||||
|
||||
// For 2×2, bidiagonal form has no elements to zero out
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
|
||||
// Test case 3: Diagonal matrix (no transformations needed)
|
||||
// C++ verified reference: unchanged
|
||||
{
|
||||
Matrix<5, 5> W{10.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 5.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 2.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> W_orig{10.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 5.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 2.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 3, 3, 3, QL, QR);
|
||||
|
||||
// Diagonal matrix may have sign flips but absolute values preserved
|
||||
float err = 0.0f;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
float diff = fabsf(W.Get(i, i)) - fabsf(W_orig.Get(i, i));
|
||||
err += diff * diff;
|
||||
}
|
||||
REQUIRE_THAT(sqrtf(err), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
|
||||
|
||||
// QL and QR may have sign flips but should remain orthogonal
|
||||
// Check that |QL[i][j]| and |QR[i][j]| match identity pattern
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
for (uint8_t j = 0; j < 5; j++) {
|
||||
float expected = (i == j) ? 1.0f : 0.0f;
|
||||
REQUIRE_THAT(fabsf(QL.Get(i, j)), Catch::Matchers::WithinAbs(expected, 1e-6f));
|
||||
REQUIRE_THAT(fabsf(QR.Get(i, j)), Catch::Matchers::WithinAbs(expected, 1e-6f));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// TEST 14: Bidiagonalize (Phase 1) — Tall matrix (m > n)
|
||||
// ===========================================================================
|
||||
TEST_CASE("SVD Phase 1: Bidiagonalize tall matrix", "[Matrix][SVD]") {
|
||||
// Test case 1: 4×3 matrix
|
||||
// C++ verified reference (partial - subdiagonal zeros):
|
||||
// W[0] = [-4.123106, -5.335784, 6.548462]
|
||||
// W[1] = [-0.000000, 12.228222, 12.026182]
|
||||
// W[2] = [ 0.000000, 0.000000, -1.577527]
|
||||
// W[3] = [ 0.000000, 0.000000, 0.000000]
|
||||
{
|
||||
Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
|
||||
4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
|
||||
0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
|
||||
0.0f, 10.0f, 9.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 4, 3, 3, QL, QR);
|
||||
|
||||
// Zero below subdiagonal: W[2][0], W[3][0], W[3][1]
|
||||
REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(3, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
// Verify orthogonality
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
|
||||
// Test case 2: 3×2 matrix
|
||||
// C++ verified reference:
|
||||
// W[0] = [-5.916080, -7.437357]
|
||||
// W[1] = [-0.000001, 0.828077]
|
||||
// W[2] = [-0.000000, -0.000000]
|
||||
{
|
||||
Matrix<5, 5> W{1.0f, 2.0f, 0.0f, 0.0f, 0.0f,
|
||||
3.0f, 4.0f, 0.0f, 0.0f, 0.0f,
|
||||
5.0f, 6.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 3, 2, 2, QL, QR);
|
||||
|
||||
// Zero below subdiagonal: W[2][0] ≈ 0
|
||||
REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
|
||||
// Verify orthogonality
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
|
||||
// Test case 3: 5×3 matrix (full 5-row tall)
|
||||
{
|
||||
Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
|
||||
4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
|
||||
0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
|
||||
0.0f, 10.0f, 9.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 5, 3, 3, QL, QR);
|
||||
|
||||
// Zero below subdiagonal: W[2][0], W[3][0], W[4][0], W[3][1], W[4][1]
|
||||
REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(4, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(3, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(4, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
// Verify orthogonality
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// TEST 15: Bidiagonalize (Phase 1) — Wide matrix (m < n)
|
||||
// ===========================================================================
|
||||
TEST_CASE("SVD Phase 1: Bidiagonalize wide matrix", "[Matrix][SVD]") {
|
||||
// Test case 1: 2×4 matrix
|
||||
// C++ verified reference:
|
||||
// W[0] = [-5.099020, -6.275717, 11.401754, 0.000000]
|
||||
// W[1] = [ 0.000000, -0.784465, 2.806586, -0.350823]
|
||||
// Note: W[1][2]=2.806586 and W[1][3]=-0.350823 are NOT zeroed because
|
||||
// right HH at k=0 has only 2 elements (cols 2,3), so it zeros col 3 but preserves col 2
|
||||
{
|
||||
Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 4.0f, 0.0f,
|
||||
5.0f, 6.0f, 7.0f, 8.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 2, 4, 2, QL, QR);
|
||||
|
||||
// For 2×4: right HH at k=0 has 2 elements (cols 2,3)
|
||||
// It zeros col 3 but preserves col 2 as the superdiagonal element for row 1
|
||||
// W[0][3] should be zeroed (above superdiagonal in row 0)
|
||||
REQUIRE_THAT(W.Get(0, 3), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
// W[1][2] and W[1][3] are part of the bidiagonal structure for row 1
|
||||
// (superdiagonal at col 2, and right HH preserves first element)
|
||||
REQUIRE_THAT(W.Get(1, 2), !Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
// Verify orthogonality
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
|
||||
// Test case 2: 3×5 matrix
|
||||
{
|
||||
Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 4.0f, 5.0f,
|
||||
6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
|
||||
0.0f, 11.0f, 12.0f, 13.0f, 14.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 3, 5, 3, QL, QR);
|
||||
|
||||
// For 3×5: check that subdiagonal elements are zero
|
||||
REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
|
||||
|
||||
// Verify orthogonality
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
|
||||
// Test case 3: 1×3 matrix (row vector)
|
||||
// C++ verified reference: W[0] = [1.0, 2.0, 3.0] (no transformations needed)
|
||||
{
|
||||
Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 1, 3, 1, QL, QR);
|
||||
|
||||
// For 1×3, no transformations needed
|
||||
REQUIRE_THAT(W.Get(0, 0), Catch::Matchers::WithinAbs(1.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(0, 1), Catch::Matchers::WithinAbs(2.0f, 1e-4f));
|
||||
REQUIRE_THAT(W.Get(0, 2), Catch::Matchers::WithinAbs(3.0f, 1e-4f));
|
||||
|
||||
// Verify orthogonality
|
||||
REQUIRE(isOrthogonal5(QL));
|
||||
REQUIRE(isOrthogonal5(QR));
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// TEST 16: Bidiagonalize — Reconstruction property
|
||||
// ===========================================================================
|
||||
TEST_CASE("SVD Phase 1: Bidiagonalize reconstruction property", "[Matrix][SVD]") {
|
||||
// Test: QLᵀ · W_original · QR = B (bidiagonal)
|
||||
// This verifies that the accumulated transformations correctly represent
|
||||
// the bidiagonalization.
|
||||
{
|
||||
Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
|
||||
4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
|
||||
0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> W = W_orig;
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 3, 3, 3, QL, QR);
|
||||
|
||||
// Compute QLᵀ · W_orig · QR and verify it equals W (the bidiagonal result)
|
||||
Matrix<5, 5> Qt = QL.Transpose();
|
||||
Matrix<5, 5> QtW_orig{0};
|
||||
Qt.Mult(W_orig, QtW_orig);
|
||||
|
||||
Matrix<5, 5> QtW_origQR{0};
|
||||
QtW_orig.Mult(QR, QtW_origQR);
|
||||
|
||||
// The reconstruction should match the bidiagonal result
|
||||
float err = frobeniusNorm5(W - QtW_origQR);
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(1e-3f, 1e-3f));
|
||||
}
|
||||
|
||||
// Test: Tall matrix reconstruction (4×3)
|
||||
{
|
||||
Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
|
||||
4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
|
||||
0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
|
||||
0.0f, 10.0f, 9.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> W = W_orig;
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 4, 3, 3, QL, QR);
|
||||
|
||||
Matrix<5, 5> Qt = QL.Transpose();
|
||||
Matrix<5, 5> QtW_orig{0};
|
||||
Qt.Mult(W_orig, QtW_orig);
|
||||
|
||||
Matrix<5, 5> QtW_origQR{0};
|
||||
QtW_orig.Mult(QR, QtW_origQR);
|
||||
|
||||
float err = frobeniusNorm5(W - QtW_origQR);
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(1e-3f, 1e-3f));
|
||||
}
|
||||
|
||||
// Test: Wide matrix reconstruction (2×4)
|
||||
{
|
||||
Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 4.0f, 0.0f,
|
||||
5.0f, 6.0f, 7.0f, 8.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
Matrix<5, 5> W = W_orig;
|
||||
|
||||
Matrix<5, 5> QL{0}, QR{0};
|
||||
for (uint8_t i = 0; i < 5; i++) {
|
||||
QL[i][i] = 1.0f;
|
||||
QR[i][i] = 1.0f;
|
||||
}
|
||||
|
||||
SVD::Bidiagonalize(W, 2, 4, 2, QL, QR);
|
||||
|
||||
Matrix<5, 5> Qt = QL.Transpose();
|
||||
Matrix<5, 5> QtW_orig{0};
|
||||
Qt.Mult(W_orig, QtW_orig);
|
||||
|
||||
Matrix<5, 5> QtW_origQR{0};
|
||||
QtW_orig.Mult(QR, QtW_origQR);
|
||||
|
||||
float err = frobeniusNorm5(W - QtW_origQR);
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(1e-3f, 1e-3f));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -62,7 +62,7 @@ TEST_CASE("SVD Integration: 2x2 [[1,2],[3,4]]", "[Matrix][SVD][Integration]") {
|
||||
err += diff * diff;
|
||||
}
|
||||
err = sqrtf(err);
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
|
||||
std::cout << "SVD 2x2 [[1,2],[3,4]]:\n";
|
||||
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0)
|
||||
@@ -86,8 +86,8 @@ TEST_CASE("SVD Integration: 3x3 diagonal [10,5,2]",
|
||||
// U and Vt should be identity (or close) for diagonal matrix
|
||||
float uErr = frobeniusNorm(U - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
|
||||
float vtErr = frobeniusNorm(Vt - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
|
||||
REQUIRE_THAT(uErr, Catch::Matchers::WithinRel(0.0f, 1e-2f));
|
||||
REQUIRE_THAT(vtErr, Catch::Matchers::WithinRel(0.0f, 1e-2f));
|
||||
REQUIRE_THAT(uErr, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
|
||||
REQUIRE_THAT(vtErr, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD Integration: 3x3 rank-deficient [[1,2,3],[4,5,6],[7,8,9]]",
|
||||
@@ -120,7 +120,7 @@ TEST_CASE("SVD Integration: 3x3 rank-deficient [[1,2,3],[4,5,6],[7,8,9]]",
|
||||
err += diff * diff;
|
||||
}
|
||||
err = sqrtf(err);
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-2f));
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
|
||||
|
||||
std::cout << "SVD 3x3 rank-deficient:\n";
|
||||
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
|
||||
@@ -155,7 +155,7 @@ TEST_CASE("SVD Integration: tall 4x3 matrix", "[Matrix][SVD][Integration]") {
|
||||
err += diff * diff;
|
||||
}
|
||||
err = sqrtf(err);
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-2f));
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
|
||||
|
||||
std::cout << "SVD tall 4x3:\n";
|
||||
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
|
||||
@@ -175,42 +175,25 @@ TEST_CASE("SVD Integration: wide 3x5 matrix", "[Matrix][SVD][Integration]") {
|
||||
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(2.46540f, 1e-2f));
|
||||
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-2f));
|
||||
|
||||
// Check reconstruction: U (3x5) * diag(sigma) (5x3) = 3x3, then * Vt (3x5) =
|
||||
// 3x5
|
||||
Matrix<3, 5> recon{0};
|
||||
Matrix<3, 5> Usig{0};
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 5; j++)
|
||||
Usig[i][j] = U.Get(i, j) * sigma.Get(j, 0);
|
||||
|
||||
// For wide matrix: A = U * Sigma * Vt where U is 3x5, Sigma is 5x5
|
||||
// (diagonal), Vt is 5x5 But our implementation returns sigma as 3x1 and Vt as
|
||||
// 3x5 So we need: recon = Usig (3x5) * Vt (3x5)^T ... no that doesn't work
|
||||
// either The SVD for wide matrices is: A = U * Sigma * Vt where:
|
||||
// U is m×m (3×3), Sigma is m×n (3×5), Vt is n×n (5×5)
|
||||
// But our API returns U as m×n (3×5), sigma as n×1 (3×1), Vt as n×n (3×5)
|
||||
// So: recon = U (3x5) * diag(sigma) (5x5) * Vt (5x5)^T ...
|
||||
// Actually, looking at the implementation, for wide matrices we swap roles.
|
||||
// Let me just check reconstruction using the actual dimensions returned.
|
||||
|
||||
// For wide matrix: A (3x5) = U (3x5) * diag(sigma) (5x5 padded) * Vt (5x5)
|
||||
// But our API returns Vt as 3x5, not 5x5
|
||||
// The implementation stores: U = QR[:,0:p]^T (3x5), sigma (3x1), Vt =
|
||||
// QL[:,0:p]^T (3x5) Reconstruction: A[i][j] = sum_k U[i][k]*sigma[k]*Vt[j][k]
|
||||
// Check reconstruction: A (3x5) = U * Sigma * Vt, where U (3x5) has
|
||||
// its meaningful part in the first 3 columns, sigma (5x1) in the
|
||||
// first 3 entries, and Vt (5x5) in its first 3 rows (right
|
||||
// singular vectors as rows). So:
|
||||
// A[i][j] = sum_k U[i][k] * sigma[k] * Vt[k][j]
|
||||
|
||||
float err2 = 0.0f;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 5; j++) {
|
||||
float recon_val = 0.0f;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
recon_val += U.Get(i, k) * sigma.Get(k, 0) * Vt.Get(j, k);
|
||||
recon_val += U.Get(i, k) * sigma.Get(k, 0) * Vt.Get(k, j);
|
||||
}
|
||||
float diff = recon_val - A.Get(i, j);
|
||||
err2 += diff * diff;
|
||||
}
|
||||
}
|
||||
err2 = sqrtf(err2);
|
||||
REQUIRE_THAT(err2, Catch::Matchers::WithinRel(0.0f, 1e-2f));
|
||||
REQUIRE_THAT(err2, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
|
||||
|
||||
std::cout << "SVD wide 3x5:\n";
|
||||
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
|
||||
@@ -230,7 +213,7 @@ TEST_CASE("SVD Integration: identity 3x3", "[Matrix][SVD][Integration]") {
|
||||
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-3f));
|
||||
|
||||
float err = frobeniusNorm(U - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-2f));
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
|
||||
}
|
||||
|
||||
TEST_CASE("SVD Integration: symmetric positive definite 2x2 [[5,3],[3,5]]",
|
||||
@@ -261,7 +244,7 @@ TEST_CASE("SVD Integration: symmetric positive definite 2x2 [[5,3],[3,5]]",
|
||||
err += diff * diff;
|
||||
}
|
||||
err = sqrtf(err);
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-3f));
|
||||
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
|
||||
|
||||
std::cout << "SVD SPD 2x2 [[5,3],[3,5]]:\n";
|
||||
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0)
|
||||
|
||||
Reference in New Issue
Block a user