diff --git a/packages/cultmath/src/CultMath/BoundedLeastSquares.cs b/packages/cultmath/src/CultMath/BoundedLeastSquares.cs new file mode 100644 index 00000000..e18a42e2 --- /dev/null +++ b/packages/cultmath/src/CultMath/BoundedLeastSquares.cs @@ -0,0 +1,233 @@ +using System.Runtime.InteropServices; + +namespace CultMath; + +/// Outcome of . +public enum BoundedLeastSquaresStatus +{ + /// The KKT conditions hold: every free variable has zero gradient and every bound has a gradient pointing into it. + Converged, + + /// The iteration cap was reached. x holds the last feasible iterate, which is never worse than the clamped start. + IterationLimit, +} + +/// +/// Dense bounded least squares: min ||A x - b||^2 subject to lo <= x <= hi, for small problems +/// (tens of rows and columns). C#-only, like the intercept helpers in ; there is no HLSL mirror. +/// +/// +/// Primal active set. Each iteration solves the free variables by Cholesky on the normal equations, takes the +/// step to the first bound it would cross, and, once a full step lands, releases the bound with the most +/// violated gradient sign. Free columns that are linearly dependent on earlier free columns (a zero column, a +/// duplicate) get a zero step instead of a failed pivot, so a singular free set never throws. The normal +/// equations square the condition number and run in float: callers that need a minimum-norm answer on +/// rank-deficient problems add a small ridge row to A and b. The solver allocates nothing and is +/// deterministic. +/// +public static class BoundedLeastSquares +{ + /// Default iteration cap for . + public const int DefaultMaxIterations = 100; + + private const float PivotRelativeTolerance = 1e-6f; + private const float KktRelativeTolerance = 1e-5f; + + /// Floats of caller-supplied workspace needed for columns. + public static int WorkspaceLength(int n) => 2 * n * n + 7 * n; + + /// + /// Minimises ||A x - b||^2 subject to lo <= x <= hi. is the warm start + /// on entry (clamped to the box first) and the result on exit. + /// + /// Rows of . + /// Columns of , and length of , and . + /// Row-major m * n matrix. + /// Length m target. + /// Lower bounds; each must be <= hi. + /// Upper bounds. + /// Warm start in, solution out. + /// At least floats; contents on entry are ignored. + /// Active-set iterations used. + /// Iteration cap. + public static BoundedLeastSquaresStatus Solve( + int m, int n, + ReadOnlySpan a, ReadOnlySpan b, + ReadOnlySpan lo, ReadOnlySpan hi, + Span x, Span workspace, + out int iterations, + int maxIterations = DefaultMaxIterations) + { + if (m < 0 || n < 0 || a.Length < m * n || b.Length < m || lo.Length < n || hi.Length < n + || x.Length < n || workspace.Length < WorkspaceLength(n)) + throw new ArgumentException("BoundedLeastSquares: span shorter than the stated dimensions."); + + var ata = workspace.Slice(0, n * n); + var chol = workspace.Slice(n * n, n * n); + var off = 2 * n * n; + var atb = workspace.Slice(off, n); + var g = workspace.Slice(off + n, n); + var p = workspace.Slice(off + 2 * n, n); + var y = workspace.Slice(off + 3 * n, n); + // One flag per column: 0 free, -1 at lo, +1 at hi. Free-list and dependency flags follow. + var ints = MemoryMarshal.Cast(workspace.Slice(off + 4 * n, 3 * n)); + var state = ints.Slice(0, n); + var freeIndex = ints.Slice(n, n); + var dependent = ints.Slice(2 * n, n); + + for (var j = 0; j < n; j++) + { + var l = lo[j]; + var h = hi[j]; + var v = x[j]; + v = !(v > l) ? l : (v < h ? v : h); + x[j] = v; + state[j] = v == l ? -1 : (v == h ? 1 : 0); + } + + var atbMax = 0f; + for (var j = 0; j < n; j++) + { + var sb = 0f; + for (var r = 0; r < m; r++) + sb += a[r * n + j] * b[r]; + atb[j] = sb; + atbMax = MathF.Max(atbMax, MathF.Abs(sb)); + for (var k = 0; k <= j; k++) + { + var s = 0f; + for (var r = 0; r < m; r++) + s += a[r * n + j] * a[r * n + k]; + ata[j * n + k] = s; + ata[k * n + j] = s; + } + } + var kktTol = KktRelativeTolerance * (1f + atbMax); + + iterations = 0; + while (true) + { + Gradient(ata, atb, x, g, n); + + var freeCount = 0; + for (var j = 0; j < n; j++) + if (state[j] == 0) + freeIndex[freeCount++] = j; + SolveFreeSet(ata, chol, g, p, y, freeIndex.Slice(0, freeCount), dependent, n); + + var t = 1f; + var block = -1; + for (var i = 0; i < freeCount; i++) + { + var j = freeIndex[i]; + var pj = p[j]; + if (pj == 0f) continue; + var tj = pj > 0f ? (hi[j] - x[j]) / pj : (lo[j] - x[j]) / pj; + if (tj < t) + { + t = tj; + block = j; + } + } + + if (block >= 0) + { + if (iterations >= maxIterations) return BoundedLeastSquaresStatus.IterationLimit; + iterations++; + for (var i = 0; i < freeCount; i++) + { + var j = freeIndex[i]; + x[j] += t * p[j]; + } + if (p[block] > 0f) { x[block] = hi[block]; state[block] = 1; } + else { x[block] = lo[block]; state[block] = -1; } + continue; + } + + for (var i = 0; i < freeCount; i++) + { + var j = freeIndex[i]; + var v = x[j] + p[j]; + x[j] = v < lo[j] ? lo[j] : (v > hi[j] ? hi[j] : v); + } + Gradient(ata, atb, x, g, n); + + var release = -1; + var worst = kktTol; + for (var j = 0; j < n; j++) + { + if (lo[j] == hi[j]) continue; + var violation = state[j] < 0 ? -g[j] : (state[j] > 0 ? g[j] : 0f); + if (violation > worst) + { + worst = violation; + release = j; + } + } + if (release < 0) return BoundedLeastSquaresStatus.Converged; + if (iterations >= maxIterations) return BoundedLeastSquaresStatus.IterationLimit; + iterations++; + state[release] = 0; + } + } + + private static void Gradient(ReadOnlySpan ata, ReadOnlySpan atb, ReadOnlySpan x, Span g, int n) + { + for (var j = 0; j < n; j++) + { + var s = -atb[j]; + for (var k = 0; k < n; k++) + s += ata[j * n + k] * x[k]; + g[j] = s; + } + } + + // Newton step over the free set: solves (A^T A)_FF p_F = -g_F by Cholesky. A free column whose pivot + // collapses (dependent on earlier free columns) is dropped from the factor and gets p = 0. + private static void SolveFreeSet( + ReadOnlySpan ata, Span l, ReadOnlySpan g, Span p, Span y, + ReadOnlySpan free, Span dependent, int n) + { + var k = free.Length; + for (var i = 0; i < k; i++) + { + var fi = free[i]; + var d = ata[fi * n + fi]; + for (var c = 0; c < i; c++) + d -= l[i * n + c] * l[i * n + c]; + if (d <= PivotRelativeTolerance * ata[fi * n + fi] || d <= 0f) + { + dependent[i] = 1; + for (var c = 0; c < i; c++) l[i * n + c] = 0f; + l[i * n + i] = 1f; + for (var r = i + 1; r < k; r++) l[r * n + i] = 0f; + continue; + } + dependent[i] = 0; + var lii = MathF.Sqrt(d); + l[i * n + i] = lii; + for (var r = i + 1; r < k; r++) + { + var s = ata[free[r] * n + fi]; + for (var c = 0; c < i; c++) + s -= l[r * n + c] * l[i * n + c]; + l[r * n + i] = s / lii; + } + } + + for (var i = 0; i < k; i++) + { + var s = dependent[i] != 0 ? 0f : -g[free[i]]; + for (var c = 0; c < i; c++) + s -= l[i * n + c] * y[c]; + y[i] = s / l[i * n + i]; + } + for (var i = k - 1; i >= 0; i--) + { + var s = y[i]; + for (var r = i + 1; r < k; r++) + s -= l[r * n + i] * p[free[r]]; + p[free[i]] = s / l[i * n + i]; + } + } +} diff --git a/packages/cultmath/src/CultMath/BoundedLeastSquares.cs.meta b/packages/cultmath/src/CultMath/BoundedLeastSquares.cs.meta new file mode 100644 index 00000000..fb5bd819 --- /dev/null +++ b/packages/cultmath/src/CultMath/BoundedLeastSquares.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4ba991e1b6c54e099bc7ea16541734c5 diff --git a/packages/cultmath/tests/CultMath.Tests/BoundedLeastSquaresTests.cs b/packages/cultmath/tests/CultMath.Tests/BoundedLeastSquaresTests.cs new file mode 100644 index 00000000..653b9158 --- /dev/null +++ b/packages/cultmath/tests/CultMath.Tests/BoundedLeastSquaresTests.cs @@ -0,0 +1,330 @@ +using System; +using CultMath; +using Xunit; + +namespace CultMath.Tests; + +// Fixtures are deliberately non-degenerate: A is non-square and non-identity, and b is outside A's range, +// so the unconstrained optimum has a real residual and the box actually bites. +public sealed class BoundedLeastSquaresTests +{ + private static double Cost(int m, int n, float[] a, float[] b, float[] x) + { + var c = 0.0; + for (var r = 0; r < m; r++) + { + var s = -(double)b[r]; + for (var j = 0; j < n; j++) s += (double)a[r * n + j] * x[j]; + c += s * s; + } + return c; + } + + // Gradient of 0.5*||Ax-b||^2, in double. + private static double[] Gradient(int m, int n, float[] a, float[] b, float[] x) + { + var g = new double[n]; + for (var r = 0; r < m; r++) + { + var s = -(double)b[r]; + for (var j = 0; j < n; j++) s += (double)a[r * n + j] * x[j]; + for (var j = 0; j < n; j++) g[j] += a[r * n + j] * s; + } + return g; + } + + private static double AtbScale(int m, int n, float[] a, float[] b) + { + var mx = 0.0; + for (var j = 0; j < n; j++) + { + var s = 0.0; + for (var r = 0; r < m; r++) s += (double)a[r * n + j] * b[r]; + mx = Math.Max(mx, Math.Abs(s)); + } + return 1 + mx; + } + + // Asserts feasibility and the KKT sign rule: free columns have ~zero gradient, columns at lo have + // gradient >= 0, columns at hi have gradient <= 0. + private static void AssertKkt(int m, int n, float[] a, float[] b, float[] lo, float[] hi, float[] x) + { + var g = Gradient(m, n, a, b, x); + var tol = 1e-3 * AtbScale(m, n, a, b); + for (var j = 0; j < n; j++) + { + Assert.True(x[j] >= lo[j] && x[j] <= hi[j], $"column {j} outside its box: {x[j]}"); + if (lo[j] == hi[j]) continue; + if (x[j] == lo[j]) Assert.True(g[j] >= -tol, $"column {j} at lo with gradient {g[j]}"); + else if (x[j] == hi[j]) Assert.True(g[j] <= tol, $"column {j} at hi with gradient {g[j]}"); + else Assert.True(Math.Abs(g[j]) <= tol, $"column {j} free with gradient {g[j]}"); + } + } + + // Projected gradient with step 1/L, L bounded by the Frobenius norm squared of A. + private static double ReferenceCost(int m, int n, float[] a, float[] b, float[] lo, float[] hi) + { + var frob = 0.0; + foreach (var v in a) frob += (double)v * v; + var step = 1.0 / Math.Max(frob, 1e-12); + var x = new float[n]; + for (var j = 0; j < n; j++) x[j] = Math.Clamp(0f, lo[j], hi[j]); + var xd = new double[n]; + for (var j = 0; j < n; j++) xd[j] = x[j]; + for (var it = 0; it < 200000; it++) + { + for (var j = 0; j < n; j++) x[j] = (float)xd[j]; + var g = Gradient(m, n, a, b, x); + for (var j = 0; j < n; j++) xd[j] = Math.Clamp(xd[j] - step * g[j], lo[j], hi[j]); + } + for (var j = 0; j < n; j++) x[j] = (float)xd[j]; + return Cost(m, n, a, b, x); + } + + private static BoundedLeastSquaresStatus Solve(int m, int n, float[] a, float[] b, float[] lo, float[] hi, float[] x, + out int iterations, int maxIterations = BoundedLeastSquares.DefaultMaxIterations) + => BoundedLeastSquares.Solve(m, n, a, b, lo, hi, x, new float[BoundedLeastSquares.WorkspaceLength(n)], out iterations, maxIterations); + + private static float[] Fill(float value, int n) + { + var v = new float[n]; + Array.Fill(v, value); + return v; + } + + private static readonly float[] A54 = + { + 1.0f, 0.3f, -0.2f, + 0.2f, 1.1f, 0.4f, + -0.5f, 0.6f, 0.9f, + 0.7f, -0.4f, 0.5f, + 0.1f, 0.8f, -0.6f, + }; + + private static readonly float[] B5 = { 0.9f, -0.4f, 1.3f, 0.2f, -0.7f }; + + [Fact] + public void InteriorOptimumEqualsNormalEquations() + { + const int m = 5, n = 3; + var x = new float[n]; + var status = Solve(m, n, A54, B5, Fill(-100f, n), Fill(100f, n), x, out _); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + + // Independent solve of (A^T A) x = A^T b by Gauss-Jordan in double. + var mat = new double[n, n + 1]; + for (var i = 0; i < n; i++) + { + for (var j = 0; j < n; j++) + for (var r = 0; r < m; r++) mat[i, j] += (double)A54[r * n + i] * A54[r * n + j]; + for (var r = 0; r < m; r++) mat[i, n] += (double)A54[r * n + i] * B5[r]; + } + for (var c = 0; c < n; c++) + { + var piv = mat[c, c]; + for (var j = 0; j <= n; j++) mat[c, j] /= piv; + for (var i = 0; i < n; i++) + { + if (i == c) continue; + var f = mat[i, c]; + for (var j = 0; j <= n; j++) mat[i, j] -= f * mat[c, j]; + } + } + for (var i = 0; i < n; i++) Assert.Equal(mat[i, n], x[i], 1e-4); + } + + [Fact] + public void KktHoldsOnRandomBoxedProblems() + { + var anyActive = false; + for (uint seed = 1; seed <= 150; seed++) + { + var rng = new CultMath.Random(seed * 7919u); + var n = rng.NextInt(2, 17); + var m = rng.NextInt(n, 25); + var a = new float[m * n]; + var b = new float[m]; + for (var i = 0; i < a.Length; i++) a[i] = rng.NextFloat(-1f, 1f); + for (var i = 0; i < m; i++) b[i] = rng.NextFloat(-3f, 3f); + var lo = new float[n]; + var hi = new float[n]; + for (var j = 0; j < n; j++) + { + switch (rng.NextInt(3)) + { + case 0: lo[j] = -1f; hi[j] = 1f; break; + case 1: lo[j] = 0f; hi[j] = 1f; break; + default: lo[j] = -0.25f; hi[j] = 0.1f; break; + } + } + var x = new float[n]; + var status = Solve(m, n, a, b, lo, hi, x, out _); + Assert.True(status == BoundedLeastSquaresStatus.Converged, $"seed {seed} ({m}x{n}) hit the cap"); + AssertKkt(m, n, a, b, lo, hi, x); + for (var j = 0; j < n; j++) anyActive |= x[j] == lo[j] || x[j] == hi[j]; + + var cs = Cost(m, n, a, b, x); + var cr = ReferenceCost(m, n, a, b, lo, hi); + Assert.True(Math.Abs(cs - cr) <= 1e-4 * cr + 1e-6, $"seed {seed} ({m}x{n}): solver cost {cs}, reference {cr}"); + } + Assert.True(anyActive); + } + + [Fact] + public void AllBoundsActive() + { + const int m = 4, n = 3; + var a = new[] { 1.0f, 0.2f, 0.1f, 0.1f, 1.0f, 0.3f, -0.2f, 0.2f, 1.0f, 0.5f, 0.5f, 0.4f }; + var b = new[] { 10f, -10f, 10f, 1f }; + var lo = Fill(-1f, n); + var hi = Fill(1f, n); + var x = new float[n]; + var status = Solve(m, n, a, b, lo, hi, x, out _); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + Assert.Equal(new[] { 1f, -1f, 1f }, x); + AssertKkt(m, n, a, b, lo, hi, x); + } + + [Fact] + public void ZeroColumn() + { + const int m = 5, n = 3; + var a = (float[])A54.Clone(); + for (var r = 0; r < m; r++) a[r * n + 1] = 0f; + var lo = Fill(-1f, n); + var hi = Fill(1f, n); + var x = new float[n]; + var status = Solve(m, n, a, B5, lo, hi, x, out _); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + AssertKkt(m, n, a, B5, lo, hi, x); + var cs = Cost(m, n, a, B5, x); + var cr = ReferenceCost(m, n, a, B5, lo, hi); + Assert.True(Math.Abs(cs - cr) <= 1e-4 * cr + 1e-6); + } + + [Fact] + public void DuplicateColumns() + { + const int m = 5, n = 3; + var a = (float[])A54.Clone(); + for (var r = 0; r < m; r++) a[r * n + 2] = a[r * n + 0]; + var lo = Fill(-1f, n); + var hi = new[] { 0.3f, 1f, 0.3f }; + var x = new float[n]; + var status = Solve(m, n, a, B5, lo, hi, x, out _); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + AssertKkt(m, n, a, B5, lo, hi, x); + var cs = Cost(m, n, a, B5, x); + var cr = ReferenceCost(m, n, a, B5, lo, hi); + Assert.True(Math.Abs(cs - cr) <= 1e-4 * cr + 1e-6); + } + + [Fact] + public void WarmStartMatchesColdStart() + { + const int m = 5, n = 3; + var lo = new[] { -0.2f, -1f, 0f }; + var hi = new[] { 0.5f, 0.3f, 1f }; + var cold = new float[n]; + Solve(m, n, A54, B5, lo, hi, cold, out _); + AssertKkt(m, n, A54, B5, lo, hi, cold); + + var starts = new[] { new[] { 0.5f, -1f, 1f }, new[] { 9f, 9f, -9f }, new[] { 0.1f, 0.1f, 0.5f }, (float[])cold.Clone() }; + foreach (var start in starts) + { + var status = Solve(m, n, A54, B5, lo, hi, start, out var iterations); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + for (var j = 0; j < n; j++) Assert.Equal(cold[j], start[j], 1e-4); + if (ReferenceEquals(start, starts[^1])) Assert.Equal(0, iterations); + } + } + + [Fact] + public void AsymmetricBounds() + { + const int m = 5, n = 3; + var lo = new[] { -1f, 0f, -1f }; + var hi = new[] { 1f, 1f, 1f }; + // Column 1 wants to go negative (b has a strong component against it) but its floor is 0. + var b = new[] { 0.2f, -2.5f, 0.4f, -1.5f, -2.0f }; + var x = new float[n]; + var status = Solve(m, n, A54, b, lo, hi, x, out _); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + AssertKkt(m, n, A54, b, lo, hi, x); + Assert.Equal(0f, x[1]); + Assert.True(x[0] < 0f || x[2] < 0f, "a symmetric-bounds column should use its negative side"); + var cs = Cost(m, n, A54, b, x); + var cr = ReferenceCost(m, n, A54, b, lo, hi); + Assert.True(Math.Abs(cs - cr) <= 1e-4 * cr + 1e-6); + } + + [Fact] + public void IterationCapReturnsFeasibleIterateAndLimitStatus() + { + const int m = 4, n = 3; + var a = new[] { 1.0f, 0.2f, 0.1f, 0.1f, 1.0f, 0.3f, -0.2f, 0.2f, 1.0f, 0.5f, 0.5f, 0.4f }; + var b = new[] { 10f, -10f, 10f, 1f }; + var lo = Fill(-1f, n); + var hi = Fill(1f, n); + var x = new float[n]; + var start = Cost(m, n, a, b, x); + var status = Solve(m, n, a, b, lo, hi, x, out var iterations, maxIterations: 1); + Assert.Equal(BoundedLeastSquaresStatus.IterationLimit, status); + Assert.Equal(1, iterations); + for (var j = 0; j < n; j++) Assert.InRange(x[j], lo[j], hi[j]); + Assert.True(Cost(m, n, a, b, x) < start); + + var zero = new float[n]; + Assert.Equal(BoundedLeastSquaresStatus.IterationLimit, Solve(m, n, a, b, lo, hi, zero, out iterations, maxIterations: 0)); + Assert.Equal(0, iterations); + Assert.Equal(new float[n], zero); + } + + [Fact] + public void StartOutsideTheBoxIsClampedFirst() + { + const int m = 5, n = 3; + var lo = Fill(-0.1f, n); + var hi = Fill(0.1f, n); + var x = new[] { 50f, -50f, float.NaN }; + Solve(m, n, A54, B5, lo, hi, x, out _, maxIterations: 0); + Assert.Equal(new[] { 0.1f, -0.1f, -0.1f }, x); + } + + [Fact] + public void FixedColumnStaysPut() + { + const int m = 5, n = 3; + var lo = new[] { -1f, 0.25f, -1f }; + var hi = new[] { 1f, 0.25f, 1f }; + var x = new float[n]; + var status = Solve(m, n, A54, B5, lo, hi, x, out _); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + Assert.Equal(0.25f, x[1]); + AssertKkt(m, n, A54, B5, lo, hi, x); + } + + [Fact] + public void WarmCallAllocatesNothing() + { + const int m = 5, n = 3; + var lo = new[] { -0.2f, -1f, 0f }; + var hi = new[] { 0.5f, 0.3f, 1f }; + var x = new float[n]; + var workspace = new float[BoundedLeastSquares.WorkspaceLength(n)]; + BoundedLeastSquares.Solve(m, n, A54, B5, lo, hi, x, workspace, out _); + + var before = GC.GetAllocatedBytesForCurrentThread(); + var status = BoundedLeastSquares.Solve(m, n, A54, B5, lo, hi, x, workspace, out _); + var after = GC.GetAllocatedBytesForCurrentThread(); + Assert.Equal(BoundedLeastSquaresStatus.Converged, status); + Assert.Equal(0L, after - before); + } + + [Fact] + public void ShortSpansThrowArgumentException() + { + var x = new float[3]; + Assert.Throws(() => BoundedLeastSquares.Solve(5, 3, A54, B5, Fill(-1f, 3), Fill(1f, 3), x, new float[4], out _)); + } +}